fix(touch): improve reversal planning and enforce speedL limits

Preserve acceleration through same-axis velocity reversal and submit retract
commands immediately after zero-dwell tactile contact. Capture the applied
command's TCP reference and measure signed retract progress, with logging of
the maximum sampled forward displacement after reversal starts.

Support per-command linear jerk in both touch.speed_l and retract, and cap
requested acceleration and jerk at the configured arm limits. Protect command
snapshots and stop completion against newer speedL submissions. Include the
current QP arm and retract parameter tuning and remove an obsolete config field.

Add headless actuator-driven MuJoCo reversal measurement and focused regression
coverage for reversal continuity, controller concurrency, limit enforcement,
and touch jerk configuration.

Validation: cmvr_es and touch_screen_task_test build successfully; 18 focused
regression tests pass, along with headless MuJoCo reversal checks.

cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt

@@ -11,3 +11,7 @@ target_link_libraries(arm_control
 
 add_library(cmvr_es::algorithms::arm_control ALIAS arm_control)
 install(TARGETS arm_control LIBRARY DESTINATION lib)
+
+add_executable(cartesian_velocity_controller_test src/cartesian_velocity_controller_test.cpp)
+target_link_libraries(cartesian_velocity_controller_test PRIVATE
+        cmvr_es::algorithms::arm_control gtest gtest_main pthread)

cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h

@@ -46,6 +46,9 @@ public:
                   double duration,
                   FrameType frame);
     Result stop(std::optional<double> acceleration = std::nullopt);
+    Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options,
+                  double duration, FrameType frame);
+    SpeedLReference getReference() const;
     void shutdown();
 
     bool busy() const { return busy_.load(); }
@@ -77,6 +80,9 @@ private:
     CartesianVelocity target_twist_{};
     FrameType target_frame_{FrameType::Base};
     double target_acceleration_{0.25};
+    SpeedLOptions target_options_{};
+    SpeedLReference reference_{};
+    CartesianVelocity command_twist_snapshot_{};
     std::uint64_t command_version_{0};
     std::atomic<bool> busy_{false};
 };

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

@@ -61,16 +61,26 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
                                            const double duration,
                                            const FrameType frame)
 {
-    if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
+    SpeedLOptions options;
+    options.acceleration = acceleration;
+    return speedL(velocity, options, duration, frame);
+}
+
+Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
+                                           const SpeedLOptions& options,
+                                           const double duration, const FrameType frame)
+{
+    const double acceleration = options.acceleration;
+    if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 ||
+        !std::isfinite(acceleration) || acceleration <= 0.0 ||
+        !std::isfinite(duration) || duration < 0.0 || !std::isfinite(twistNorm_(velocity)) ||
+        (options.linear_jerk && (!std::isfinite(*options.linear_jerk) || *options.linear_jerk <= 0.0))) {
         return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
     }
     if (!worker_ || !worker_->joinable()) {
         if (busy_.exchange(true)) {
             return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
         }
-    } else {
-        // speedL is a streaming command: an existing worker may receive a new target.
-        busy_.store(true);
     }
 
     ensureWorkerStarted_();
@@ -81,7 +91,10 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
         target_twist_ = velocity;
         target_acceleration_ = acceleration;
         target_frame_ = frame;
+        target_options_ = options;
+        reference_ = {};
         command_active_ = true;
+        busy_.store(true);
         command_version = ++command_version_;
     }
     cv_.notify_all();
@@ -143,10 +156,14 @@ void CartesianVelocityController::shutdown()
 
 CartesianVelocity CartesianVelocityController::getCommandTwistBase() const
 {
-    if (!planner_) {
+    std::lock_guard<std::mutex> lock(mutex_);
-        return {};
+    return command_twist_snapshot_;
-    }
+}
-    return planner_->getSpeedLCommandTwistBase();
+
+SpeedLReference CartesianVelocityController::getReference() const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    return reference_;
 }
 
 void CartesianVelocityController::ensureWorkerStarted_()
@@ -167,6 +184,9 @@ void CartesianVelocityController::workerLoop_()
         CartesianVelocity target_twist;
         double acceleration = 0.25;
         FrameType target_frame = FrameType::Base;
+        SpeedLOptions options;
+        std::uint64_t applied_version = 0;
+        bool capture_reference = false;
         {
             std::unique_lock<std::mutex> lock(mutex_);
             cv_.wait(lock, [&]() {
@@ -195,9 +215,13 @@ void CartesianVelocityController::workerLoop_()
                 target_twist = target_twist_;
                 acceleration = target_acceleration_;
                 target_frame = target_frame_;
+                options = target_options_;
+                options.acceleration = acceleration;
+                applied_version = command_version_;
+                capture_reference = options.capture_reference && !reference_.valid;
             }
 
-            if (!planner_->updateSpeedLAcceleration(acceleration)) {
+            if (!planner_->updateSpeedLLimits(options)) {
                 if (twistNorm_(target_twist) < config_.stop_twist_norm) {
                     abortCommand_();
                     break;
@@ -221,6 +245,13 @@ void CartesianVelocityController::workerLoop_()
             }
 
             std::vector<double> qd_cmd;
+            SpeedLReference reference;
+            if (capture_reference &&
+                !planner_->captureSpeedLReference(q_now, target_twist, target_frame, reference)) {
+                CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] cannot capture motion reference";
+                abortCommand_();
+                break;
+            }
             if (!planner_->speedLStep(target_twist, dt, q_now, qd_now, qd_cmd, target_frame)) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] speedLStep failed, target_twist=["
                                 << target_twist.vx << ", " << target_twist.vy << ", "
@@ -249,15 +280,32 @@ void CartesianVelocityController::workerLoop_()
                 continue;
             }
 
+            {
+                std::lock_guard<std::mutex> lock(mutex_);
+                command_twist_snapshot_ = planner_->getSpeedLCommandTwistBase();
+                if (capture_reference && command_version_ == applied_version) {
+                    reference.command_version = applied_version;
+                    reference_ = reference;
+                    // Lock a captured Tool-frame translation in Base for the
+                    // entire command, matching its distance reference axis.
+                    target_twist_ = reference.target_base;
+                    target_frame_ = FrameType::Base;
+                }
+            }
+
             if (twistNorm_(target_twist) < config_.stop_twist_norm &&
                 velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm &&
                 velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) {
                 {
                     std::lock_guard<std::mutex> lock(mutex_);
+                    // A newer target may have arrived during planning/I/O.
+                    if (command_version_ != applied_version) continue;
                     command_active_ = false;
+                    // Serialize the final zero and busy transition with new
+                    // submissions, not only the version comparison.
+                    sendZero_();
+                    busy_.store(false);
                 }
-                sendZero_();
-                busy_.store(false);
                 break;
             }
 
@@ -279,6 +327,8 @@ void CartesianVelocityController::requestStop_(const std::optional<double> accel
         target_frame_ = FrameType::Base;
         target_acceleration_ = acceleration.has_value() ? *acceleration
                                                          : config_.stop_acceleration;
+        target_options_.acceleration = target_acceleration_;
+        target_options_.capture_reference = false;
         command_active_ = true;
         ++command_version_;
     }
@@ -292,9 +342,9 @@ void CartesianVelocityController::abortCommand_()
         command_active_ = false;
         target_twist_ = {};
         target_frame_ = FrameType::Base;
+        sendZero_();
+        busy_.store(false);
     }
-    sendZero_();
-    busy_.store(false);
 }
 
 void CartesianVelocityController::sendZero_()

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

