Skip to content

tp: apply parabolic half-accel only during an actual blend #4221

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
grandixximo wants to merge 1 commit into
base: master
Choose a base branch
from
+72 −28
Open

tp: apply parabolic half-accel only during an actual blend #4221

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 33 additions & 3 deletions src/emc/tp/tc.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,21 +64,29 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc,
return fmin(v_max_target, tc->maxvel);
}

double tcGetOverallMaxAccel(const TC_STRUCT *tc)
static double tcOverallMaxAccelScaled(const TC_STRUCT *tc, int apply_parabolic)
{
// Handle any acceleration reduction due to an approximate-tangent "blend" with the previous or next segment
double a_scale = (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev));

// Parabolic blending conditions: If the next segment or previous segment
// has a parabolic blend with this one, acceleration is scaled down by 1/2
// so that the sum of the two does not exceed the maximum.
if (tc->blend_prev || TC_TERM_COND_PARABOLIC == tc->term_cond) {
// so that the sum of the two does not exceed the maximum. This is only
// physically required while both segments accelerate simultaneously in the
// blend overlap; callers that need the geometric corner limit pass
// apply_parabolic != 0, the per-cycle motion cap gates it on the overlap.
if (apply_parabolic && (tc->blend_prev || TC_TERM_COND_PARABOLIC == tc->term_cond)) {
a_scale *= 0.5;
}

return tc->maxaccel * a_scale;
}

double tcGetOverallMaxAccel(const TC_STRUCT *tc)
{
return tcOverallMaxAccelScaled(tc, 1);
}

/**
* Get acceleration for a tc based on the trajectory planner state.
*/
Expand All @@ -96,6 +104,28 @@ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc)
return a_scale;
}

/**
* Per-cycle tangential acceleration limit.
*
* Same as tcGetTangentialMaxAccel, except the parabolic-blend 1/2 reduction is
* only applied when the segment is actually overlapping a neighbor in an active
* parabolic blend this cycle (in_overlap != 0). Away from that overlap (a lone
* segment, the accel-from-rest of the first segment, or the decel-to-stop of the
* last segment) the full path acceleration is used. The path acceleration is
* derived to respect every joint's limit for the segment direction, so it is safe
* whenever only one segment is moving; the 1/2 split is only needed while two
* blended segments accelerate at once and their per-axis contributions add.
*/
double tcGetCycleMaxAccel(TC_STRUCT const * const tc, int in_overlap)
{
double a_scale = tcOverallMaxAccelScaled(tc, in_overlap);

if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) {
a_scale *= tc->acc_ratio_tan;
}
return a_scale;
}


int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction)
{
Expand Down
1 change: 1 addition & 0 deletions src/emc/tp/tc.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc,

double tcGetOverallMaxAccel(TC_STRUCT const * tc);
double tcGetTangentialMaxAccel(TC_STRUCT const * const tc);
double tcGetCycleMaxAccel(TC_STRUCT const * const tc, int in_overlap);

int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction);
int tcInitKinkProperties(TC_STRUCT *tc);
Expand Down
57 changes: 34 additions & 23 deletions src/emc/tp/tp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,10 +130,10 @@ STATIC double estimateParabolicBlendPerformance(
TC_STRUCT const *tc,
TC_STRUCT const *nexttc);

STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc);
STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int in_overlap);

STATIC int tpUpdateCycle(TP_STRUCT * const tp,
TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode);
TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode, int in_overlap);

STATIC int tpRunOptimization(TP_STRUCT * const tp);

Expand Down Expand Up @@ -2640,7 +2640,7 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const
* non-zero velocity at the instant the target is reached.
*/
void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc,
double * const acc, double * const vel_desired)
double * const acc, double * const vel_desired, int in_overlap)
{
tc_debug_print("using trapezoidal acceleration\n");

Expand All @@ -2658,7 +2658,7 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t

/* Calculations for desired velocity based on trapezoidal profile */
double dx = tcGetDistanceToGo(tc, tp->reverse_run);
double maxaccel = tcGetTangentialMaxAccel(tc);
double maxaccel = tcGetCycleMaxAccel(tc, in_overlap);

double discr_term1 = pmSq(tc_finalvel);
double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time);
Expand Down Expand Up @@ -2703,7 +2703,8 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp,
TC_STRUCT * const tc,
TC_STRUCT const * const nexttc,
double * const acc,
double * const vel_desired)
double * const vel_desired,
int in_overlap)
{
tc_debug_print("using ramped acceleration\n");
// displacement remaining in this segment
Expand Down Expand Up @@ -2736,7 +2737,7 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp,
double acc_final = dv / dt;

// Saturate estimated acceleration against maximum allowed by segment
double acc_max = tcGetTangentialMaxAccel(tc);
double acc_max = tcGetCycleMaxAccel(tc, in_overlap);

// Output acceleration and velocity for position update
*acc = saturate(acc_final, acc_max);
Expand Down Expand Up @@ -2802,7 +2803,8 @@ STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double j
* non-zero velocity at the instant the target is reached.
*/
int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc,
double * const acc, double * const jerk, double * const vel_desired, double * const pos_error, int blend, double * const req_pos)
double * const acc, double * const jerk, double * const vel_desired, double * const pos_error, int blend, double * const req_pos,
int in_overlap)
{
tc_debug_print("using s-curve acceleration with Ruckig\n");

Expand All @@ -2819,7 +2821,7 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_
double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc);

