Skip to content

Commit 1b31859

Browse files
grandixximograndixximo
committed
Bidirectional reachability cap: eliminate Working profiles from fixup and backtrack passes
1 parent a18952e commit 1b31859

1 file changed

Lines changed: 43 additions & 44 deletions

File tree

‎src/emc/motion_planning/motion_planning_9d.cc‎

Lines changed: 43 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -685,6 +685,25 @@ static double jerkLimitedMaxEntryVelocity(double v_f, double d,
685685
return v_lo;
686686
}
687687

688+
/**
689+
* @brief Bidirectional reachability cap for entry/exit velocities.
690+
*
691+
* Ensures both velocities are physically achievable from each other within
692+
* the given segment distance, respecting jerk and acceleration limits.
693+
* By time-reversal symmetry, jerkLimitedMaxEntryVelocity(v, d, a, j) gives
694+
* the max velocity at the other end — works for both accel and decel.
695+
* Without this cap, Ruckig returns Working (overshooting profiles).
696+
*/
697+
static void applyBidirectionalReachability(double &v_entry, double &v_exit,
698+
double distance, double max_acc,
699+
double max_jrk)
700+
{
701+
double v_fwd = jerkLimitedMaxEntryVelocity(v_entry, distance, max_acc, max_jrk);
702+
double v_bwd = jerkLimitedMaxEntryVelocity(v_exit, distance, max_acc, max_jrk);
703+
v_exit = fmin(v_exit, v_fwd);
704+
v_entry = fmin(v_entry, v_bwd);
705+
}
706+
688707
/**
689708
* @brief Check if exit velocity at this queue index is safe for the next segment.
690709
*
@@ -2110,18 +2129,8 @@ static int recomputeDownstreamProfiles(TP_STRUCT *tp,
21102129
scaled_v_exit = applyKinkVelCap(scaled_v_exit, v_exit_unscaled, max_vel, tc->kink_vel);
21112130
double desired_fvel_for_profile = scaled_v_exit;
21122131

2113-
// Reachability cap: ensure exit velocity is achievable from entry
2114-
// within segment distance (and vice versa). Without this, Ruckig
2115-
// returns Working (overshoot profiles) that cross the target position
2116-
// at an intermediate velocity much lower than the intended exit.
2117-
{
2118-
double v_fwd = jerkLimitedMaxEntryVelocity(
2119-
scaled_v_entry, tc->target, max_acc, max_jrk);
2120-
double v_bwd = jerkLimitedMaxEntryVelocity(
2121-
scaled_v_exit, tc->target, max_acc, max_jrk);
2122-
scaled_v_exit = fmin(scaled_v_exit, v_fwd);
2123-
scaled_v_entry = fmin(scaled_v_entry, v_bwd);
2124-
}
2132+
applyBidirectionalReachability(scaled_v_entry, scaled_v_exit,
2133+
tc->target, max_acc, max_jrk);
21252134

21262135
// Skip if profile already matches: same feed, entry velocity, and exit target.
21272136
// Avoids redundant Ruckig solves when forward pass already wrote correct profiles.
@@ -2157,9 +2166,12 @@ static int recomputeDownstreamProfiles(TP_STRUCT *tp,
21572166
double p_max_vel = applyVLimit(tp, prev_seg, p_vel_limit * feed_scale);
21582167
double p_max_acc = tcGetTangentialMaxAccel_9D_user(prev_seg);
21592168
double p_max_jrk = prev_seg->maxjerk > 0 ? prev_seg->maxjerk : default_jerk;
2160-
RuckigProfileParams rp_prev = {p_entry, curr_v0,
2169+
double p_exit = curr_v0;
2170+
applyBidirectionalReachability(p_entry, p_exit,
2171+
prev_seg->target, p_max_acc, p_max_jrk);
2172+
RuckigProfileParams rp_prev = {p_entry, p_exit,
21612173
p_max_vel, p_max_acc, p_max_jrk, prev_seg->target,
2162-
feed_scale, p_vel_limit, tp->vLimit, curr_v0};
2174+
feed_scale, p_vel_limit, tp->vLimit, p_exit};
21632175
if (computeAndStoreProfile(prev_seg, rp_prev)) {
21642176
atomicStoreInt((int*)&prev_seg->shared_9d.optimization_state,
21652177
TC_PLAN_FINALIZED);
@@ -4002,15 +4014,8 @@ extern "C" void checkFeedOverride(TP_STRUCT *tp)
40024014
double scaled_v_exit = applyKinkVelCap(scaled_v_exit_pre, v_exit, max_vel, tc->kink_vel);
40034015
double desired_fvel_for_profile = scaled_v_exit;
40044016

4005-
// Reachability cap
4006-
{
4007-
double v_fwd = jerkLimitedMaxEntryVelocity(
4008-
scaled_v_entry, tc->target, max_acc, max_jrk);
4009-
double v_bwd = jerkLimitedMaxEntryVelocity(
4010-
scaled_v_exit, tc->target, max_acc, max_jrk);
4011-
scaled_v_exit = fmin(scaled_v_exit, v_fwd);
4012-
scaled_v_entry = fmin(scaled_v_entry, v_bwd);
4013-
}
4017+
applyBidirectionalReachability(scaled_v_entry, scaled_v_exit,
4018+
tc->target, max_acc, max_jrk);
40144019

40154020
try {
40164021
RuckigProfileParams rp = {scaled_v_entry, scaled_v_exit,
@@ -4044,9 +4049,12 @@ extern "C" void checkFeedOverride(TP_STRUCT *tp)
40444049
double p_max_vel = applyVLimit(tp, prev_seg, p_vel_limit * feed_scale);
40454050
double p_max_acc = tcGetTangentialMaxAccel_9D_user(prev_seg);
40464051
double p_max_jrk = prev_seg->maxjerk > 0 ? prev_seg->maxjerk : default_jerk;
4047-
RuckigProfileParams rp_prev = {p_entry, curr_v0,
4052+
double p_exit = curr_v0;
4053+
applyBidirectionalReachability(p_entry, p_exit,
4054+
prev_seg->target, p_max_acc, p_max_jrk);
4055+
RuckigProfileParams rp_prev = {p_entry, p_exit,
40484056
p_max_vel, p_max_acc, p_max_jrk, prev_seg->target,
4049-
feed_scale, p_vel_limit, tp->vLimit, curr_v0};
4057+
feed_scale, p_vel_limit, tp->vLimit, p_exit};
40504058
if (computeAndStoreProfile(prev_seg, rp_prev)) {
40514059
atomicStoreInt((int*)&prev_seg->shared_9d.optimization_state,
40524060
TC_PLAN_FINALIZED);
@@ -4415,19 +4423,8 @@ int computeRuckigProfiles_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, int optimiza
44154423
// convergence detection — see RECOMP_FVEL check above.
44164424
double desired_fvel_for_profile = scaled_v_exit;
44174425

4418-
// Reachability cap: ensure exit velocity is physically achievable
4419-
// from entry velocity within segment distance. By time-reversal
4420-
// symmetry, jerkLimitedMaxEntryVelocity(v, d, a, j) gives the
4421-
// max velocity at the other end — works for both accel and decel.
4422-
// Without this cap, Ruckig returns Working (overshooting profiles).
4423-
{
4424-
double v_fwd = jerkLimitedMaxEntryVelocity(
4425-
scaled_v_entry, tc->target, max_acc, max_jrk);
4426-
double v_bwd = jerkLimitedMaxEntryVelocity(
4427-
scaled_v_exit, tc->target, max_acc, max_jrk);
4428-
scaled_v_exit = fmin(scaled_v_exit, v_fwd);
4429-
scaled_v_entry = fmin(scaled_v_entry, v_bwd);
4430-
}
4426+
applyBidirectionalReachability(scaled_v_entry, scaled_v_exit,
4427+
tc->target, max_acc, max_jrk);
44314428

44324429
try {
44334430
RuckigProfileParams rp = {scaled_v_entry, scaled_v_exit,
@@ -4460,9 +4457,12 @@ int computeRuckigProfiles_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, int optimiza
44604457
double p_max_vel = applyVLimit(tp, prev_seg, p_vel_limit * p_feed);
44614458
double p_max_acc = tcGetTangentialMaxAccel_9D_user(prev_seg);
44624459
double p_max_jrk = prev_seg->maxjerk > 0 ? prev_seg->maxjerk : default_jerk;
4463-
RuckigProfileParams rp_prev = {p_entry, curr_v0,
4460+
double p_exit = curr_v0;
4461+
applyBidirectionalReachability(p_entry, p_exit,
4462+
prev_seg->target, p_max_acc, p_max_jrk);
4463+
RuckigProfileParams rp_prev = {p_entry, p_exit,
44644464
p_max_vel, p_max_acc, p_max_jrk, prev_seg->target,
4465-
p_feed, p_vel_limit, tp->vLimit, curr_v0};
4465+
p_feed, p_vel_limit, tp->vLimit, p_exit};
44664466
if (computeAndStoreProfile(prev_seg, rp_prev)) {
44674467
atomicStoreInt((int*)&prev_seg->shared_9d.optimization_state,
44684468
TC_PLAN_FINALIZED);
@@ -4557,10 +4557,9 @@ int computeRuckigProfiles_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, int optimiza
45574557
// Capped exit velocity for A
45584558
double capped_exit = B_max_entry;
45594559

4560-
// Reachability: can A reach capped_exit from its entry within its length?
4561-
double A_v_fwd = jerkLimitedMaxEntryVelocity(
4562-
A_v_entry_scaled, tc_A->target, A_acc, A_jrk);
4563-
capped_exit = fmin(capped_exit, A_v_fwd);
4560+
// Bidirectional reachability: cap both entry and exit
4561+
applyBidirectionalReachability(A_v_entry_scaled, capped_exit,
4562+
tc_A->target, A_acc, A_jrk);
45644563

45654564
try {
45664565
RuckigProfileParams rp = {A_v_entry_scaled, capped_exit,

0 commit comments

Comments
 (0)