@@ -0,0 +1,127 @@
+#include <gtest/gtest.h>
+#include "algorithms/controllers/arm_control/include/cartesian_velocity_controller.h"
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <limits>
+
+namespace cmvr::device {
+namespace {
+using namespace std::chrono_literals;
+class Planner final : public CartesianMotionPlanner {
+public:
+    bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override { return true; }
+    bool configureMoveL(const config::MoveLPlannerConfig&) override { return true; }
+    bool planMoveL(const CartesianPose&, const std::vector<double>&, const std::vector<double>&,
+                   double, double, double, FrameType, CartesianJointTrajectory&) override { return false; }
+    bool updateSpeedLAcceleration(double) override { return true; }
+    bool updateSpeedLLimits(const SpeedLOptions& options) override {
+        applied_jerk.store(options.linear_jerk.value_or(10.0)); return true;
+    }
+    bool captureSpeedLReference(const std::vector<double>&, const CartesianVelocity& target,
+                               FrameType, SpeedLReference& ref) override {
+        ref.valid = true;
+        ref.tcp_pose_base.y = .5;
+        ref.target_base.vx = target.vy;
+        return true;
+    }
+    bool speedLStep(const CartesianVelocity& v, double, const std::vector<double>&,
+                    const std::vector<double>&, std::vector<double>& out, FrameType) override {
+        current = v; out = {v.vy}; return true;
+    }
+    CartesianVelocity getSpeedLCommandTwistBase() const override { return current; }
+    CartesianVelocity current;
+    std::atomic<double> applied_jerk{0.0};
+};
+TEST(CartesianVelocityController, CompletedStopCannotClearNewReversal) {
+    std::mutex mutex;
+    std::condition_variable cv;
+    bool forward_sent = false, block_zero = false, zero_entered = false;
+    bool release_zero = false, reverse_sent = false;
+    auto planner = std::make_shared<Planner>();
+    CartesianVelocityController controller({}, planner, 1,
+        [](auto& q, auto& qd) { q = {0}; qd = {0}; return true; },
+        [&](const JointVelocityCommand& command, double) {
+            std::unique_lock<std::mutex> lock(mutex);
+            forward_sent |= command.velocity[0] > 0;
+            reverse_sent |= command.velocity[0] < 0;
+            if (command.velocity[0] == 0 && block_zero && !zero_entered) {
+                zero_entered = true; cv.notify_all();
+                cv.wait_for(lock, 2s, [&] { return release_zero; });
+            }
+            cv.notify_all(); return Result::success();
+        });
+    CartesianVelocity v; v.vy = .08;
+    ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok());
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return forward_sent; }));
+        block_zero = true;
+    }
+    ASSERT_TRUE(controller.stop(3).ok());
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return zero_entered; }));
+    }
+    v.vy = -.08;
+    ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok());
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        release_zero = true; cv.notify_all();
+        EXPECT_TRUE(cv.wait_for(lock, 1s, [&] { return reverse_sent; }));
+    }
+    EXPECT_TRUE(controller.busy());
+    controller.shutdown();
+}
+TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimits) {
+    std::mutex mutex;
+    std::condition_variable cv;
+    bool entered = false, release = false;
+    auto planner = std::make_shared<Planner>();
+    CartesianVelocityController controller({}, planner, 1,
+        [](auto& q, auto& qd) { q = {0}; qd = {0}; return true; },
+        [&](const JointVelocityCommand&, double) {
+            std::unique_lock<std::mutex> lock(mutex);
+            if (!entered) {
+                entered = true; cv.notify_all();
+                cv.wait_for(lock, 2s, [&] { return release; });
+            }
+            return Result::success();
+        });
+    CartesianVelocity v; v.vy = -.08;
+    SpeedLOptions options;
+    options.acceleration = 3;
+    options.linear_jerk = 60;
+    options.capture_reference = true;
+    ASSERT_TRUE(controller.speedL(v, options, 0, FrameType::Tool).ok());
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return entered; }));
+        EXPECT_FALSE(controller.getReference().valid);
+        release = true; cv.notify_all();
+    }
+    const auto deadline = std::chrono::steady_clock::now() + 1s;
+    while (!controller.getReference().valid && std::chrono::steady_clock::now() < deadline)
+        std::this_thread::sleep_for(1ms);
+    const auto ref = controller.getReference();
+    EXPECT_TRUE(ref.valid);
+    EXPECT_GT(ref.command_version, 0U);
+    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);
+    controller.shutdown();
+}
+TEST(CartesianVelocityController, RejectsInvalidMotionLimitsBeforeStarting) {
+    auto planner = std::make_shared<Planner>();
+    CartesianVelocityController controller({}, planner, 1,
+        [](auto&, auto&) { return false; },
+        [](const auto&, double) { return Result::success(); });
+    SpeedLOptions options;
+    options.linear_jerk = std::numeric_limits<double>::quiet_NaN();
+    EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok());
+    options.linear_jerk = -1;
+    EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok());
+    EXPECT_FALSE(controller.busy());
+}
+}
+}

cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt

@@ -25,3 +25,11 @@ target_link_libraries(toppra_joint_motion_planner_test
         gtest
         gtest_main
 )
+
+add_executable(pinocchio_speedl_limits_test
+        cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp
+)
+target_compile_definitions(pinocchio_speedl_limits_test PRIVATE
+        CMVR_TEST_SOURCE_DIR="${PROJECT_SOURCE_DIR}")
+target_link_libraries(pinocchio_speedl_limits_test PRIVATE
+        cmvr_es::algorithms::arm_motion gtest gtest_main)

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

@@ -44,6 +44,11 @@ public:
                             FrameType frame) = 0;
 
     virtual bool updateSpeedLAcceleration(double acceleration) = 0;
+    virtual bool updateSpeedLLimits(const SpeedLOptions& options) {
+        return !options.linear_jerk && updateSpeedLAcceleration(options.acceleration);
+    }
+    virtual bool captureSpeedLReference(const std::vector<double>&,
+                                       const CartesianVelocity&, FrameType, SpeedLReference&) { return false; }
     virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0;
 };
 

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h

@@ -37,6 +37,9 @@ public:
                     FrameType frame) override;
 
     bool updateSpeedLAcceleration(double acceleration) override;
+    bool updateSpeedLLimits(const SpeedLOptions& options) override;
+    bool captureSpeedLReference(const std::vector<double>& q, const CartesianVelocity& target,
+                               FrameType frame, SpeedLReference& reference) override;
     CartesianVelocity getSpeedLCommandTwistBase() const override;
 
 private:
@@ -96,6 +99,8 @@ private:
     Eigen::Vector3d speedl_line_start_tcp_base_{Eigen::Vector3d::Zero()};
     Eigen::Vector3d speedl_line_direction_base_{Eigen::Vector3d::Zero()};
     double speedl_applied_acceleration_{0.25};
+    double speedl_applied_angular_acceleration_{0.25};
+    double speedl_applied_linear_jerk_{-1.0};
     bool speedl_line_check_active_{false};
     bool speedl_line_deviation_warned_{false};
     bool speedl_line_direction_warned_{false};

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

@@ -121,6 +121,7 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne
     }
 
     cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_);
+    speedl_applied_linear_jerk_ = -1.0;
 
     prev_qdot_command_.assign(dof, 0.0);
     speedl_command_twist_base_.setZero();
@@ -129,6 +130,7 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne
     speedl_line_direction_warned_ = false;
     speedl_stop_active_ = false;
     speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0);
+    speedl_applied_angular_acceleration_ = positiveOr(speedl_config_.angular_acceleration_max(), 5.0);
     speedl_configured_ = true;
     return true;
 }
@@ -920,7 +922,8 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         speedl_stop_active_ = false;
 
         // 从静止开始一个新的 speedL command。
-        if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
+        if (!twist_limiter_.isMoving() &&
+            speedl_command_twist_base_.squaredNorm() <= 1e-12) {
             twist_limiter_.initialize(
                 Eigen::Matrix<double, 6, 1>::Zero());
         }
@@ -1249,18 +1252,60 @@ std::string PinocchioCartesianMotionPlanner::describeJointLimitCandidates_(
 
 bool PinocchioCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration)
 {
-    if (!speedl_configured_ || acceleration <= 0.0) {
+    SpeedLOptions options;
+    options.acceleration = acceleration;
+    return updateSpeedLLimits(options);
+}
+
+bool PinocchioCartesianMotionPlanner::updateSpeedLLimits(const SpeedLOptions& options)
+{
+    const double jerk_max = positiveOr(speedl_config_.linear_jerk_max(), 10.0);
+    const double requested_jerk = options.linear_jerk.value_or(jerk_max);
+    // Validate before clamping: invalid requests must not become valid limits.
+    if (!speedl_configured_ || !std::isfinite(options.acceleration) || options.acceleration <= 0.0 ||
+        !std::isfinite(requested_jerk) || requested_jerk <= 0.0) {
         return false;
     }
-    if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) {
+    const double acceleration = std::min(options.acceleration,
+        positiveOr(speedl_config_.linear_acceleration_max(), 5.0));
+    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);
+    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) {
         return true;
     }
 
-    cartesian_motion::updateTwistLimiterAcceleration(
+    twist_limiter_.setLinearConstraints(
-        twist_limiter_,
+        positiveOr(speedl_config_.linear_velocity_max(), .55),
-        speedl_config_,
+        acceleration, jerk);
-        acceleration);
+    twist_limiter_.setAngularConstraints(
+        positiveOr(speedl_config_.angular_velocity_max(), 1.0),
+        angular_acceleration, positiveOr(speedl_config_.angular_jerk_max(), 12.0));
     speedl_applied_acceleration_ = acceleration;
+    speedl_applied_angular_acceleration_ = angular_acceleration;
+    speedl_applied_linear_jerk_ = jerk;
+    return true;
+}
+
+bool PinocchioCartesianMotionPlanner::captureSpeedLReference(
+    const std::vector<double>& q, const CartesianVelocity& target,
+    const FrameType frame, SpeedLReference& reference)
+{
+    Eigen::Matrix4d pose;
+    if (!solver_ || !solver_->fk(q, pose, true) || !pose.allFinite()) return false;
+    auto twist = common::math::velocityToVector(target);
+    if (frame == FrameType::Tool) {
+        const Eigen::Matrix3d rotation = pose.block<3, 3>(0, 0);
+        twist.head<3>() = rotation * twist.head<3>().eval();
+        twist.tail<3>() = rotation * twist.tail<3>().eval();
+    } else if (frame != FrameType::Base) {
+        return false;
+    }
+    reference.tcp_pose_base = common::math::matrixToPose(pose);
+    reference.target_base = common::math::vectorToVelocity(twist);
+    reference.valid = true;
     return true;
 }
 

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

