fix(touch): restore streaming direction tracking during alignment

2026-09-18 15:31:10 +08:00 · 2026-09-18 15:31:10 +08:00 · d51ceab46e
commit d51ceab46e
parent ffccdea26d
11 changed files with 128 additions and 13 deletions
--- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp
+++ b/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) {
--- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h
+++ b/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; }
--- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp
+++ b/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() &&
-            speedl_command_twist_base_.squaredNorm() <= 1e-12) {
+        if (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) {
--- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp
+++ b/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
--- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h
+++ b/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.
-  void setContinuousLinearReversal(bool enabled) { continuous_linear_reversal_ = enabled; }
+  // at zero. Disable for streaming direction tracking (e.g. visual alignment).
+  void setContinuousLinearReversal(bool enabled);
+  bool continuousLinearReversalEnabled() const { return continuous_linear_reversal_; }

  /**
   * @brief 初始化当前 twist 状态
--- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp
+++ b/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();
--- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp
+++ b/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);
+        }
+    }
+}
 }
 }
--- a/cmvr-es/common/types/arm/arm_types.h
+++ b/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};
--- a/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md
+++ b/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` 的旧项目库混用。
--- a/cmvr-es/devices/arm/robot_arm.h
+++ b/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);
--- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
+++ b/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,