double dx = tcGetDistanceToGo(tc, tp->reverse_run);
double maxaccel = tcGetTangentialMaxAccel(tc);
double maxaccel = tcGetCycleMaxAccel(tc, in_overlap);

*pos_error = 0;
if(!blend && tc->cycle_time < TP_TIME_EPSILON){
Expand Down Expand Up @@ -3277,7 +3279,9 @@ STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc,
if(is_abort) mode = 0;
else mode = 1;

tpUpdateCycle(tp, nexttc, NULL, &mode);
// Secondary segment of an active parabolic blend: it overlaps the primary
// this cycle, so the 1/2 accel split applies.
tpUpdateCycle(tp, nexttc, NULL, &mode, 1);
//Restore the original target velocity
nexttc->target_vel = save_vel;
}
Expand Down Expand Up @@ -3692,7 +3696,7 @@ STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc,
* Handles the majority of updates on a single segment for the current cycle.
*/
STATIC int tpUpdateCycle(TP_STRUCT * const tp,
TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode) {
TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode, int in_overlap) {

//placeholders for position for this update
EmcPose before;
Expand All @@ -3717,13 +3721,13 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
// Also, don't ramp up for parabolic blends
if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) {
if(planner_type == 0)
res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired);
res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap);
}

// Check the return in case the ramp calculation failed, fall back to trapezoidal
if (res_accel != TP_ERR_OK) {
if(planner_type == 0)
tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired);
tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap);
}

tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run);
Expand All @@ -3735,17 +3739,17 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
double req_pos = -1.0; // -1.0 means not provided
tc->cycle_time = tp->cycleTime;

int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1, &req_pos);
int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1, &req_pos, in_overlap);
if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration.
*mode = TP_SCURVE_ACCEL_ERROR;
res_accel = 1;
acc=0, vel_desired=0;
if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) {
res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired);
res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap);
}
// Check the return in case the ramp calculation failed, fall back to trapezoidal
if (res_accel != TP_ERR_OK) {
tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired);
tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap);
}
tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run);
}else{
Expand All @@ -3755,17 +3759,17 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
double jerk;
double perror;
double req_pos = -1.0; // -1.0 means not provided
int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0, &req_pos);
int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0, &req_pos, in_overlap);
if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration.
*mode = TP_SCURVE_ACCEL_ERROR;
res_accel = 1;
acc=0, vel_desired=0;
if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) {
res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired);
res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap);
}
// Check the return in case the ramp calculation failed, fall back to trapezoidal
if (res_accel != TP_ERR_OK) {
tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired);
tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap);
}
tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run);
}else{
Expand All @@ -3777,7 +3781,7 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
}

//Check if we're near the end of the cycle and set appropriate changes
tpCheckEndCondition(tp, tc, nexttc);
tpCheckEndCondition(tp, tc, nexttc, in_overlap);

EmcPose displacement;

Expand Down Expand Up @@ -3843,7 +3847,7 @@ STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time,
* then we flag the segment as "splitting", so that during the next cycle,
* it handles the transition to the next segment.
*/
STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) {
STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int in_overlap) {

//Assume no split time unless we find otherwise
tc->cycle_time = tp->cycleTime;
Expand Down Expand Up @@ -3901,7 +3905,7 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc,

//If this is a valid acceleration, then we're done. If not, then we solve
//for v_f and dt given the max acceleration allowed.
double a_max = tcGetTangentialMaxAccel(tc);
double a_max = tcGetCycleMaxAccel(tc, in_overlap);

//If we exceed the maximum acceleration, then the dt estimate is too small.
double a = a_f;
Expand Down Expand Up @@ -4024,7 +4028,9 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc,
TC_STRUCT *next2tc = tcqItem(&tp->queue, queue_dir_step*2);

int mode = 0;
tpUpdateCycle(tp, nexttc, next2tc, &mode);
// Tangent hand-off after a split cycle: no simultaneous parabolic blend, so
// the next segment runs at full acceleration.
tpUpdateCycle(tp, nexttc, next2tc, &mode, 0);

// Update status for the split portion
// FIXME redundant tangent check, refactor to switch
Expand Down Expand Up @@ -4053,7 +4059,12 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp,
tc->cycle_time = tp->cycleTime;

int mode = 0;
tpUpdateCycle(tp, tc, nexttc, &mode);
// The 1/2 parabolic-blend accel reduction is only needed while this segment
// actually overlaps a neighbor in an active blend. blending_next latches once
// the blend into nexttc has begun; away from that (lone segment, accel from
// rest, decel to a final stop) the segment gets its full path acceleration.
int in_overlap = (nexttc != NULL) && tc->blending_next;
tpUpdateCycle(tp, tc, nexttc, &mode, in_overlap);

/* Parabolic blending */

Expand Down
6 changes: 4 additions & 2 deletions tests/motion/g0/checkresult
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,10 @@ max_acceleration_ips2 = float(Popen(
).communicate()[0])

# max acceleration in inches per second per servo-thread cycle
# the factor of half at the end is because emc2 reserves half the acceleration for blending
max_acceleration_ipspc = (max_acceleration_ips2 / cycles_per_second) * 0.5
# A move that never blends (like this lone G0) runs at the full machine
# acceleration; the 1/2 parabolic-blend reservation only applies during an
# actual blend overlap.
max_acceleration_ipspc = (max_acceleration_ips2 / cycles_per_second)

# max velocity in inches per second
max_velocity_ips = float(Popen(
Expand Down
Loading