@@ -0,0 +1,157 @@
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <cmath>
+#include <filesystem>
+#include <limits>
+
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
+#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h"
+#include "common/io/proto_file_io.h"
+#include "common/math/transform_math.h"
+
+namespace cmvr::device {
+namespace {
+constexpr double kDt = .001;
+using Twist = Eigen::Matrix<double, 6, 1>;
+
+// Exercise the production planner and real URDF kinematics, without motor I/O.
+class PinocchioSpeedLLimits : public ::testing::Test {
+protected:
+    void SetUp() override {
+        const auto root = std::filesystem::path(CMVR_TEST_SOURCE_DIR);
+        config::ArmRootConfig arms;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (root / "cmvr-es/config/devices/arm/arm.pb.txt").string(), &arms));
+        ASSERT_GT(arms.arm().robot_arms_size(), 0);
+        auto ik = arms.arm().robot_arms(0).kinematics().pinocchio_dls_ik_solver();
+        ik.set_urdf_path((root / "model/xiaoyan_description/dual_arm.urdf").string());
+        solver_ = std::make_shared<PinocchioDlsIKSolver>(ik);
+        ASSERT_TRUE(solver_->init());
+        planner_ = std::make_unique<PinocchioCartesianMotionPlanner>(solver_);
+        limits_.set_linear_velocity_max(.2);
+        limits_.set_linear_acceleration_max(.4);
+        limits_.set_linear_jerk_max(2);
+        limits_.set_angular_velocity_max(1);
+        limits_.set_angular_acceleration_max(.8);
+        limits_.set_angular_jerk_max(4);
+        limits_.set_enforce_joint_acceleration_limits(false);
+        ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
+    }
+
+    struct Peaks { double velocity{0}, acceleration{0}, jerk{0}; };
+    Peaks sample(const CartesianVelocity& target, const SpeedLOptions& options,
+                 bool angular = false, int steps = 2200, bool legacy_api = false) {
+        Peaks peaks;
+        Twist previous = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase());
+        Twist previous_acceleration = Twist::Zero();
+        for (int i = 0; i < steps; ++i) {
+            const bool updated = legacy_api
+                ? planner_->updateSpeedLAcceleration(options.acceleration)
+                : planner_->updateSpeedLLimits(options);
+            if (!updated || !planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)) {
+                ADD_FAILURE() << "Planning failed at sample " << i;
+                break;
+            }
+            const Twist velocity = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase());
+            const Twist acceleration = (velocity - previous) / kDt;
+            const Twist jerk = (acceleration - previous_acceleration) / kDt;
+            const int offset = angular ? 3 : 0;
+            peaks.velocity = std::max(peaks.velocity, velocity.segment<3>(offset).norm());
+            peaks.acceleration = std::max(peaks.acceleration, acceleration.segment<3>(offset).norm());
+            peaks.jerk = std::max(peaks.jerk, jerk.segment<3>(offset).norm());
+            previous = velocity;
+            previous_acceleration = acceleration;
+        }
+        return peaks;
+    }
+
+    void expectPeaks(const Peaks& p, double velocity, double acceleration, double jerk) {
+        // These trajectories contain plateaus: limits must be reached, as well
+        // as obeyed, so an unintended smaller cap cannot pass the test.
+        EXPECT_NEAR(p.velocity, velocity, 1e-8);
+        EXPECT_NEAR(p.acceleration, acceleration, 1e-8);
+        EXPECT_NEAR(p.jerk, jerk, 1e-6);
+    }
+
+    std::shared_ptr<PinocchioDlsIKSolver> solver_;
+    std::unique_ptr<PinocchioCartesianMotionPlanner> planner_;
+    config::SpeedLPlannerConfig limits_;
+    const std::vector<double> q_{.25, 1, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0};
+    const std::vector<double> qd_ = std::vector<double>(7, 0);
+    std::vector<double> command_;
+};
+
+TEST_F(PinocchioSpeedLLimits, ExcessiveRequestsRespectAllLinearLimits) {
+    SpeedLOptions options;
+    options.acceleration = 60;
+    options.linear_jerk = 60;
+    CartesianVelocity target;
+    target.vx = .6;
+    target.vy = .8;
+    expectPeaks(sample(target, options), .2, .4, 2);
+}
+
+TEST_F(PinocchioSpeedLLimits, LowerRequestsRemainEffective) {
+    SpeedLOptions options;
+    options.acceleration = .15;
+    options.linear_jerk = .8;
+    CartesianVelocity target;
+    target.vy = 1;
+    expectPeaks(sample(target, options), .2, .15, .8);
+}
+
+TEST_F(PinocchioSpeedLLimits, OmittedJerkRestoresArmLimit) {
+    SpeedLOptions options;
+    options.acceleration = .4;
+    options.linear_jerk = .3;
+    ASSERT_TRUE(planner_->updateSpeedLLimits(options));
+    options.linear_jerk.reset();
+    CartesianVelocity target;
+    target.vy = 1;
+    expectPeaks(sample(target, options), .2, .4, 2);
+}
+
+TEST_F(PinocchioSpeedLLimits, LegacyAccelerationAndStopAreCapped) {
+    SpeedLOptions options;
+    options.acceleration = 60;
+    CartesianVelocity target;
+    target.vy = 1;
+    expectPeaks(sample(target, options, false, 1200, true), .2, .4, 2);
+    const auto stop = sample({}, options, false, 1200, true);
+    EXPECT_LE(stop.velocity, .2);
+    EXPECT_NEAR(stop.acceleration, .4, 1e-8);
+    EXPECT_NEAR(stop.jerk, 2, 1e-6);
+    EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-12);
+}
+
+TEST_F(PinocchioSpeedLLimits, AngularAccelerationHasItsOwnCapAndCache) {
+    limits_.set_linear_acceleration_max(.2);
+    ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
+    SpeedLOptions options;
+    options.acceleration = .3;
+    ASSERT_TRUE(planner_->updateSpeedLLimits(options));
+    // Linear effective acceleration stays at .2, but angular must change.
+    options.acceleration = .6;
+    CartesianVelocity target;
+    target.wy = 2;
+    expectPeaks(sample(target, options, true), 1, .6, 4);
+    ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
+    options.acceleration = 60;
+    expectPeaks(sample(target, options, true), 1, .8, 4);
+}
+
+TEST_F(PinocchioSpeedLLimits, InvalidRequestsAreRejectedBeforeClamping) {
+    for (double invalid : {0.0, -1.0, std::numeric_limits<double>::infinity(),
+                           std::numeric_limits<double>::quiet_NaN()}) {
+        SpeedLOptions options;
+        options.acceleration = invalid;
+        EXPECT_FALSE(planner_->updateSpeedLLimits(options));
+        EXPECT_FALSE(planner_->updateSpeedLAcceleration(invalid));
+        options.acceleration = .3;
+        options.linear_jerk = invalid;
+        EXPECT_FALSE(planner_->updateSpeedLLimits(options));
+    }
+}
+} // namespace
+} // namespace cmvr::device

cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h

@@ -1,6 +1,8 @@
 #ifndef CMVR_ES_TWIST_LIMITER_CONFIG_H
 #define CMVR_ES_TWIST_LIMITER_CONFIG_H
 
+#include <algorithm>
+
 #include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
 #include "cmvr/config/arm_config/arm_config.pb.h"
 #include "common/config/config_files.h"
@@ -28,6 +30,8 @@ inline void configureTwistLimiterFromSpeedLConfig(
             ? config.linear_reverse_cos_threshold()
             : -0.8660254037844386,
         positiveOr(config.linear_reverse_switch_speed_threshold(), 1e-3));
+    limiter.setContinuousLinearReversal(!config.has_continuous_linear_reversal() ||
+                                       config.continuous_linear_reversal());
     limiter.initialize(Eigen::Matrix<double, 6, 1>::Zero());
 }
 
@@ -39,10 +43,10 @@ inline void updateTwistLimiterAcceleration(
     using cmvr::common::config::positiveOr;
 
     limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55),
-                                 acceleration,
+                                 std::min(acceleration, positiveOr(config.linear_acceleration_max(), 5.0)),
                                  positiveOr(config.linear_jerk_max(), 10.0));
     limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0),
-                                  acceleration,
+                                  std::min(acceleration, positiveOr(config.angular_acceleration_max(), 5.0)),
                                   positiveOr(config.angular_jerk_max(), 12.0));
 }
 

cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt

@@ -20,6 +20,10 @@ target_link_libraries(base_motion PUBLIC
 )
 
 add_library(cmvr_es::base_motion ALIAS base_motion)
