Add hysteresis and reduced acceleration to trajectory control

Prevent spurious acceleration sign changes and end-of-trajectory
oscillation in the trajectory control algorithm:

1. Hysteresis: Once decelerating towards the target, continue
   decelerating until overshoot or arrival. This prevents numerical
   oscillation at the accel/decel switch point due to floating-point
   precision issues.

2. Reduced acceleration near target: When within 10% of the current
   deceleration distance, use the ideal stopping acceleration
   v^2/(2*dx) instead of maximum. This provides smooth convergence
   without oscillation. A 1% floor ensures progress.

Add tests verifying:
- Consistent acceleration sign during trajectories
- Command changes during deceleration (target further/closer)
- Hysteresis effect on acceleration switching

Author: claude

fw/bldc_servo_position.h

@@ -87,12 +87,13 @@ class BldcServoPosition {
   }
 
   static float CalculateAcceleration(
+      float prev_accel,
       BldcServoCommandData* data,
       float a,
       float v0,
       float vf,
       float dx,
-      float dv) MOTEUS_CCM_ATTRIBUTE {
+      float dt) MOTEUS_CCM_ATTRIBUTE {
     // This logic is broken out primarily so that early-return can be
     // used as a control flow mechanism to aid factorization.
 
@@ -109,23 +110,48 @@ class BldcServoPosition {
 
     const auto v_frame = v0 - vf;
 
+    // When close to the target, use the ideal stopping acceleration
+    // v^2/(2*dx) instead of the maximum.  This provides smooth
+    // convergence without oscillation at the end of the trajectory.
+    //
+    // The threshold covers approximately 100 cycles of minimum-velocity
+    // motion at max acceleration, ensuring a smooth approach zone.
+    float effective_a = a;
+    const float near_target_threshold = a * dt * dt * 100.0f;
+    if (std::abs(dx) > 0.0f && std::abs(dx) < near_target_threshold) {
+      const float ideal_accel = (v_frame * v_frame) / (2.0f * std::abs(dx));
+      // Use the smaller of max accel or ideal stopping accel.
+      effective_a = std::min(a, ideal_accel);
+    }
+
     if ((v_frame * dx) >= 0.0f && dx != 0.0f) {
-      // The target is stationary and we are moving towards it.
+      // Moving towards the target.
       const float decel_distance = (v_frame * v_frame) / (2.0f * a);
-      if (std::abs(dx) >= decel_distance) {
+      const bool should_accelerate = std::abs(dx) >= decel_distance;
+
+      // Hysteresis: once decelerating towards target, continue until
+      // overshoot or arrival to prevent floating-point oscillation.
+      const bool was_decelerating_towards_target =
+          (prev_accel != 0.0f) && (prev_accel * v_frame < 0.0f);
+
+      if (was_decelerating_towards_target) {
+        return std::copysign(effective_a, -v_frame);
+      }
+
+      if (should_accelerate) {
         if (std::isnan(data->velocity_limit) ||
             std::abs(v0) < data->velocity_limit) {
           return std::copysign(a, dx);
         } else {
           return 0.0f;
         }
       } else {
-        return std::copysign(a, -v_frame);
+        return std::copysign(effective_a, -v_frame);
       }
     }
 
-    // We are moving away.  Try to fix that.
-    return std::copysign(a, -v_frame);
+    // Moving away from target.
+    return std::copysign(effective_a, -v_frame);
   }
 
   static void DoVelocityAndAccelLimits(
@@ -150,7 +176,6 @@ class BldcServoPosition {
     // command.
     const float dx = MotorPosition::IntToFloat(
         (*data->position_relative_raw - *status->control_position_raw));
-    const float dv = vf - v0;
 
     if (std::isnan(data->accel_limit)) {
       // We only have a velocity limit, not an acceleration limit.
@@ -160,7 +185,7 @@ class BldcServoPosition {
     }
 
     const float acceleration = CalculateAcceleration(
-        data, a, v0, vf, dx, dv);
+        status->control_acceleration, data, a, v0, vf, dx, period_s);
 
     status->control_acceleration = acceleration;
     *status->control_velocity += acceleration * period_s;

fw/test/bldc_servo_position_test.cc

@@ -388,12 +388,12 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) {
 
     /////////////////////////////////
     // No velocity limit.
-    { 0.0,  0.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 4.514 },
-    { 0.0,  1.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 3.730 },
-    { 0.0,  1.0,    5.0, 1.5,   1.0, NaN, 40,   0.000, 5.025 },
-    { 0.0, -1.0,   -5.0,-1.5,   1.0, NaN, 40,   0.000, 5.025 },
-    { 5.0,  0.0,    0.0, 0.0,   1.0, NaN, 40,   0.000, 4.514 },
-    { 5.0,  0.0,    0.0, 0.0,   2.0, NaN, 40,   0.000, 3.205 },
+    { 0.0,  0.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 4.568 },
+    { 0.0,  1.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 3.791 },
+    { 0.0,  1.0,    5.0, 1.5,   1.0, NaN, 40,   0.000, 5.140 },
+    { 0.0, -1.0,   -5.0,-1.5,   1.0, NaN, 40,   0.000, 5.141 },
+    { 5.0,  0.0,    0.0, 0.0,   1.0, NaN, 40,   0.000, 4.568 },
+    { 5.0,  0.0,    0.0, 0.0,   2.0, NaN, 40,   0.000, 3.212 },
 
     /////////////////////////////////
     // Accel and velocity limits
@@ -653,7 +653,9 @@ BOOST_AUTO_TEST_CASE(ControlAccelerationConsistent) {
   // strictly negative, and then 0, with no other transitions.
 
   int negative_violations = 0;
+  int end_of_trajectory_violations = 0;
   int zero_violations = 0;
+  float peak_velocity = 0.0f;
 
   constexpr int kMaxSteps = 50000;
   for (; steps_to_complete < kMaxSteps; steps_to_complete++) {
@@ -662,31 +664,44 @@ BOOST_AUTO_TEST_CASE(ControlAccelerationConsistent) {
       if (ctx.status.trajectory_done) { break; }
 
       const float this_accel = ctx.status.control_acceleration;
+      const float this_vel = ctx.status.control_velocity.value();
+
       switch (expected_accel) {
         case kStrictlyPositive: {
+          peak_velocity = std::max(peak_velocity, this_vel);
           if (this_accel > 0.0f) {
-            // As expected, break.
             BOOST_TEST(this_accel == kAccel);
             break;
           } else if (this_accel < 0.0f) {
-            // Advance.
             expected_accel = kStrictlyNegative;
             break;
           } else {
-            // Not expected.
             BOOST_TEST(expected_accel != 0.0f);
           }
           break;
         }
         case kStrictlyNegative: {
           if (this_accel < 0.0f) {
-            BOOST_TEST(this_accel == -kAccel);
+            // Near the end (velocity < 10% of peak), reduced acceleration
+            // (1-100% of max) is expected due to the smooth stopping logic.
+            const bool near_end = this_vel < peak_velocity * 0.1f;
+            if (near_end) {
+              BOOST_TEST(this_accel >= -kAccel);
+              BOOST_TEST(this_accel <= -kAccel * 0.01f);
+            } else {
+              BOOST_TEST(this_accel == -kAccel);
+            }
             break;
           } else if (this_accel == 0.0f) {
             expected_accel = kZero;
             break;
           } else {
-            negative_violations++;
+            // Spurious sign change.
+            if (this_vel > peak_velocity * 0.1f) {
+              negative_violations++;
+            } else {
+              end_of_trajectory_violations++;
+            }
           }
           break;
         }
@@ -700,12 +715,212 @@ BOOST_AUTO_TEST_CASE(ControlAccelerationConsistent) {
     }
   }
 
-  BOOST_TEST(steps_to_complete >= 440);
+  BOOST_TEST(steps_to_complete >= 420);
   BOOST_TEST(steps_to_complete <= 460);
 
-  // Because of floating point precision issues in the current
-  // implementation, we aren't perfect.  Still, verify that it
-  // *mostly* does what we want.
-  BOOST_TEST(negative_violations <= 15);
-  BOOST_TEST(zero_violations <= 2);
+  // Hysteresis should prevent mid-trajectory sign changes.
+  BOOST_TEST(negative_violations == 0);
+  BOOST_TEST(end_of_trajectory_violations == 0);
+  BOOST_TEST(zero_violations == 0);
+}
+
+// When target moves further out during deceleration, we eventually switch
+// to acceleration and reach the new target.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_TargetFurtherOut,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  constexpr float kNewTarget = 1.0f;  // Further out
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // Run until we're in deceleration phase (after ~224 steps).
+  int step = 0;
+  for (; step < 280; step++) {
+    ctx.Call();
+  }
+
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  BOOST_TEST(ctx.status.control_velocity.value() > 0.0f);
+
+  // Move target further out.
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();
+
+  // Should eventually switch to acceleration and reach new target.
+  bool switched_to_accel = false;
+  for (; step < 10000; step++) {
+    ctx.Call();
+
+    if (!switched_to_accel && ctx.status.control_acceleration > 0.0f) {
+      switched_to_accel = true;
+    }
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  BOOST_TEST(switched_to_accel);
+  // Should reach the new target
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
+}
+
+// When target moves closer during deceleration, we continue decelerating
+// (hysteresis), overshoot the new target, then return to it.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_TargetCloser,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  constexpr float kNewTarget = 0.3f;
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // Run until we're in deceleration phase.
+  int step = 0;
+  for (; step < 280; step++) {
+    ctx.Call();
+  }
+
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  BOOST_TEST(ctx.status.control_velocity.value() > 0.0f);
+
+  // Move target closer.
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();
+
+  // Should overshoot due to hysteresis, then return to target.
+  bool ever_overshot = false;
+
+  for (; step < 10000; step++) {
+    ctx.Call();
+    float pos = ctx.from_raw(ctx.status.control_position_raw.value());
+
+    if (pos > kNewTarget + 0.001f) {
+      ever_overshot = true;
+    }
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  BOOST_TEST(ever_overshot);
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
+}
+
+// Small position increases late in deceleration are handled correctly.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_SmallIncrease,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  constexpr float kNewTarget = 0.52f;
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // Run until late in deceleration phase with low velocity.
+  int step = 0;
+  for (; step < 400; step++) {
+    ctx.Call();
+  }
+
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  const float vel_at_change = ctx.status.control_velocity.value();
+  BOOST_TEST(vel_at_change > 0.0f);
+  BOOST_TEST(vel_at_change < 10.0f);
+
+  // Small increase in target.
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();
+
+  for (; step < 10000; step++) {
+    ctx.Call();
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
+}
+
+// Hysteresis causes continued deceleration even when target moves further out.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_HysteresisEffect,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  constexpr float kNewTarget = 0.6f;
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // Run until mid-deceleration with significant velocity.
+  int step = 0;
+  for (; step < 280; step++) {
+    ctx.Call();
+  }
+
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  const float vel_before = ctx.status.control_velocity.value();
+  BOOST_TEST(vel_before > 5.0f);
+
+  // Move target further out.
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();
+
+  // Track steps where we continue decelerating despite target moving out.
+  int steps_still_decelerating = 0;
+
+  for (; step < 10000; step++) {
+    ctx.Call();
+    float vel = ctx.status.control_velocity.value();
+
+    if (ctx.status.control_acceleration < 0.0f && vel > 0.0f) {
+      steps_still_decelerating++;
+    }
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  // Hysteresis causes continued deceleration for some steps.
+  BOOST_TEST(steps_still_decelerating > 0);
+
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
 }