@@ -37,19 +37,20 @@ void JerkLimitedTrajectory1D::generate(const double initial_pos, const double fi
3737 return ;
3838 }
3939
40- // Note: if we are already at the target but still moving, we intentionally do not stop
41- // here. Doing so would leave the robot wherever its momentum carries it rather than at
42- // the destination. Instead we fall through to the logic below, which will overshoot the
43- // target and then plan a trajectory back to it.
40+ // Note: if we are already at the target but still moving, we intentionally do not
41+ // stop here. Doing so would leave the robot wherever its momentum carries it rather
42+ // than at the destination. Instead we fall through to the logic below, which will
43+ // overshoot the target and then plan a trajectory back to it.
4444
4545 // Ramp the initial acceleration to zero via a preliminary jerk phase, then plan the
4646 // rest of the trajectory from that zero-acceleration state. The 2D layer feeds the
4747 // robot's actual (unscaled) initial acceleration into each axis while scaling the
48- // per-axis acceleration/jerk limits, so the initial acceleration can exceed or conflict
49- // with those limits. The planning primitives below (planAccelProfile, planDecelToStop,
50- // planDecelToSpeed) are only well-defined for a zero initial acceleration; feeding them
51- // an out-of-range initial acceleration produces inconsistent profiles (e.g. negative
52- // phase durations) that fail to reach the destination.
48+ // per-axis acceleration/jerk limits, so the initial acceleration can exceed or
49+ // conflict with those limits. The planning primitives below (planAccelProfile,
50+ // planDecelToStop, planDecelToSpeed) are only well-defined for a zero initial
51+ // acceleration; feeding them an out-of-range initial acceleration produces
52+ // inconsistent profiles (e.g. negative phase durations) that fail to reach the
53+ // destination.
5354 double t = 0.0 , p = 0.0 , v = initial_vel, a = initial_accel;
5455 if (std::abs (a) > EPSILON )
5556 {
@@ -492,12 +493,12 @@ void JerkLimitedTrajectory1D::generateDirect(
492493
493494 // Plan the phase that brings the robot from its initial velocity to signed_peak. We
494495 // accelerate if the peak is above the initial speed, but decelerate if the initial
495- // speed already exceeds the peak (which happens when a scaled 2D axis velocity limit is
496- // smaller than the robot's initial speed component along that axis). Previously this
497- // always used planAccelProfile, which returns an empty plan when the initial speed is
498- // already above the peak. The robot would then never slow down to the cruise speed, so
499- // the cruise/deceleration distances were computed for the peak speed while the robot
500- // was actually travelling faster, causing it to overshoot the destination.
496+ // speed already exceeds the peak (which happens when a scaled 2D axis velocity limit
497+ // is smaller than the robot's initial speed component along that axis). Previously
498+ // this always used planAccelProfile, which returns an empty plan when the initial
499+ // speed is already above the peak. The robot would then never slow down to the cruise
500+ // speed, so the cruise/deceleration distances were computed for the peak speed while
501+ // the robot was actually travelling faster, causing it to overshoot the destination.
501502 auto planToPeak = [&](const double signed_peak)
502503 {
503504 if (std::abs (signed_peak) >= std::abs (initial_vel))
0 commit comments