+add_executable(cartesian_twist_limiter_reversal_test
+        cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp)
+target_link_libraries(cartesian_twist_limiter_reversal_test PRIVATE
+        cmvr_es::base_motion gtest gtest_main)
 install(TARGETS base_motion LIBRARY DESTINATION lib)
 
 add_executable(toppra_multi_waypoint_test

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

@@ -16,7 +16,8 @@ enum class CartesianFrame
  * @brief 6维末端 twist 在线限幅器（基于 SCurveVelocityPlanner1D）
  *
  * 线速度部分：
- * - 模长使用 SCurveVelocityPlanner1D 做 jerk-limited 速度规划
+ * - 固定轴上的有符号速度使用 SCurveVelocityPlanner1D 做 jerk-limited 规划
+ * - 同轴反向连续过零，保留加速度；可显式选择旧的停止后换向策略
  * - 运动中锁定当前方向，不做方向插值
  * - 若目标方向与当前方向不共线，则采用“先减速到0，再切方向”的 switch policy
  *
@@ -67,6 +68,10 @@ public:
   void setLinearReverseSwitchPolicy(double cos_threshold,
                                     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; }
+
   /**
    * @brief 初始化当前 twist 状态
    *
@@ -120,7 +125,7 @@ public:
    *
    * 作用：
    * - 更新当前执行方向
-   * - 用测得模长与模长加速度同步两个 planner
+   * - 线速度使用固定轴投影（保留正负号），角速度使用模长
    * - keep_target=true 时：
    *   - 若测量状态仍贴着当前 profile，则保持当前 profile
    *   - 否则由 planner 内部从测量状态重规划到当前目标
@@ -178,6 +183,7 @@ private:
   double linear_reverse_switch_speed_threshold_;
   double angular_switch_speed_threshold_;
   bool emergency_stop_active_;
+  bool continuous_linear_reversal_{true};
 
   // 目标/当前状态
   Twist target_twist_input_;

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

@@ -249,7 +249,8 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base,
   const Eigen::Vector3d v_meas = measured_twist_base.head<3>();
   const Eigen::Vector3d w_meas = measured_twist_base.tail<3>();
 
-  const double v_norm = v_meas.norm();
+  const bool signed_linear = continuous_linear_reversal_ && current_linear_dir_base_.norm() > EPSILON;
+  const double v_norm = signed_linear ? v_meas.dot(current_linear_dir_base_) : v_meas.norm();
   const double w_norm = w_meas.norm();
 
   double v_acc = 0.0;
@@ -273,7 +274,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base,
   last_measured_twist_base_ = measured_twist_base;
   has_measured_sync_ = true;
 
-  // 用测得模长/模长加速度同步 planner 当前状态。
+  // 线速度按固定轴投影保留正负号；角速度仍按模长同步 planner。
   // keep_target=true:
   //   - 若测量值仍贴着当前 profile，则继续沿旧 profile 走
   //   - 否则从测量状态重规划到当前目标
@@ -289,7 +290,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base,
     target_twist_base_.setZero();
   }
 
-  if (v_norm > EPSILON) {
+  if (!signed_linear && v_norm > EPSILON) {
     current_linear_dir_base_ = v_meas / v_norm;
     last_target_linear_dir_base_ = current_linear_dir_base_;
   }
@@ -464,7 +465,18 @@ CartesianTwistLimiter::update(double dt, const Eigen::Matrix3d& base_R_tool)
     const bool must_switch_axis =
         !same_axis || dir_dot < linear_reverse_cos_threshold_;
 
-    if (must_switch_axis && v_cur_norm > linear_reverse_switch_speed_threshold_) {
+    if (continuous_linear_reversal_ && same_axis) {
+      // Keep the axis fixed: the scalar profile carries the direction sign.
+      // Crossing zero is an interior point, with continuous acceleration.
+      linear_norm_planner_.setTargetVelocity(v_des.dot(current_linear_dir_base_));
+    } else if (continuous_linear_reversal_ &&
+               (std::abs(v_cur_norm) > EPSILON ||
+                std::abs(linear_norm_planner_.getAcceleration()) > EPSILON ||
+                linear_norm_planner_.hasActiveProfile())) {
+      // A different axis may only be adopted after the old profile settles.
+      linear_norm_planner_.setTargetVelocity(0.0);
+    } else if (!continuous_linear_reversal_ && must_switch_axis &&
+               v_cur_norm > linear_reverse_switch_speed_threshold_) {
       linear_norm_planner_.setTargetVelocity(0.0);
     } else {
       current_linear_dir_base_ = v_target_dir;

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

@@ -0,0 +1,97 @@
+#include <gtest/gtest.h>
+#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
+
+namespace cmvr {
+namespace {
+using Twist = CartesianTwistLimiter::Twist;
+constexpr double dt = .001;
+Twist y(double v) { Twist t = Twist::Zero(); t.y() = v; return t; }
+struct Motion { double peak{0}, reverse_time{-1}, return_time{-1}; };
+Motion reverse(bool continuous, int approach_ticks) {
+    CartesianTwistLimiter limiter;
+    limiter.setLinearConstraints(.55, 5, 10);
+    limiter.setContinuousLinearReversal(continuous);
+    limiter.initialize(approach_ticks == 0 ? y(.08) : Twist::Zero());
+    limiter.setTargetTwist(y(.08), CartesianFrame::Base);
+    for (int i = 0; i < approach_ticks; ++i) limiter.update(dt, Eigen::Matrix3d::Identity());
+    limiter.setLinearConstraints(.55, 3, 10);
+    limiter.setTargetTwist(y(-.08), CartesianFrame::Base);
+    Motion result;
+    double position = 0;
+    for (int i = 1; i <= 1500; ++i) {
+        const auto v = limiter.update(dt, Eigen::Matrix3d::Identity());
+        position += v.y() * dt;
+        result.peak = std::max(result.peak, position);
+        if (v.y() < 0 && result.reverse_time < 0) result.reverse_time = i * dt;
+        if (result.reverse_time > 0 && position <= 0 && result.return_time < 0) result.return_time = i * dt;
+        if (continuous) {
+            EXPECT_LE(limiter.getAccelerationBase().norm(), 3.0 + 1e-7);
+            EXPECT_LE(limiter.getJerkBase().norm(), 10.0 + 1e-6);
+            EXPECT_NEAR(v.x(), 0, 1e-12);
+            EXPECT_NEAR(v.z(), 0, 1e-12);
+        }
+    }
+    EXPECT_NEAR(limiter.getTwistBase().y(), -.08, 1e-9);
+    return result;
+}
+TEST(CartesianTwistReversal, ContinuousReversalReducesTimeAndForwardTravel) {
+    for (const int ticks : {0, 80, 120}) {
+        SCOPED_TRACE(ticks);
+        const auto legacy = reverse(false, ticks);
+        const auto continuous = reverse(true, ticks);
+        EXPECT_GT(continuous.reverse_time, 0);
+        EXPECT_LT(continuous.reverse_time, legacy.reverse_time);
+        EXPECT_LT(continuous.return_time, legacy.return_time);
+        EXPECT_LT(continuous.peak, legacy.peak);
+    }
+}
+TEST(CartesianTwistReversal, ExactZeroCrossingIsStillMovingAndKeepsAcceleration) {
+    CartesianTwistLimiter limiter;
+    limiter.setLinearConstraints(1, 1, 4);
+    limiter.initialize(y(.02));
+    limiter.setTargetTwist(y(-.02), CartesianFrame::Base);
+    for (int i = 0; i < 100; ++i) limiter.update(dt, Eigen::Matrix3d::Identity());
+    EXPECT_NEAR(limiter.getTwistBase().y(), 0, 1e-12);
+    EXPECT_TRUE(limiter.isMoving());
+    EXPECT_LT(limiter.getAccelerationBase().y(), -.39);
+    EXPECT_LT(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.0003);
+}
+TEST(CartesianTwistReversal, StopFromEitherDirectionSettlesWithoutReversing) {
+    for (double v : {.08, -.08}) {
+        CartesianTwistLimiter limiter;
+        limiter.setLinearConstraints(1, 3, 10);
+        limiter.initialize(y(v));
+        limiter.stop();
+        for (int i = 0; i < 1000; ++i) {
+            EXPECT_GE(limiter.update(dt, Eigen::Matrix3d::Identity()).y() * v, -1e-12);
+        }
+        EXPECT_FALSE(limiter.isMoving());
+        EXPECT_NEAR(limiter.getTwistBase().norm(), 0, 1e-12);
+    }
+}
+TEST(CartesianTwistReversal, FeedbackPreservesNegativeVelocityOnLockedAxis) {
+    CartesianTwistLimiter limiter;
+    limiter.setLinearConstraints(1, 3, 10);
+    limiter.initialize(y(.08));
+    limiter.setTargetTwist(y(-.08), CartesianFrame::Base);
+    for (int i = 0; i < 400; ++i) limiter.update(dt, Eigen::Matrix3d::Identity());
+    for (int i = 0; i < 10; ++i) {
+        limiter.synchronize(y(-.08), dt, true);
+        EXPECT_NEAR(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.08, 1e-9);
+    }
+}
+TEST(CartesianTwistReversal, NonCollinearChangeStopsBeforeSwitchingAxis) {
+    CartesianTwistLimiter limiter;
+    limiter.setLinearConstraints(1, 3, 10);
+    limiter.initialize(y(.08));
+    Twist x = Twist::Zero(); x.x() = .08;
+    limiter.setTargetTwist(x, CartesianFrame::Base);
+    for (int i = 0; i < 1000; ++i) {
+        const auto v = limiter.update(dt, Eigen::Matrix3d::Identity());
+        EXPECT_FALSE(v.x() > 1e-9 && std::abs(v.y()) > 1e-9);
+        EXPECT_LE(limiter.getJerkBase().norm(), 10 + 1e-6);
+    }
+    EXPECT_NEAR(limiter.getTwistBase().x(), .08, 1e-9);
+}
+}
+}

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

@@ -2,6 +2,7 @@
 #define CMVR_ES_ARM_TYPES_H
 
 #include <cstdint>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -64,6 +65,24 @@ struct CartesianVelocity {
     double wz{0.0};
 };
 
+struct SpeedLOptions {
+    // Requested acceleration, capped separately by the arm's linear/angular maxima.
+    double acceleration{0.5};
+    // Requested linear jerk (m/s^3), capped by the arm's configured maximum.
+    // Unset uses that maximum.
+    std::optional<double> linear_jerk;
+    // 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};
+};
+
+struct SpeedLReference {
+    bool valid{false};
+    std::uint64_t command_version{0};
+    CartesianPose tcp_pose_base{};
+    CartesianVelocity target_base{};
+};
+
 struct CartesianWrench {
     double fx{0.0};
     double fy{0.0};

cmvr-es/config/devices/arm/arm.pb.txt

@@ -211,7 +211,6 @@ arm {
                 gain: 0.2
                 margin_ratio: 0.15
                 max_push: 0.25
-                weight: 0.05
               }
             }
           }

cmvr-es/config/devices/arm/arm_qp.pb.txt

@@ -109,9 +109,9 @@ arm {
 
       speed_l {
         pinocchio_cartesian_motion_planner {
-          linear_velocity_max: 0.55
+          linear_velocity_max: 0.8
-          linear_acceleration_max: 5.0
+          linear_acceleration_max: 10.0
-          linear_jerk_max: 10.0
+          linear_jerk_max: 30.0
           angular_velocity_max: 1.0
           angular_acceleration_max: 5.0
           angular_jerk_max: 12.0

cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt

@@ -96,6 +96,7 @@ touch_screen_task {
     speed_l {
       twist_tool { x: 0.0 y: -0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
       acceleration: 5.0
+      linear_jerk: 10.0
       max_distance_m: 0.03
     }
     tactile {
@@ -109,7 +110,8 @@ touch_screen_task {
 
   retract {
     twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 3.0
+    acceleration: 5.0
+    linear_jerk: 10.0  # m/s^3，约束接触后的减速和连续换向。
     # TCP 后退目标距离，单位为米。
     distance_m: 0.03
   }

cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt

@@ -105,6 +105,7 @@ touch_screen_task {
     speed_l {
       twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
       acceleration: 6.0
+      linear_jerk: 60.0  # m/s^3，接近阶段请求值，受机械臂 linear_jerk_max 限制。
       max_distance_m: 0.02
     }
     tactile {
@@ -119,6 +120,7 @@ touch_screen_task {
   retract {
     twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
     acceleration: 4.0
+    linear_jerk: 60.0  # m/s^3，保持仿真机械臂原有的 jerk 上限。
     # TCP 后退目标距离，单位为米。
     distance_m: 0.05
   }

cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt

@@ -19,6 +19,11 @@ target_link_libraries(motor_robot_arm
 add_library(cmvr_es::device::motor_robot_arm ALIAS motor_robot_arm)
 install(TARGETS motor_robot_arm LIBRARY DESTINATION lib)
 
+add_executable(speedl_reversal_mujoco_test src/speedl_reversal_mujoco_test.cpp)
+target_link_libraries(speedl_reversal_mujoco_test PRIVATE
+        cmvr_es::device::motor_robot_arm cmvr_es::device::motor_manager
+        cmvr_es::device::mujoco_motor_driver cmvr_es::proto pthread)
+
 add_executable(motor_robot_arm_mujoco_test
         src/motor_robot_arm_mujoco_test.cpp
 )

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

@@ -0,0 +1,77 @@
+# speedL 连续换向与 MuJoCo 验证
+
+同轴线速度换向使用固定轴上的有符号 S 曲线，从当前速度和加速度直接规划到反向目标。过零时保留加速度，不重置规划器。非同轴变向先停止再换轴；角速度仍使用原有策略。
+
+`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true；设为 false 可以复现旧的同轴停止后换向策略，用于同条件比较。
+
+`MotorRobotArm::speedL(velocity, SpeedLOptions, duration, frame)` 可以为本条命令指定 `linear_jerk`（m/s³）。未指定时使用机器人配置的 jerk；普通 `speedL(velocity, acceleration, duration, frame)` 仍可直接使用。控制线程将速度、加速度、jerk 作为同一命令读取。正常停止的收尾操作检查命令版本，避免清除之后提交的运动命令。
+
+机械臂的 `SpeedLPlannerConfig` 提供规划上限：速度保持按配置限幅；线加速度、角加速度分别取请求 `acceleration` 与各自配置上限的较小值；线性 jerk 取请求值与 `linear_jerk_max` 的较小值。省略 jerk 时使用配置上限。普通 `stopL` 的加速度也遵循此规则。未配置或无效的上限继续使用规划器默认值（线性 0.55/5/10，角向 1/5/12）。
+
+例如机械臂配置加速度 5、jerk 10 时，回撤请求 60/60 实际按 5/10 规划，请求 3/6 则按 3/6 规划。任务参数不能提高机械臂上限。
+
+接近阶段也可在 `touch.speed_l` 内配置 `linear_jerk`（m/s³），与 `retract.linear_jerk` 独立设置。两者都必须为有限正数，省略时使用机械臂上限；执行时都取请求值与上限的较小值。配置示例：
+
+```protobuf
+touch {
+  speed_l {
+    twist_tool { x: 0 y: -0.08 z: 0 rx: 0 ry: 0 rz: 0 }
+    acceleration: 5.0
+    linear_jerk: 10.0
+    max_distance_m: 0.03
+  }
+  # tactile 和 dwell_time_s 等其他必填项仍按任务配置填写。
+}
+```
+
+`capture_reference=true` 时，控制线程从第一拍关节状态计算 TCP 位置及 Base 下的目标方向，成功发送速度后发布 `SpeedLReference`；Tool 目标随后固定到这一 Base 方向。新命令提交时旧参考立即失效。尚不支持这些选项的机械臂后端明确返回不支持。
+
+触屏任务在 `dwell_time_s=0` 时检测到阈值就提交回撤，随后才记录日志，跳过停止/停留中间目标。回撤距离为沿回撤方向的有符号位移，继续前压不会算成回撤。`retract.linear_jerk` 控制整个减速、过零及反向加速过程，并受机械臂上限限制。实机触控任务当前接近和回撤都请求 10 m/s³；DLS 机械臂配置上限为 10，QP 配置上限为 30，MuJoCo 配置上限为 60 m/s³，执行时取所加载配置与任务请求的较小值。
+
+触屏任务的 `[RETRACT]`、`[RETRACT_DONE]` 日志包含 `max_forward_after_retract_mm`：以回撤首个控制周期锁存的实测 TCP 为起点，统计沿回撤反方向的最大正位移，单位毫米。每次回撤清零，后续后退不抵消已记录的峰值。统计复用任务每次回撤步骤的 FK 位置采样，不额外阻塞触觉触发后的命令提交。它是采样到的最大值，不包括触发到首个回撤控制周期之前的移动，也不包括非零 dwell 阶段的移动；短暂峰值可能落在两次任务采样之间。已取得回撤参考后发生失败时，`[RETRACT_FAILED]` 也会打印已记录的最大值。
+
+## 实际仿真测试
+
+测试创建 MuJoCo 世界、MuJoCo 电机组和 MotorRobotArm，不创建真实硬件或视觉任务。使用 `dual_arm.xml` 的位置执行器驱动动力学；没有通过直接修改关节状态模拟运动。
+
+先 MoveJ 到已有运动测试姿态，然后调用 `speedL(vy=+0.08)`，运动过程中直接调用 `speedL(vy=-0.08)`。默认 Base 坐标系，`--tool-frame true` 可验证 Tool 坐标系。测量来自 MuJoCo `R_FINGER_TIP_SITE` 的位置和雅可比乘实际 qvel；不是规划速度的积分。
+
+在仓库根目录运行（需要已配置 `cmake-build-debug`；可用 `CMVR_BUILD_DIR` 覆盖）：
+
+```bash
+# 匀速阶段换向：接近加速度 5，回撤加速度 3，jerk 均为 10。
+./script/test_speedl_reversal_mujoco.sh --csv /tmp/reversal-new.csv
+
+# 同一新版本中选择旧换向策略，比较算法本身。
+./script/test_speedl_reversal_mujoco.sh --legacy true --csv /tmp/reversal-legacy.csv
+
+# 仍在加速时，命令 vy 首次达到 0.032 m/s 即反向。
+./script/test_speedl_reversal_mujoco.sh --trigger-speed 0.032 --csv /tmp/reversal-accelerating.csv
+
+# 上限仍为 10，回撤请求 60 将被限制为 10。
+./script/test_speedl_reversal_mujoco.sh --reverse-jerk 60 --reverse-acceleration 60 \
+  --capture-reference true --csv /tmp/reversal-capped.csv
+
+# 将仿真机械臂 jerk 上限设为 60；接近请求 10，回撤请求 60。
+./script/test_speedl_reversal_mujoco.sh --jerk 60 --approach-jerk 10 \
+  --reverse-jerk 60 --capture-reference true \
+  --csv /tmp/reversal-jerk60.csv
+
+# Tool 坐标系和触屏任务使用的参数接口。
+./script/test_speedl_reversal_mujoco.sh --tool-frame true --jerk 60 --approach-jerk 10 --reverse-jerk 60 \
+  --capture-reference true --csv /tmp/reversal-tool.csv
+```
+
+默认前进 500 ms 后换向；`--approach-ms` 可修改，`--trigger-speed` 非零时优先按命令速度触发。默认速度为 0.08 m/s，可用 `--speed` 修改；`--jerk` 设置仿真机械臂 jerk 上限，`--approach-jerk` 和 `--reverse-jerk` 分别设置接近、回撤请求（省略时使用上限）。`--reverse-acceleration` 设置回撤加速度请求，默认 3，机械臂上限为 5。输出同时标注请求值和限幅值。测试采样目标周期和仿真步长均为 1 ms；线程由操作系统调度，CSV 同时记录 wall time 和 simulation time。
+
+`max_forward_mm` 是反向调用前采样点之后，实际 TCP 沿前进轴的最大正位移；`peak_at_ms` 是到达该最远点的时间；`actual_reverse_ms` 要求至少连续 5 次采样的轴向速度小于 -0.0001 m/s；`return_to_origin_ms` 是确认反向后返回调用时位置的时间。Tool 模式的 CSV 速度字段也表示沿锁定前进轴的投影。
+
+测试要求换向前实际速度为正、之后产生反向速度并退回起点，且控制器未提前退出；使用起点锁存时还要求参考有效。失败返回非零。该自由空间实验不包含屏幕接触、触觉延迟或硅胶形变，不能把返回起点时间直接当作屏幕抬起事件时间。
+
+## 回归测试
+
+构建并运行 `cartesian_twist_limiter_reversal_test`、`s_curve_velocity_planner_stop_test` 和 `cartesian_velocity_controller_test`。覆盖匀速/加速中换向、速度与 jerk 连续性、恰好过零、普通停止、非同轴转向、负速度反馈、停止/回撤竞争、成功发送后才发布回撤参考及非法参数拒绝。
+
+`pinocchio_speedl_limits_test` 使用真实 URDF 和生产规划器，采样输出速度并差分验证实际规划的速度、加速度、jerk 上限；覆盖超限请求、较小请求、省略 jerk、旧接口及停止、独立角加速度限幅和非法请求。测试不初始化电机。
+
+运行时应优先加载当前构建的项目动态库，测试脚本已处理；不要把新可执行文件与 `output/lib` 的旧项目库混用。

cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h

@@ -62,6 +62,9 @@ public:
                   double duration,
                   FrameType frame = FrameType::Base) override;
     Result stopL(std::optional<double> acceleration = std::nullopt) override;
+    Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options,
+                  double duration, FrameType frame = FrameType::Base) override;
+    SpeedLReference getSpeedLReference() const override;
     Result stopMotion() override;
 
     Result startServoMode(const ServoOptions& options) override;

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp

@@ -748,6 +748,21 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
     return cartesian_velocity_controller_->stop(acceleration);
 }
 
+Result MotorRobotArm::speedL(const CartesianVelocity& velocity, const SpeedLOptions& options,
+                            const double duration, const FrameType frame)
+{
+    if (const auto stopped = safetyStopResult_("speedL")) return *stopped;
+    if (busy_.load() || !cartesian_velocity_controller_) {
+        return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy or velocity controller is unavailable");
+    }
+    return cartesian_velocity_controller_->speedL(velocity, options, duration, frame);
+}
+
+SpeedLReference MotorRobotArm::getSpeedLReference() const
+{
+    return cartesian_velocity_controller_ ? cartesian_velocity_controller_->getReference() : SpeedLReference{};
+}
+
 Result MotorRobotArm::stopMotion()
 {
     const auto cartesian_stop = stopCartesianMotionAndWait_();

cmvr-es/devices/arm/motor_robot_arm/src/speedl_reversal_mujoco_test.cpp

@@ -0,0 +1,196 @@
+#include "arm/motor_robot_arm/include/motor_robot_arm.h"
+#include "common/io/proto_file_io.h"
+#include "devices/motor/manager/include/motor_manager.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <filesystem>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <stdexcept>
+#include <thread>
+
+// Headless, actuator-driven MuJoCo experiment. Never creates real motor drivers.
+namespace {
+using namespace cmvr;
+using namespace cmvr::device;
+using Clock = std::chrono::steady_clock;
+
+template<class T> T readConfig(const std::filesystem::path& path) {
+    T config;
+    if (!ProtoMessageIo::getProtoFromAsciiFile(path.string(), &config)) {
+        throw std::runtime_error("Cannot read " + path.string());
+    }
+    return config;
+}
+void require(const Result& result) {
+    if (!result.ok()) throw std::runtime_error(result.message);
+}
+struct Sample {
+    double sim_time;
+    Eigen::Vector3d position;
+    Eigen::Vector3d velocity;
+    Eigen::Matrix3d rotation;
+};
+Sample sample(const std::shared_ptr<simulate::MujocoWorld>& world, int site) {
+    std::lock_guard<std::mutex> lock(world->mutex());
+    const auto* model = world->model();
+    const auto* data = world->data();
+    std::vector<mjtNum> jac(3 * model->nv);
+    mj_jacSite(model, data, jac.data(), nullptr, site);
+    Sample s{data->time, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity()};
+    for (int axis = 0; axis < 3; ++axis) {
+        s.position[axis] = data->site_xpos[site * 3 + axis];
+        for (int j = 0; j < 3; ++j) s.rotation(axis, j) = data->site_xmat[site * 9 + axis * 3 + j];
+        for (int j = 0; j < model->nv; ++j) s.velocity[axis] += jac[axis * model->nv + j] * data->qvel[j];
+    }
+    return s;
+}
+}
+
+int main(int argc, char** argv) {
+    try {
+        const auto root = std::filesystem::current_path();
+        double approach_ms = 500.0, jerk = 10.0, speed = 0.08, reverse_jerk = 0.0, trigger_speed = 0.0;
+        double approach_jerk = 0.0, reverse_acceleration = 3.0;
+        bool legacy = false, capture = false, tool_frame = false;
+        std::string csv_path = "/tmp/speedl-reversal.csv";
+        for (int i = 1; i < argc; ++i) {
+            const std::string arg = argv[i];
+            if (++i >= argc) throw std::runtime_error("Missing value for " + arg);
+            if (arg == "--approach-ms") approach_ms = std::stod(argv[i]);
+            else if (arg == "--jerk") jerk = std::stod(argv[i]);
+            else if (arg == "--speed") speed = std::stod(argv[i]);
+            else if (arg == "--reverse-jerk") reverse_jerk = std::stod(argv[i]);
+            else if (arg == "--approach-jerk") approach_jerk = std::stod(argv[i]);
+            else if (arg == "--reverse-acceleration") reverse_acceleration = std::stod(argv[i]);
+            else if (arg == "--trigger-speed") trigger_speed = std::stod(argv[i]);
+            else if (arg == "--legacy") legacy = std::string(argv[i]) == "true";
+            else if (arg == "--capture-reference") capture = std::string(argv[i]) == "true";
+            else if (arg == "--tool-frame") tool_frame = std::string(argv[i]) == "true";
+            else if (arg == "--csv") csv_path = argv[i];
+            else throw std::runtime_error("Unknown option " + arg);
+        }
+        if (!(approach_ms > 0 && approach_ms <= 1000 && jerk > 0 && speed > 0 && speed <= .1 &&
+              reverse_jerk >= 0 && trigger_speed >= 0 && trigger_speed <= speed &&
+              approach_jerk >= 0 && reverse_acceleration > 0 && std::isfinite(jerk) &&
+              std::isfinite(reverse_jerk) && std::isfinite(approach_jerk) && std::isfinite(reverse_acceleration))) {
+            throw std::runtime_error("Invalid experiment parameters");
+        }
+        auto worlds = readConfig<config::MujocoWorldRootConfig>(root / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt");
+        auto wc = worlds.worlds(0);
+        wc.set_model_path((root / "model/xiaoyan_description/dual_arm.xml").string());
+        simulate::MujocoWorldDevice world_device(wc);
+        if (!world_device.init() || !world_device.start()) throw std::runtime_error("MuJoCo start failed");
+        auto motors = readConfig<config::MotorRootConfig>(root / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt");
+        auto manager = std::make_shared<MotorManager>("right_arm_mujoco_motors", motors.motor(), "right_arm_mujoco_motors");
+        manager->init();
+        const auto world = world_device.world();
+        const int site = mj_name2id(world->model(), mjOBJ_SITE, "R_FINGER_TIP_SITE");
+        if (site < 0) throw std::runtime_error("TCP site not found");
+        auto arms = readConfig<config::ArmRootConfig>(root / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt");
+        auto ac = arms.arm().robot_arms(0);
+        ac.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path(
+            (root / "model/xiaoyan_description/dual_arm.urdf").string());
+        auto* planner_config = ac.mutable_motion()->mutable_speed_l()->mutable_pinocchio_cartesian_motion_planner();
+        // Match physical-arm limits instead of the simulation's faster defaults.
+        planner_config->set_linear_velocity_max(.55);
+        planner_config->set_linear_acceleration_max(5.0);
+        planner_config->set_linear_jerk_max(jerk);
+        planner_config->set_continuous_linear_reversal(!legacy);
+        MotorRobotArm arm(ac);
+        if (!arm.init()) throw std::runtime_error("Simulated arm initialization failed");
+        MotionOptions move;
+        move.velocity = 1.0;
+        move.acceleration = 3.0;
+        require(arm.moveJ(JointPositionCommand{{.25, 1.0, M_PI/2, M_PI/2, -M_PI/2, 0, 0}}, move));
+        std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+        std::ofstream csv(csv_path);
+        if (!csv) throw std::runtime_error("Cannot open CSV");
+        csv << "wall_ms,sim_ms,after_reverse,x_m,y_m,z_m,vy_m_s,forward_displacement_m,command_vy_m_s\n" << std::setprecision(12);
+        CartesianVelocity forward;
+        forward.vy = speed;
+        const auto frame = tool_frame ? FrameType::Tool : FrameType::Base;
+        const Eigen::Vector3d axis = tool_frame ? Eigen::Vector3d(sample(world, site).rotation.col(1))
+                                                : Eigen::Vector3d::UnitY();
+        SpeedLOptions forward_options;
+        forward_options.acceleration = 5.0;
+        if (approach_jerk > 0) forward_options.linear_jerk = approach_jerk;
+        require(arm.speedL(forward, forward_options, 0.0, frame));
+        const auto begin = Clock::now();
+        auto next = begin;
+        auto reverse_time = begin;
+        Sample origin{};
+        bool reversed = false;
+        bool reference_valid = !capture;
+        double peak = 0, peak_ms = 0, return_ms = -1, reverse_ms = -1;
+        int negative_samples = 0;
+        while (Clock::now() - begin < std::chrono::milliseconds(static_cast<int>(approach_ms) + 1000)) {
+            auto now = Clock::now();
+            auto s = sample(world, site);
+            const auto command = arm.getSpeedLCommandTwistBase();
+            const double command_vy = Eigen::Vector3d(command.vx, command.vy, command.vz).dot(axis);
+            const double actual_vy = s.velocity.dot(axis);
+            const double wall_ms = std::chrono::duration<double, std::milli>(now - begin).count();
+            if (!reversed && (trigger_speed > 0 ? command_vy >= trigger_speed : wall_ms >= approach_ms)) {
+                origin = s;
+                reverse_time = Clock::now();
+                CartesianVelocity backward;
+                backward.vy = -speed;
+                SpeedLOptions options;
+                options.acceleration = reverse_acceleration;
+                options.capture_reference = capture;
+                if (reverse_jerk > 0) options.linear_jerk = reverse_jerk;
+                require(arm.speedL(backward, options, 0.0, frame));
+                reversed = true;
+            }
+            const double elapsed = std::chrono::duration<double, std::milli>(now - reverse_time).count();
+            const double displacement = reversed ? (s.position - origin.position).dot(axis) : 0.0;
+            if (reversed) {
+                if (capture) {
+                    const auto ref = arm.getSpeedLReference();
+                    reference_valid |= ref.valid && Eigen::Vector3d(ref.target_base.vx,
+                        ref.target_base.vy, ref.target_base.vz).dot(axis) < 0 &&
+                        std::isfinite(ref.tcp_pose_base.y) && ref.command_version != 0;
+                }
+                if (displacement > peak) { peak = displacement; peak_ms = elapsed; }
+                negative_samples = actual_vy < -1e-4 ? negative_samples + 1 : 0;
+                if (negative_samples >= 5 && reverse_ms < 0) reverse_ms = elapsed;
+                if (reverse_ms >= 0 && displacement <= 0 && return_ms < 0) return_ms = elapsed;
+            }
+            csv << wall_ms << ',' << s.sim_time * 1000 << ',' << reversed << ','
+                << s.position.x() << ',' << s.position.y() << ',' << s.position.z() << ','
+                << actual_vy << ',' << displacement << ',' << command_vy << '\n';
+            next += std::chrono::milliseconds(1);
+            std::this_thread::sleep_until(next);
+        }
+        const bool active = arm.busy();
+        require(arm.stopL(3.0));
+        const auto stop_deadline = Clock::now() + std::chrono::seconds(3);
+        while (arm.busy() && Clock::now() < stop_deadline) std::this_thread::sleep_for(std::chrono::milliseconds(5));
+        std::cout << std::fixed << std::setprecision(3)
+                  << "REVERSAL legacy=" << legacy << " approach_ms=" << approach_ms
+                  << " frame=" << (tool_frame ? "Tool" : "Base")
+                  << " trigger_speed=" << trigger_speed << " speed_m_s=" << speed << " jerk_m_s3=" << jerk
+                  << " approach_jerk_m_s3=" << std::min(approach_jerk > 0 ? approach_jerk : jerk, jerk)
+                  << " requested_reverse_acceleration_m_s2=" << reverse_acceleration
+                  << " reverse_acceleration_m_s2=" << std::min(reverse_acceleration, 5.0)
+                  << " requested_reverse_jerk_m_s3=" << (reverse_jerk > 0 ? reverse_jerk : jerk)
+                  << " reverse_jerk_m_s3=" << std::min(reverse_jerk > 0 ? reverse_jerk : jerk, jerk)
+                  << " actual_vy_at_reverse=" << origin.velocity.dot(axis)
+                  << " max_forward_mm=" << peak * 1000 << " peak_at_ms=" << peak_ms
+                  << " actual_reverse_ms=" << reverse_ms << " return_to_origin_ms=" << return_ms
+                  << " remained_active=" << active << " reference_valid=" << reference_valid << " csv=" << csv_path << '\n';
+        arm.stop();
+        manager->stop();
+        world_device.stop();
+        return active && reference_valid && reversed && origin.velocity.dot(axis) > .001 && reverse_ms > 0 && return_ms > 0 ? 0 : 1;
+    } catch (const std::exception& e) {
+        std::cerr << "Experiment failed: " << e.what() << '\n';
+        return 1;
+    }
+}

cmvr-es/devices/arm/robot_arm.h

@@ -58,6 +58,16 @@ public:
                           double duration,
                           FrameType frame = FrameType::Base) = 0;
     virtual Result stopL(std::optional<double> acceleration = std::nullopt) = 0;
+    virtual Result speedL(const CartesianVelocity& velocity,
+                          const SpeedLOptions& options,
+                          double duration,
+                          FrameType frame = FrameType::Base) {
+        if (options.linear_jerk || options.capture_reference) {
+            return Result::failure(ArmErrorCode::UnsupportedCommand, "speedL options are not supported by this arm");
+        }
+        return speedL(velocity, options.acceleration, duration, frame);
+    }
+    virtual SpeedLReference getSpeedLReference() const { return {}; }
     virtual Result stopMotion() = 0;
 
     virtual Result moveP(const CartesianPose& target,

cmvr-es/task/touch_screen_task/include/touch_screen_task.h

@@ -200,6 +200,10 @@ private:
     Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
     bool retract_start_position_valid_{false};
     Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()};
+    Eigen::Vector3d retract_direction_base_{Eigen::Vector3d::Zero()};
+    // Sampled maximum TCP displacement opposite to the retract direction,
+    // relative to the first applied retract command's measured TCP position.
+    double max_forward_after_retract_m_{0.0};
     bool have_last_T_B_G_{false};
     Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()};
     double max_T_B_G_translation_delta_m_{0.0};

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

@@ -988,6 +988,9 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
         !std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 ||
         !cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() ||
         !std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 ||
+        (retract.has_linear_jerk() &&
+         (!std::isfinite(retract.linear_jerk()) || retract.linear_jerk() <= 0.0)) ||
+        cmvr::common::math::toEigenVec6(retract.twist_tool()).head<3>().norm() <= 1e-9 ||
         !std::isfinite(retract.distance_m()) || retract.distance_m() <= 0.0) {
         return false;
     }
@@ -1058,6 +1061,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
             return twist.allFinite() &&
                    std::isfinite(speed_l.acceleration()) &&
                    speed_l.acceleration() > 0.0 &&
+                   (!speed_l.has_linear_jerk() ||
+                    (std::isfinite(speed_l.linear_jerk()) && speed_l.linear_jerk() > 0.0)) &&
                    std::isfinite(speed_l.max_distance_m()) &&
                    speed_l.max_distance_m() >= 0.0;
         }
@@ -1638,6 +1643,28 @@ bool TouchScreenTask::stepRetracting() {
     const auto now = Clock::now();
     const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
     const double retract_distance_m = config_.retract().distance_m();
+    if (!retract_start_position_valid_) {
+        const auto reference = arm_->getSpeedLReference();
+        if (!reference.valid) {
+            if (!arm_->busy() || elapsed > 1.0) {
+                enterFailed(Status::ROBOT_STATE_FAILED);
+                return false;
+            }
+            return true; // First controller tick has not captured the origin yet.
+        }
+        retract_start_position_base_ << reference.tcp_pose_base.x,
+                                       reference.tcp_pose_base.y,
+                                       reference.tcp_pose_base.z;
+        retract_direction_base_ << reference.target_base.vx,
+                                   reference.target_base.vy, reference.target_base.vz;
+        if (!retract_start_position_base_.allFinite() || !retract_direction_base_.allFinite() ||
+            retract_direction_base_.norm() <= 1e-9) {
+            enterFailed(Status::ROBOT_STATE_FAILED);
+            return false;
+        }
+        retract_direction_base_.normalize();
+        retract_start_position_valid_ = true;
+    }
     Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
     if (!retract_start_position_valid_ ||
         !readCurrentTouchPointPositionBase(current_position_base)) {
@@ -1651,14 +1678,23 @@ bool TouchScreenTask::stepRetracting() {
 
     const Eigen::Vector3d delta_base =
         current_position_base - retract_start_position_base_;
-    const double traveled_distance_m = delta_base.norm();
+    const double traveled_distance_m = delta_base.dot(retract_direction_base_);
+    // Keep the forward peak: subsequent backward travel must not cancel it.
+    // Reuse the existing measured-position sample, without delaying reversal.
+    max_forward_after_retract_m_ =
+        std::max(max_forward_after_retract_m_, -traveled_distance_m);
     if (traveled_distance_m < retract_distance_m) {
+        if (!arm_->busy()) {
+            enterFailed(Status::ROBOT_COMMAND_FAILED);
+            return false;
+        }
         if (std::chrono::duration<double>(now - last_retract_log_time_).count() >= 0.2) {
             last_retract_log_time_ = now;
             const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{};
             CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
                            << ", distance=" << traveled_distance_m
                            << "/" << retract_distance_m
+                           << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0
                            << ", cmd_base=[" << cmd_base.vx << ", "
                            << cmd_base.vy << ", " << cmd_base.vz << ", "
                            << cmd_base.wx << ", " << cmd_base.wy << ", "
@@ -1676,10 +1712,13 @@ bool TouchScreenTask::stepRetracting() {
     const bool have_final_delta = retract_start_position_valid_ && have_final_position;
     if (have_final_delta) {
         final_delta_base = final_position_base - retract_start_position_base_;
+        max_forward_after_retract_m_ = std::max(max_forward_after_retract_m_,
+                                                -final_delta_base.dot(retract_direction_base_));
     }
     if (have_final_delta) {
         CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
                        << ", target_distance_m=" << retract_distance_m
+                       << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0
                        << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
                        << ", final_tcp_delta_base=[" << final_delta_base.x() << ", "
                        << final_delta_base.y() << ", " << final_delta_base.z()
@@ -1687,6 +1726,7 @@ bool TouchScreenTask::stepRetracting() {
     } else {
         CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
                        << ", target_distance_m=" << retract_distance_m
+                       << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0
                        << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
                        << ", final_tcp_delta_base=unavailable";
     }
@@ -1858,6 +1898,12 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
 }
 
 bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
+    if (config_.touch().dwell_time_s() <= 0.0) {
+        // Submit reversal in this tick, before any synchronous FK or logging.
+        if (!startRetractPhase(Phase::DONE, Status::DONE)) return false;
+        logTouchPressure(true);
+        return true;
+    }
     if (stop_forward_motion) {
         const auto result = arm_->stopL();
         if (!result.ok()) {
@@ -1899,9 +1945,12 @@ bool TouchScreenTask::startTouchPhase() {
             last_status_ = Status::ROBOT_STATE_FAILED;
             return false;
         }
+        device::SpeedLOptions options;
+        options.acceleration = speed_l.acceleration();
+        if (speed_l.has_linear_jerk()) options.linear_jerk = speed_l.linear_jerk();
         const auto result = arm_->speedL(toCartesianVelocity(
                                              cmvr::common::math::toEigenVec6(speed_l.twist_tool())),
-                                         speed_l.acceleration(),
+                                         options,
                                          0.0,
                                          device::FrameType::Tool);
         if (!result.ok()) {
@@ -1957,24 +2006,17 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
     }
 
     const auto& retract = config_.retract();
-    retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_);
+    retract_start_position_valid_ = false;
-    if (!retract_start_position_valid_) {
+    retract_direction_base_.setZero();
-        CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] cannot read TCP start pose";
+    max_forward_after_retract_m_ = 0.0;
-        return false;
-    }
     const auto retract_cmd = toCartesianVelocity(
         cmvr::common::math::toEigenVec6(retract.twist_tool()));
-    CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
+    device::SpeedLOptions options;
-                   << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
+    options.acceleration = retract.acceleration();
-                   << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
+    if (retract.has_linear_jerk()) options.linear_jerk = retract.linear_jerk();
-                   << "], acceleration=" << retract.acceleration()
+    options.capture_reference = true;
-                   << ", distance_m=" << retract.distance_m()
-                   << ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
-                   << retract_start_position_base_.y() << ", "
-                   << retract_start_position_base_.z() << "]";
-
     const auto result = arm_->speedL(retract_cmd,
-                                     retract.acceleration(),
+                                     options,
                                      0.0,
                                      device::FrameType::Tool);
     if (!result.ok()) {
@@ -1990,11 +2032,20 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
     last_retract_log_time_ = phase_start_time_;
     retract_command_started_ = true;
     last_status_ = Status::RETRACTING;
+    CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] submitted twist_tool=["
+                   << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
+                   << "], acceleration=" << retract.acceleration()
+                   << ", linear_jerk=" << (retract.has_linear_jerk() ? retract.linear_jerk() : 0.0)
+                   << ", distance_m=" << retract.distance_m();
     return true;
 }
 
 void TouchScreenTask::enterFailed(const Status status) {
     stopPbvsMotion();
+    if (phase_ == Phase::RETRACTING && retract_start_position_valid_) {
+        CMVR_LOG(WARNING) << "[TouchScreenTask][RETRACT_FAILED] status=" << statusToString(status)
+                          << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0;
+    }
     phase_ = Phase::FAILED;
     setCoordinateOverlayEnabled(true);
     touch_command_started_ = false;

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

@@ -1,4 +1,5 @@
 #include "gtest/gtest.h"
+#include <google/protobuf/text_format.h>
 
 #include <algorithm>
 #include <atomic>
@@ -306,6 +307,45 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
     }
 }
 
+TEST(TouchScreenTaskTest, TouchSpeedLJerkParsesFromTextConfig) {
+    cmvr::config::TouchScreenTaskRootConfig parsed;
+    ASSERT_TRUE(google::protobuf::TextFormat::ParseFromString(
+        "touch_screen_task { touch { speed_l { acceleration: 5 linear_jerk: 10 } } }", &parsed));
+    ASSERT_TRUE(parsed.touch_screen_task().touch().speed_l().has_linear_jerk());
+    EXPECT_DOUBLE_EQ(parsed.touch_screen_task().touch().speed_l().linear_jerk(), 10);
+
+    const auto project_root = findProjectRoot();
+    ASSERT_FALSE(project_root.empty());
+    for (const auto* file_name : {"touch_screen_task.pb.txt", "touch_screen_task_mujoco.pb.txt"}) {
+        cmvr::config::TouchScreenTaskRootConfig root;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root / "cmvr-es/config/tasks/touch_screen_task" / file_name).string(), &root));
+        cmvr::task::TouchScreenTask task(root.touch_screen_task());
+        EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED)
+            << file_name;
+    }
+}
+
+TEST(TouchScreenTaskTest, TouchSpeedLJerkIsOptionalAndMustBeFinitePositive) {
+    auto config = loadMujocoTouchConfig(findProjectRoot());
+    config.mutable_touch()->mutable_speed_l()->clear_linear_jerk();
+    {
+        cmvr::task::TouchScreenTask task(config);
+        EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED);
+    }
+    for (const double jerk : {.5, 10.0, 60.0}) {
+        config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk);
+        cmvr::task::TouchScreenTask task(config);
+        EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED);
+    }
+    for (const double jerk : {0.0, -1.0, std::numeric_limits<double>::infinity(),
+                             std::numeric_limits<double>::quiet_NaN()}) {
+        config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk);
+        cmvr::task::TouchScreenTask task(config);
+        EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::INVALID_CONFIG);
+    }
+}
+
 TEST(TouchScreenTaskTest, CoordinateFrameProjectionUsesBgrAxisColors) {
     cv::Mat image = cv::Mat::zeros(240, 320, CV_8UC3);
     cmvr::device::Rs2Intrinsics intrinsics{};

protos/cmvr/config/arm_config/arm_config.proto

@@ -46,6 +46,11 @@ message VendorRobotArmBackendConfig {
 }
 
 message SpeedLPlannerConfig {
+  // Default true: preserve acceleration through same-axis velocity reversal.
+  optional bool continuous_linear_reversal = 21;
+  // Arm-level speedL limits. Command requests may lower, but not raise them.
+  // Linear units: m/s, m/s^2, m/s^3. Angular units: rad/s, rad/s^2, rad/s^3.
+  // Unset/nonpositive values use the planner defaults.
   double linear_velocity_max = 1;
   double linear_acceleration_max = 2;
   double linear_jerk_max = 3;

protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto

@@ -97,6 +97,9 @@ message TouchScreenTouchSpeedLConfig {
   .cmvr.common.Vec6 twist_tool = 1;
   optional double acceleration = 2;
   optional double max_distance_m = 3;
+  // Requested linear jerk during approach, m/s^3, capped by the arm's
+  // linear_jerk_max. Unset uses that arm limit.
+  optional double linear_jerk = 4;
 }
 
 message TouchScreenTouchMoveLConfig {
@@ -129,11 +132,15 @@ message TouchScreenTaskTouchConfig {
 
 message TouchScreenTaskRetractConfig {
   .cmvr.common.Vec6 twist_tool = 1;
+  // Requested acceleration, capped by the arm's speedL acceleration limits.
   optional double acceleration = 2;
   reserved 3;
   reserved "duration_s";
-  // Distance traveled by the TCP before the retract motion stops, in meters.
+  // Signed displacement along the retract direction before stopping, in meters.
   optional double distance_m = 4;
+  // Requested linear jerk for braking and reversal, m/s^3, capped by the arm's
+  // linear_jerk_max. Unset uses that arm limit.
+  optional double linear_jerk = 5;
 }
 
 message TouchScreenTaskConfig {

script/test_speedl_reversal_mujoco.sh

@@ -0,0 +1,22 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+# Headless MuJoCo only. Never initializes the physical robot.
+repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
+build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}"
+cmake --build "$build_dir" --target speedl_reversal_mujoco_test -j 4
+cd "$repo_root"
+python3 - "$build_dir" "$@" <<'PY'
+from pathlib import Path
+import os
+import sys
+
+build = Path(sys.argv[1]).resolve()
+# Prefer every freshly built project library over output/lib's installed copy.
+paths = sorted({str(path.parent) for path in build.rglob('*.so')})
+paths.append(str(Path('output/lib').resolve()))
+env = os.environ.copy()
+env['LD_LIBRARY_PATH'] = ':'.join(paths + [env.get('LD_LIBRARY_PATH', '')])
+binary = build / 'cmvr-es/devices/arm/motor_robot_arm/speedl_reversal_mujoco_test'
+os.execve(str(binary), [str(binary), *sys.argv[2:]], env)
+PY