fix(touch): restore streaming direction tracking during alignment

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp

@@ -16,6 +16,7 @@ public:
                    double, double, double, FrameType, CartesianJointTrajectory&) override { return false; }
     bool updateSpeedLAcceleration(double) override { return true; }
     bool updateSpeedLLimits(const SpeedLOptions& options) override {
+        applied_reversal.store(options.continuous_linear_reversal.value_or(false));
         applied_jerk.store(options.linear_jerk.value_or(10.0)); return true;
     }
     bool captureSpeedLReference(const std::vector<double>&, const CartesianVelocity& target,
@@ -32,6 +33,7 @@ public:
     CartesianVelocity getSpeedLCommandTwistBase() const override { return current; }
     CartesianVelocity current;
     std::atomic<double> applied_jerk{0.0};
+    std::atomic<bool> applied_reversal{false};
 };
 TEST(CartesianVelocityController, CompletedStopCannotClearNewReversal) {
     std::mutex mutex;
@@ -92,6 +94,7 @@ TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimi
     SpeedLOptions options;
     options.acceleration = 3;
     options.linear_jerk = 60;
+    options.continuous_linear_reversal = true;
     options.capture_reference = true;
     ASSERT_TRUE(controller.speedL(v, options, 0, FrameType::Tool).ok());
     {
@@ -109,6 +112,7 @@ TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimi
     EXPECT_DOUBLE_EQ(ref.tcp_pose_base.y, .5);
     EXPECT_DOUBLE_EQ(ref.target_base.vx, -.08);
     EXPECT_DOUBLE_EQ(planner->applied_jerk.load(), 60);
+    EXPECT_TRUE(planner->applied_reversal.load());
     controller.shutdown();
 }
 TEST(CartesianVelocityController, RejectsInvalidMotionLimitsBeforeStarting) {

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h

@@ -45,7 +45,8 @@ public:
 
     virtual bool updateSpeedLAcceleration(double acceleration) = 0;
     virtual bool updateSpeedLLimits(const SpeedLOptions& options) {
-        return !options.linear_jerk && updateSpeedLAcceleration(options.acceleration);
+        return !options.linear_jerk && !options.continuous_linear_reversal.value_or(false) &&
+               updateSpeedLAcceleration(options.acceleration);
     }
     virtual bool captureSpeedLReference(const std::vector<double>&,
                                        const CartesianVelocity&, FrameType, SpeedLReference&) { return false; }

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp

@@ -922,8 +922,8 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         speedl_stop_active_ = false;
 
         // 从静止开始一个新的 speedL command。
-        if (!twist_limiter_.isMoving() &&
+        if (speedl_command_twist_base_.squaredNorm() <= 1e-12 &&
-            speedl_command_twist_base_.squaredNorm() <= 1e-12) {
+            (!twist_limiter_.continuousLinearReversalEnabled() || !twist_limiter_.isMoving())) {
             twist_limiter_.initialize(
                 Eigen::Matrix<double, 6, 1>::Zero());
         }
@@ -1271,6 +1271,10 @@ bool PinocchioCartesianMotionPlanner::updateSpeedLLimits(const SpeedLOptions& op
     const double angular_acceleration = std::min(options.acceleration,
         positiveOr(speedl_config_.angular_acceleration_max(), 5.0));
     const double jerk = std::min(requested_jerk, jerk_max);
+    // Apply the command's motion policy even when acceleration/jerk are
+    // unchanged. PBVS streams directions; contact retraction locks an axis.
+    twist_limiter_.setContinuousLinearReversal(options.continuous_linear_reversal.value_or(
+        !speedl_config_.has_continuous_linear_reversal() || speedl_config_.continuous_linear_reversal()));
     if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9 &&
         std::abs(speedl_applied_angular_acceleration_ - angular_acceleration) <= 1e-9 &&
         std::abs(speedl_applied_linear_jerk_ - jerk) <= 1e-9) {

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp

@@ -153,5 +153,61 @@ TEST_F(PinocchioSpeedLLimits, InvalidRequestsAreRejectedBeforeClamping) {
         EXPECT_FALSE(planner_->updateSpeedLLimits(options));
     }
 }
+
+TEST_F(PinocchioSpeedLLimits, StreamingAlignmentOverridesGlobalReversalPolicy) {
+    for (const double jerk : {10.0, 30.0}) {
+        SCOPED_TRACE(jerk);
+        limits_.set_continuous_linear_reversal(true);
+        limits_.set_linear_acceleration_max(5);
+        limits_.set_linear_jerk_max(jerk);
+        ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
+        SpeedLOptions options;
+        options.acceleration = .5;
+        CartesianVelocity target;
+        target.vy = .02;
+        sample(target, options, false, 300);
+        ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10);
+
+        // Same acceleration/jerk: the policy override must bypass their cache.
+        options.continuous_linear_reversal = false;
+        double min_speed = .02;
+        double max_error = 0;
+        for (int i = 0; i < 2000; ++i) {
+            const double angle = (i / 20) * .01;
+            target.vx = .02 * std::sin(angle);
+            target.vy = .02 * std::cos(angle);
+            ASSERT_TRUE(planner_->updateSpeedLLimits(options));
+            ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base));
+            const Twist actual = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase());
+            min_speed = std::min(min_speed, actual.head<3>().norm());
+            max_error = std::max(max_error, (actual - common::math::velocityToVector(target)).norm());
+        }
+        EXPECT_NEAR(min_speed, .02, 1e-10);
+        EXPECT_LT(max_error, 1e-10);
+    }
+}
+
+TEST_F(PinocchioSpeedLLimits, ContactReversalStillCrossesZeroAfterStreamingAlignment) {
+    limits_.set_linear_acceleration_max(1);
+    limits_.set_linear_jerk_max(4);
+    ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
+    SpeedLOptions options;
+    options.acceleration = 1;
+    options.continuous_linear_reversal = false;
+    CartesianVelocity target;
+    target.vy = .02;
+    sample(target, options, false, 300);
+    ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10);
+
+    options.continuous_linear_reversal = true;
+    target.vy = -.02;
+    for (int i = 0; i < 100; ++i) {
+        ASSERT_TRUE(planner_->updateSpeedLLimits(options));
+        ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base));
+    }
+    EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-10);
+    ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base));
+    EXPECT_LT(planner_->getSpeedLCommandTwistBase().vy, -.0003);
+}
 } // namespace
 } // namespace cmvr::device

cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h

@@ -69,8 +69,9 @@ public:
                                     double switch_speed_threshold);
 
   // Same-axis reversal uses a signed velocity without resetting acceleration
-  // at zero. Disable only to reproduce the legacy stop-then-switch policy.
+  // at zero. Disable for streaming direction tracking (e.g. visual alignment).
-  void setContinuousLinearReversal(bool enabled) { continuous_linear_reversal_ = enabled; }
+  void setContinuousLinearReversal(bool enabled);
+  bool continuousLinearReversalEnabled() const { return continuous_linear_reversal_; }
 
   /**
    * @brief 初始化当前 twist 状态

cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp

@@ -108,6 +108,25 @@ void CartesianTwistLimiter::setLinearReverseSwitchPolicy(double cos_threshold,
   linear_reverse_switch_speed_threshold_ = std::max(0.0, switch_speed_threshold);
 }
 
+void CartesianTwistLimiter::setContinuousLinearReversal(bool enabled)
+{
+  if (continuous_linear_reversal_ == enabled) return;
+
+  if (!enabled) {
+    const double velocity = linear_norm_planner_.getVelocity();
+    const double acceleration = linear_norm_planner_.getAcceleration();
+    // The streaming policy stores a speed magnitude and a physical direction.
+    // Convert a negative signed state without reversing its physical motion or
+    // dropping its acceleration when a command changes policy mid-retraction.
+    if (velocity < 0.0 || (std::abs(velocity) <= EPSILON && acceleration < 0.0)) {
+      current_linear_dir_base_ = -current_linear_dir_base_;
+      linear_norm_planner_.overwriteState(-velocity, -acceleration, false);
+      prev_measured_linear_norm_ = -prev_measured_linear_norm_;
+    }
+  }
+  continuous_linear_reversal_ = enabled;
+}
+
 void CartesianTwistLimiter::initialize(const Twist& initial_twist)
 {
   restoreNominalConstraints();

cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp

@@ -93,5 +93,26 @@ TEST(CartesianTwistReversal, NonCollinearChangeStopsBeforeSwitchingAxis) {
     }
     EXPECT_NEAR(limiter.getTwistBase().x(), .08, 1e-9);
 }
+
+TEST(CartesianTwistReversal, SwitchingToStreamingPreservesNegativePhysicalVelocity) {
+    for (const int reverse_ticks : {150, 250, 500}) {
+        SCOPED_TRACE(reverse_ticks);
+        CartesianTwistLimiter reference;
+        reference.setLinearConstraints(1, 3, 10);
+        reference.initialize(y(.08));
+        reference.setTargetTwist(y(-.08), CartesianFrame::Base);
+        for (int i = 0; i < reverse_ticks; ++i) reference.update(dt, Eigen::Matrix3d::Identity());
+        ASSERT_LT(reference.getTwistBase().y(), 0);
+        auto streaming = reference;
+        streaming.setContinuousLinearReversal(false);
+        EXPECT_NEAR((streaming.getTwistBase() - reference.getTwistBase()).norm(), 0, 1e-12);
+        for (int i = 0; i < 500; ++i) {
+            const auto expected = reference.update(dt, Eigen::Matrix3d::Identity());
+            const auto actual = streaming.update(dt, Eigen::Matrix3d::Identity());
+            EXPECT_NEAR((actual - expected).norm(), 0, 1e-9);
+            EXPECT_LE(streaming.getJerkBase().norm(), 10 + 1e-6);
+        }
+    }
+}
 }
 }

cmvr-es/common/types/arm/arm_types.h

@@ -71,6 +71,9 @@ struct SpeedLOptions {
     // Requested linear jerk (m/s^3), capped by the arm's configured maximum.
     // Unset uses that maximum.
     std::optional<double> linear_jerk;
+    // Unset follows the arm config. False preserves the original streaming
+    // direction-following policy used by PBVS; true enables fixed-axis reversal.
+    std::optional<bool> continuous_linear_reversal;
     // Capture the first applied command's measured TCP position and target
     // direction in Base. Useful for distance-based motion without caller FK.
     bool capture_reference{false};

cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md

@@ -1,8 +1,8 @@
 # speedL 连续换向与 MuJoCo 验证
 
-同轴线速度换向使用固定轴上的有符号 S 曲线，从当前速度和加速度直接规划到反向目标。过零时保留加速度，不重置规划器。非同轴变向先停止再换轴；角速度仍使用原有策略。
+触控接近和回撤的同轴线速度换向使用固定轴上的有符号 S 曲线，从当前速度和加速度直接规划到反向目标。过零时保留加速度，不重置规划器。该模式下非同轴变向先停止再换轴；角速度仍使用原有策略。
 
-`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true；设为 false 可以复现旧的同轴停止后换向策略，用于同条件比较。
+`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true；`SpeedLOptions.continuous_linear_reversal` 可按命令覆盖。触控任务的 PBVS 对齐明确设为 false，保持原有的小角度方向跟随和低速换轴行为；接近及回撤设为 true。固定轴模式不适合方向不断变化的视觉对齐：小方向变化会累积到换轴阈值，导致反复停走。设为 false 也可用于与旧换向策略做同条件比较。运动中退出固定轴模式时会保留实际运动方向和加速度。
 
 `MotorRobotArm::speedL(velocity, SpeedLOptions, duration, frame)` 可以为本条命令指定 `linear_jerk`（m/s³）。未指定时使用机器人配置的 jerk；普通 `speedL(velocity, acceleration, duration, frame)` 仍可直接使用。控制线程将速度、加速度、jerk 作为同一命令读取。正常停止的收尾操作检查命令版本，避免清除之后提交的运动命令。
 
@@ -74,4 +74,6 @@ touch {
 
 `pinocchio_speedl_limits_test` 使用真实 URDF 和生产规划器，采样输出速度并差分验证实际规划的速度、加速度、jerk 上限；覆盖超限请求、较小请求、省略 jerk、旧接口及停止、独立角加速度限幅和非法请求。测试不初始化电机。
 
+对齐回归覆盖每 20 ms 小幅更新速度方向：显式关闭固定轴模式后，20 mm/s 的命令不会因方向变化反复降到零；同时验证对齐后启用连续换向仍能平滑过零，以及反向运动中退出固定轴模式不会翻转实际速度。
+
 运行时应优先加载当前构建的项目动态库，测试脚本已处理；不要把新可执行文件与 `output/lib` 的旧项目库混用。

cmvr-es/devices/arm/robot_arm.h

@@ -62,7 +62,8 @@ public:
                           const SpeedLOptions& options,
                           double duration,
                           FrameType frame = FrameType::Base) {
-        if (options.linear_jerk || options.capture_reference) {
+        if (options.linear_jerk || options.capture_reference ||
+            options.continuous_linear_reversal.value_or(false)) {
             return Result::failure(ArmErrorCode::UnsupportedCommand, "speedL options are not supported by this arm");
         }
         return speedL(velocity, options.acceleration, duration, frame);

cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp

@@ -1525,14 +1525,15 @@ bool TouchScreenTask::stepAligning(const double dt) {
         return true;
     }
 
+    device::SpeedLOptions alignment_options;
+    alignment_options.acceleration = pbvs_command_acceleration_;
+    // PBVS changes direction on each visual update. Keep the original
+    // direction-following policy instead of repeatedly braking to change axes.
+    alignment_options.continuous_linear_reversal = false;
     const auto speed_result = arm_->speedL(twist_B,
-                                           pbvs_command_acceleration_,
+                                           alignment_options,
                                            0.0,
                                            device::FrameType::Base);
-    // const auto speed_result = arm_->speedL({0,0,0,0,0,0},
-    //                                pbvs_command_acceleration_,
-    //                                0.0,
-    //                                device::FrameType::Base);
     if (!speed_result.ok()) {
         CMVR_LOG(ERROR) << "[TouchScreenTask][ALIGNING] speedL failed: "
                         << speed_result.message;
@@ -1948,6 +1949,7 @@ bool TouchScreenTask::startTouchPhase() {
         device::SpeedLOptions options;
         options.acceleration = speed_l.acceleration();
         if (speed_l.has_linear_jerk()) options.linear_jerk = speed_l.linear_jerk();
+        options.continuous_linear_reversal = true;
         const auto result = arm_->speedL(toCartesianVelocity(
                                              cmvr::common::math::toEigenVec6(speed_l.twist_tool())),
                                          options,
@@ -2014,6 +2016,7 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
     device::SpeedLOptions options;
     options.acceleration = retract.acceleration();
     if (retract.has_linear_jerk()) options.linear_jerk = retract.linear_jerk();
+    options.continuous_linear_reversal = true;
     options.capture_reference = true;
     const auto result = arm_->speedL(retract_cmd,
                                      options,