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_) {
-        return {};
+    std::lock_guard<std::mutex> lock(mutex_);
+    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);
+                }
                 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,10 +342,10 @@ void CartesianVelocityController::abortCommand_()
         command_active_ = false;
         target_twist_ = {};
         target_frame_ = FrameType::Base;
-    }
         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_,
-        speedl_config_,
-        acceleration);
+    twist_limiter_.setLinearConstraints(
+        positiveOr(speedl_config_.linear_velocity_max(), .55),
+        acceleration, jerk);
+    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_acceleration_max: 5.0
-          linear_jerk_max: 10.0
+          linear_velocity_max: 0.8
+          linear_acceleration_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/devices/dexhand/dexhand.pb.txt

@@ -28,6 +28,8 @@ dexhand {
       resultant_length: 3
       poll_interval_ms: 5
       response_timeout_ms: 200
+      # 触觉数据最大有效期；超过此时间报不可用，不继续返回旧力值。
+      max_sample_age_ms: 50
       response_header_bytes: 14
       tactile_rows: 1
       tactile_cols: 51

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

@@ -96,20 +96,22 @@ 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 {
       finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
       region: TOUCH_SCREEN_TACTILE_REGION_TIP
       criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
-      force_threshold: 1.0
+      force_threshold: 0.1  # N
     }
     dwell_time_s: 0.0
   }
 
   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,13 +105,14 @@ 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 {
       finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
       region: TOUCH_SCREEN_TACTILE_REGION_TIP
       criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
-      force_threshold: 1.0
+      force_threshold: 0.1  # N
     }
     dwell_time_s: 0.0
   }
@@ -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/devices/dexhand/abstract_dexhand.h

@@ -9,6 +9,7 @@
 #include <cmath>
 #include <cstdint>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 #include <vector>
@@ -43,6 +44,17 @@ namespace cmvr::device {
 
         using ResultantForce = TactilePoint;
 
+        // Physical force, separate from device-specific integer tactile data.
+        struct ForceNewtons {
+            double fx{0.0};
+            double fy{0.0};
+            double fz{0.0};
+
+            double magnitude() const {
+                return std::hypot(fx, fy, fz);
+            }
+        };
+
         enum class FingerType {
             PINKY,
             RING,
@@ -147,6 +159,11 @@ namespace cmvr::device {
         virtual std::vector<TactileRegionData> getSensorData() = 0;
         virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0;
         virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0;
+        // Backends must provide a documented conversion; raw pressure counts
+        // cannot be assumed to represent newtons.
+        virtual ForceNewtons getResultantForceNewtons(FingerType, TactileRegion) {
+            throw std::runtime_error(typeName() + " does not provide force in newtons.");
+        }
 
         virtual void setPositions(const std::vector<int>&) {
             CMVR_LOG(ERROR) << "[AbstractDexHand] setPositions is not supported by this dexhand abstraction.";

cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt

@@ -12,3 +12,12 @@ target_link_libraries(px_6ax_gen3
 )
 
 install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib)
+
+# Sensor-only executable: no DeviceManager, arm initialization or calibration.
+add_executable(px_6ax_gen3_real_test src/px_6ax_gen3_real_test.cpp)
+target_link_libraries(px_6ax_gen3_real_test PRIVATE
+        px_6ax_gen3 cmvr_es::proto cmvr_es::logging pthread)
+
+add_executable(px_6ax_gen3_test src/px_6ax_gen3_test.cpp)
+target_link_libraries(px_6ax_gen3_test PRIVATE
+        px_6ax_gen3 cmvr_es::proto cmvr_es::logging gtest gtest_main pthread)

cmvr-es/devices/dexhand/px_6ax_gen3/README.md

@@ -0,0 +1,51 @@
+# PX-6AX GEN3 传感器读取与真实 USB 测试
+
+真实测试只创建 PX6AXGen3 和 POSIX 串口，不初始化机械臂或 DeviceManager；禁用自动标定，传输层只允许 `0xFB` 读取命令。
+
+在仓库根目录运行：
+
+```bash
+# 每次读取都发起一个真实请求：检查串口应答耗时和按压力值。
+./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \
+  --mode sync --duration-s 30 --csv /tmp/paxini-sync.csv --raw-log /tmp/paxini-frames.log
+
+# 与触屏任务相同：后台轮询，前台读取最新有效缓存。
+./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \
+  --mode stream --duration-s 30 --csv /tmp/paxini-stream.csv
+```
+
+也可把 `--port` 指定为 `/dev/serial/by-id/` 下的稳定设备链接。设备端口取决于连接顺序；测试参数不会更改机器人部署配置里的串口。`module-id=2` 对应协议设备地址 3。
+
+脚本会构建真实测试并优先加载本次构建的驱动和 protobuf。默认构建目录为 `cmake-build-debug`，可用 `CMVR_BUILD_DIR` 覆盖；构建目录须已完成 CMake 配置。串口应可读写，运行测试前退出占用同一串口的程序。
+
+测试期间可用手轻按、松开传感器，终端每 100 ms 显示一次力值，CSV 记录每次 getter 调用。按 Ctrl-C 可结束。CSV 的无效读数留空，不能当作零力。进程在初始化失败、没有有效数据或存在读取失败时返回非零退出码。
+
+- `sync` 的 `read_ms` 是一次驱动请求/应答调用的耗时；默认每 5 ms 请求一次。
+- `stream` 的 `read_ms` 是读取缓存耗时，成功次数包含重复快照，不能据此推算传感器实际更新频率。
+- `force_N`、`fz_N` 和 CSV 中的三个力分量均由驱动直接返回，单位为 N；例如原始值 1 对应 0.1 N、108 对应 10.8 N。传感器标称输出频率 83.3 Hz；请求频率可以高于内部测量更新频率。
+- 串口往返时间不包含“物理接触到传感器产生非零输出”的全部时间。需要按压试验或外部同步信号才能测量接触检测延迟。
+- `--raw-log` 会记录原始 TX/RX 和单调时钟时间，用于对照手册定位帧问题；写日志会给时间测量带来少量开销。
+
+## 驱动行为
+
+`getResultantForceNewtons()` 将合力寄存器的三个原始分量各乘以 0.1，返回以 N 为单位的浮点力值；原有 `getResultantForce()` 保留原始整数。触屏任务和 USB 测试使用牛顿接口，不再额外换算。触屏任务的 `force_threshold`、力值日志和 `lastTouchPressureSum()` 均使用 N；阈值 `0.1` 与旧版原始值阈值 `1.0` 对应相同力度。FZ 判据比较法向力，MAGNITUDE 判据比较三轴合力大小，多区域按原逻辑累加。
+
+MuJoCo 零值触觉后端也实现牛顿接口。尚无确定换算系数的其他后端（如 RH56DFTP）调用该接口会明确报错，不会把原始压力计数当成 N。
+
+合力应答按 `14 字节头部 + 3 字节数据 + 1 字节 LRC` 完整读取。验证帧长度、设备地址、预留位、功能码、寄存器地址、字节数及 LRC，并支持分片、请求回显和噪声后的重新定位。
+
+读状态字节属于内部调试信息（手册 5.3.5）；实测正常读应答为 `0x01`，不能套用写应答 `0x00=成功` 的规则。自动标定的写应答要求完整 15 字节并且状态为 0。
+
+`max_sample_age_ms` 默认 50 ms，可在设备配置中调整。每类数据分别记录请求开始时间：较晚返回的旧请求不能被重新标记为新数据。此值独立于 `response_timeout_ms`（默认 200 ms）。后台读取接口遇到过期或失效快照会抛出异常，不等待串口补读、不返回旧力值或伪造零值；触屏任务现有的异常捕获会将其识别为触觉不可用。
+
+通信失败会使快照失效，后台继续尝试恢复，只有通过验证的新应答才能恢复有效数据。`stop()` 清除缓存；停止或故障状态下的 getter 报错，显式 `init()` / `start()` 后才能恢复。初始化重试失败会返回 false。
+
+## 自动回归测试
+
+真实设备无法稳定制造的坏帧和超时，用可注入的串口实现验证：
+
+```bash
+cmake --build cmake-build-debug --target px_6ax_gen3_test -j 4
+LD_LIBRARY_PATH="$PWD/cmake-build-debug:$PWD/cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3:$PWD/cmake-build-debug/cmvr-es/hardware:$PWD/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \
+  ./cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3/px_6ax_gen3_test
+```

cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h

@@ -39,6 +39,8 @@ namespace cmvr::device {
         };
 
         explicit PX6AXGen3(const config::PX6AXGen3& cfg);
+        PX6AXGen3(const config::PX6AXGen3& cfg,
+                  std::unique_ptr<::cmvr::AbstractSerialTransport> serial);
         ~PX6AXGen3() override;
 
         std::string typeName() const override { return "PX6AXGen3"; }
@@ -54,7 +56,10 @@ namespace cmvr::device {
         void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
         std::vector<TactileRegionData> getSensorData() override;
         TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
+        // Throws when no fresh, validated sample is available. A failed read is
+        // never represented as a zero force (which would mean no contact).
         ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
+        ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override;
 
     private:
         struct SensorSnapshot {
@@ -64,6 +69,8 @@ namespace cmvr::device {
             int cols{0};
             bool tactile_valid{false};
             bool resultant_valid{false};
+            std::chrono::steady_clock::time_point tactile_request_time{};
+            std::chrono::steady_clock::time_point resultant_request_time{};
         };
 
         static PollingReadMode parsePollingReadMode(config::PX6AXGen3PollingReadMode mode);
@@ -81,17 +88,19 @@ namespace cmvr::device {
 
         bool isSupportedRegion(FingerType finger, TactileRegion region) const;
         bool isSnapshotReady(bool require_tactile, bool require_resultant) const;
+        bool isSampleFresh(std::chrono::steady_clock::time_point request_time) const;
         TactileRegionData buildSupportedRegionSnapshot() const;
         std::pair<bool, bool> resolvePollingReadSelection() const;
 
         void clearOperationalError();
-        void handleRefreshFailure(const std::string& error, bool had_valid_snapshot);
+        void handleRefreshFailure(const std::string& error);
         void transitionTo(Status next_state);
         void enterFault(const std::string& error);
         bool isOperationalState(Status lifecycle) const;
 
         std::unique_ptr<::cmvr::AbstractSerialTransport> serial_;
         config::PX6AXGen3 config_;
+        bool config_valid_{false};
 
         mutable std::mutex lifecycle_mutex_;
         Status lifecycle_state_{Status::CREATED};
@@ -109,6 +118,7 @@ namespace cmvr::device {
         int tactile_rows_{1};
         int tactile_cols_{0};
         int response_timeout_ms_{200};
+        std::chrono::milliseconds max_sample_age_{50};
         FingerType tactile_finger_{FingerType::INDEX};
         TactileRegion tactile_region_{TactileRegion::TIP};
         PollingReadMode polling_read_mode_{PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE};

cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp

@@ -125,32 +125,13 @@ namespace {
         return frame;
     }
 
-    std::vector<uint8_t> extractPayload(const std::vector<uint8_t>& response,
-                                        const size_t frame_offset,
-                                        const size_t response_header_bytes,
-                                        const size_t payload_length) {
-        if (response.size() < frame_offset + response_header_bytes + payload_length) {
-            CMVR_LOG(ERROR) << "PX-6AX GEN3 response shorter than expected payload window.";
-            return {};
-        }
-        return std::vector<uint8_t>(
-            response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes),
-            response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes + payload_length));
-    }
+    // UART read reply: 14-byte header, N payload bytes, one LRC byte.
+    constexpr size_t kResponseHeaderBytes = 14U;
+    constexpr size_t kResponseOverheadBytes = kResponseHeaderBytes + 1U;
 
-    size_t findResponseFrameOffset(const std::vector<uint8_t>& response,
-                                   const size_t expected_frame_bytes) {
-        if (response.size() < expected_frame_bytes) {
-            return std::string::npos;
-        }
-
-        for (size_t offset = 0; offset + expected_frame_bytes <= response.size(); ++offset) {
-            if (response[offset] == 0xAA && response[offset + 1] == 0x55) {
-                return offset;
-            }
-        }
-
-        return std::string::npos;
+    uint16_t readLe16(const std::vector<uint8_t>& bytes, const size_t offset) {
+        return static_cast<uint16_t>(bytes[offset]) |
+               static_cast<uint16_t>(static_cast<uint16_t>(bytes[offset + 1U]) << 8U);
     }
 
     std::string previewBytesHex(const std::vector<uint8_t>& data, const size_t max_bytes = 32U) {
@@ -171,39 +152,83 @@ namespace {
         return stream.str();
     }
 
-    std::vector<uint8_t> readFramedResponse(cmvr::AbstractSerialTransport& serial,
-                                            const size_t expected_frame_bytes,
-                                            const size_t max_prefix_bytes,
-                                            const std::chrono::milliseconds timeout,
-                                            const std::string& response_name) {
+    std::vector<uint8_t> transact(cmvr::AbstractSerialTransport& serial,
+                                  const std::vector<uint8_t>& request,
+                                  const size_t payload_length,
+                                  const std::chrono::milliseconds timeout) {
+        if (!serial.flushInput()) {
+            throw std::runtime_error("Failed to flush PX6AXGen3 input: " + serial.lastError());
+        }
+        if (!serial.write(request)) {
+            throw std::runtime_error("Failed to send PX6AXGen3 request: " + serial.lastError());
+        }
+
+        const size_t expected_size = kResponseOverheadBytes + payload_length;
+        // Permit a request echo/noisy prefix, but bound resynchronization work.
+        const size_t max_response_bytes = expected_size + 4096U;
         const auto deadline = std::chrono::steady_clock::now() + timeout;
         std::vector<uint8_t> response;
-        const bool read_ok = serial.read(expected_frame_bytes, timeout, response);
-        auto frame_offset = findResponseFrameOffset(response, expected_frame_bytes);
-
-        while (frame_offset == std::string::npos &&
-               response.size() < expected_frame_bytes + max_prefix_bytes) {
+        size_t offset = 0;
+        std::string error = "Incomplete PX6AXGen3 response";
+        while (response.size() < max_response_bytes) {
             const auto now = std::chrono::steady_clock::now();
             if (now >= deadline) {
                 break;
             }
-
-            std::vector<uint8_t> extra_bytes;
-            const auto remaining_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now);
-            if (!serial.read(1U, remaining_timeout, extra_bytes)) {
+            std::vector<uint8_t> chunk;
+            const auto remaining = std::max(std::chrono::milliseconds(1),
+                std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now));
+            // Read available bytes; never wait for a guessed length before
+            // identifying the header. This also handles split headers/echoes.
+            const bool read_ok = serial.read(1U, remaining, chunk);
+            if (chunk.size() > max_response_bytes - response.size()) {
+                throw std::runtime_error("PX6AXGen3 response exceeds resynchronization limit");
+            }
+            response.insert(response.end(), chunk.begin(), chunk.end());
+            while (offset + kResponseHeaderBytes <= response.size()) {
+                if (response[offset] != 0xAA || response[offset + 1U] != 0x55) {
+                    ++offset;
+                    continue;
+                }
+                // Frame length counts data[4] through data[N+13], excluding LRC.
+                if (readLe16(response, offset + 2U) != payload_length + 10U ||
+                    response[offset + 4U] != request[4] ||
+                    response[offset + 5U] != 0x00 ||
+                    !std::equal(request.begin() + 6, request.begin() + 13,
+                                response.begin() + static_cast<std::ptrdiff_t>(offset + 6U))) {
+                    error = "PX6AXGen3 response does not match request (length/address/function)";
+                    ++offset;
+                    continue;
+                }
+                if (response.size() - offset < expected_size) {
                     break;
                 }
-
-            response.insert(response.end(), extra_bytes.begin(), extra_bytes.end());
-            frame_offset = findResponseFrameOffset(response, expected_frame_bytes);
+                uint8_t sum = 0;
+                for (size_t i = offset; i < offset + expected_size; ++i) {
+                    sum = static_cast<uint8_t>(sum + response[i]);
                 }
-
-        if (!read_ok && frame_offset == std::string::npos) {
-            CMVR_LOG(ERROR) << "Failed to read " << response_name << ": " << serial.lastError()
-                            << ", raw=" << previewBytesHex(response);
+                if (sum != 0U) {
+                    error = "Invalid PX6AXGen3 response LRC";
+                    ++offset;
+                    continue;
                 }
-
-        return response;
+                // Manual 5.3.5: read status is internal/debug information;
+                // hardware returns 0x01 on normal force reads. Only write ACKs
+                // define 0x00 as success (5.4.2), so do not apply it to reads.
+                if (request[6] == 0x79 && response[offset + 13U] != 0x00) {
+                    throw std::runtime_error("PX6AXGen3 returned status " +
+                                             std::to_string(response[offset + 13U]));
+                }
+                return std::vector<uint8_t>(
+                    response.begin() + static_cast<std::ptrdiff_t>(offset + kResponseHeaderBytes),
+                    response.begin() + static_cast<std::ptrdiff_t>(offset + kResponseHeaderBytes + payload_length));
+            }
+            if (!read_ok || chunk.empty()) {
+                break;
+            }
+        }
+        throw std::runtime_error(error + "; " + serial.lastError() +
+                                 ", raw=" + previewBytesHex(response));
     }
 
     std::array<int, 3> parseResultantPayload(const std::vector<uint8_t>& payload) {
@@ -303,21 +328,28 @@ PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadModeName(std::string value
 }
 
 PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
-    : serial_(std::make_unique<::cmvr::PosixSerialTransport>()),
-      config_(cfg) {
+    : PX6AXGen3(cfg, std::make_unique<::cmvr::PosixSerialTransport>()) {}
+
+PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg,
+                     std::unique_ptr<::cmvr::AbstractSerialTransport> serial)
+    : serial_(std::move(serial)), config_(cfg) {
     id_ = config_.id();
     port_name_ = config_.serial_port();
     if (!config_.sensor_model().empty()) {
         sensor_model_ = config_.sensor_model();
     }
-    module_id_ = std::max(0, config_.module_id());
+    module_id_ = config_.module_id();
+    if (!serial_ || module_id_ < 0 || module_id_ > 254) {
+        enterFault("PX6AXGen3 requires a serial transport and module_id in [0, 254].");
+        return;
+    }
     device_address_ = module_id_ + 1;
     if (config_.baud_rate() > 0) {
         baud_rate_ = config_.baud_rate();
     }
     distributed_length_ = config_.distributed_length();
-    if (distributed_length_ <= 0) {
-        enterFault("PX6AXGen3 requires config.distributed_length to be explicitly configured.");
+    if (distributed_length_ <= 0 || distributed_length_ > 65525 || distributed_length_ % 3 != 0) {
+        enterFault("PX6AXGen3 distributed_length must be a positive multiple of 3 fitting the UART frame.");
         return;
     }
     if (config_.resultant_length() > 0) {
@@ -326,6 +358,15 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
     if (config_.response_header_bytes() > 0) {
         response_header_bytes_ = config_.response_header_bytes();
     }
+    if (resultant_length_ != 3 || response_header_bytes_ != kResponseHeaderBytes ||
+        config_.resultant_length() < 0 || config_.response_header_bytes() < 0 ||
+        config_.max_sample_age_ms() < 0) {
+        enterFault("PX6AXGen3 requires resultant_length=3, response_header_bytes=14 and a nonnegative max_sample_age_ms.");
+        return;
+    }
+    if (config_.max_sample_age_ms() > 0) {
+        max_sample_age_ = std::chrono::milliseconds(config_.max_sample_age_ms());
+    }
     if (config_.tactile_rows() > 0) {
         tactile_rows_ = config_.tactile_rows();
     }
@@ -350,6 +391,7 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
         poll_interval_ = std::chrono::milliseconds(config_.poll_interval_ms());
     }
     initializeSnapshot();
+    config_valid_ = true;
 }
 
 PX6AXGen3::~PX6AXGen3() {
@@ -357,15 +399,15 @@ PX6AXGen3::~PX6AXGen3() {
 }
 
 bool PX6AXGen3::init() {
-    try {
-        ensureConnected();
-        if (!isOperationalState(state())) {
+    if (!config_valid_) {
         return false;
     }
+    try {
         refreshSensorDataWithRetry(
             5,
             std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
-        return isOperationalState(state());
+        const auto [tactile, resultant] = resolvePollingReadSelection();
+        return isOperationalState(state()) && isSnapshotReady(tactile, resultant);
     } catch (const std::exception& e) {
         enterFault("[PX6AXGen3](init): " + std::string(e.what()));
         return false;
@@ -373,8 +415,10 @@ bool PX6AXGen3::init() {
 }
 
 bool PX6AXGen3::start() {
+    if (!config_valid_) {
+        return false;
+    }
     if (polling_thread_running_.exchange(true, std::memory_order_acq_rel)) {
-        transitionTo(Status::STREAMING);
         return true;
     }
 
@@ -383,19 +427,22 @@ bool PX6AXGen3::start() {
             polling_thread_.join();
         }
 
-        ensureConnected();
-        if (!isOperationalState(state())) {
-            polling_thread_running_.store(false, std::memory_order_release);
-            return false;
+        bool requested_polling;
+        {
+            std::lock_guard<std::mutex> lock(polling_mutex_);
+            requested_polling = requested_polling_;
         }
-        if (requested_polling_) {
+        if (requested_polling) {
             refreshSensorDataWithRetry(
                 5,
                 std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
+        } else {
+            std::lock_guard<std::mutex> lock(refresh_mutex_);
+            ensureConnected();
         }
 
-        polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this);
         transitionTo(Status::STREAMING);
+        polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this);
         polling_cv_.notify_all();
         return true;
     } catch (const std::exception& e) {
@@ -413,6 +460,8 @@ bool PX6AXGen3::stop() {
         polling_thread_.join();
     }
 
+    std::lock_guard<std::mutex> lock(refresh_mutex_);
+    initializeSnapshot();
     closeConnection();
 
     if (state() != Status::FAULT) {
@@ -434,10 +483,13 @@ std::string PX6AXGen3::lastError() const {
 void PX6AXGen3::getState(DexHandState& state_out) {
     DexHandState next_state{};
     next_state.is_initialized = isOperationalState(state());
+    const auto [tactile, resultant] = resolvePollingReadSelection();
+    const bool ready = isSnapshotReady(tactile, resultant);
+    next_state.is_initialized = next_state.is_initialized && ready;
 
     {
         std::lock_guard<std::mutex> lock(snapshot_mutex_);
-        if (latest_snapshot_.resultant_valid) {
+        if (latest_snapshot_.resultant_valid && isSampleFresh(latest_snapshot_.resultant_request_time)) {
             next_state.hands[0].force = latest_snapshot_.resultant_force_tenths[2];
         }
     }
@@ -445,6 +497,8 @@ void PX6AXGen3::getState(DexHandState& state_out) {
     const auto error = lastError();
     if (!error.empty()) {
         next_state.hands[0].error_message.push_back(error);
+    } else if (!ready) {
+        next_state.hands[0].error_message.push_back("PX6AXGen3 sample is unavailable or stale.");
     }
 
     state_out = std::move(next_state);
@@ -468,7 +522,8 @@ void PX6AXGen3::setTactilePollingRegions(const std::vector<TactileRegionKey>& re
     }
     polling_cv_.notify_all();
 
-    if (!regions.empty() && isOperationalState(state())) {
+    if (!regions.empty() && isOperationalState(state()) &&
+        !polling_thread_running_.load(std::memory_order_acquire)) {
         refreshSensorData();
     }
 }
@@ -482,8 +537,7 @@ std::vector<TactileRegionData> PX6AXGen3::getSensorData() {
 
 TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion region) {
     if (!isSupportedRegion(finger, region)) {
-        CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data.";
-        return {};
+        throw std::invalid_argument("PX6AXGen3 requested tactile region is not configured.");
     }
 
     ensureSensorReady(true, true, false);
@@ -492,16 +546,14 @@ TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion regi
 
 PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, TactileRegion region) {
     if (!isSupportedRegion(finger, region)) {
-        CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data.";
-        return {};
+        throw std::invalid_argument("PX6AXGen3 requested resultant-force region is not configured.");
     }
 
     ensureSensorReady(true, false, true);
 
     std::lock_guard<std::mutex> lock(snapshot_mutex_);
-    if (!latest_snapshot_.resultant_valid) {
-        CMVR_LOG(ERROR) << "PX6AXGen3 resultant-force snapshot is not ready.";
-        return {};
+    if (!latest_snapshot_.resultant_valid || !isSampleFresh(latest_snapshot_.resultant_request_time)) {
+        throw std::runtime_error("PX6AXGen3 resultant-force sample is unavailable or stale.");
     }
 
     return ResultantForce{
@@ -511,29 +563,28 @@ PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, Tactil
     };
 }
 
+PX6AXGen3::ForceNewtons PX6AXGen3::getResultantForceNewtons(FingerType finger, TactileRegion region) {
+    const auto raw = getResultantForce(finger, region);
+    // PX-6AX GEN3 manual 5.6.2: one resultant-force LSB is 0.1 N.
+    constexpr double kNewtonsPerCount = 0.1;
+    return {raw.fx * kNewtonsPerCount, raw.fy * kNewtonsPerCount, raw.fz * kNewtonsPerCount};
+}
+
 void PX6AXGen3::initializeSnapshot() {
     std::lock_guard<std::mutex> lock(snapshot_mutex_);
     latest_snapshot_ = SensorSnapshot{};
 }
 
 void PX6AXGen3::ensureConnected() {
-    if (port_name_.empty()) {
-        enterFault("PX6AXGen3 serial port is not configured.");
-        return;
+    if (!config_valid_ || port_name_.empty()) {
+        throw std::runtime_error("PX6AXGen3 serial/configuration is invalid.");
     }
-
-    if (!serial_) {
-        serial_ = std::make_unique<::cmvr::PosixSerialTransport>();
-    }
-
     if (!serial_->isOpen()) {
         if (!serial_->open(::cmvr::AbstractSerialTransport::Config{port_name_, baud_rate_})) {
-            enterFault("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError());
-            return;
+            throw std::runtime_error("Failed to open PX6AXGen3 serial transport: " + serial_->lastError());
         }
         calibration_performed_ = false;
     }
-
     calibrateIfRequested();
     if (!isOperationalState(state())) {
         transitionTo(Status::INITIALIZED);
@@ -553,27 +604,9 @@ void PX6AXGen3::calibrateIfRequested() {
     if (!auto_calibrate_ || calibration_performed_) {
         return;
     }
-
     const auto frame = buildCommandFrame(CommandType::CALIBRATION, device_address_, distributed_length_);
-    if (!serial_->flushInput()) {
-        enterFault("Failed to flush serial input before calibration: " + serial_->lastError());
-        return;
-    }
-    if (!serial_->write(frame)) {
-        enterFault("Failed to send calibration command: " + serial_->lastError());
-        return;
-    }
-    const auto response = readFramedResponse(
-        *serial_,
-        2U,
-        frame.size(),
-        std::chrono::milliseconds(response_timeout_ms_),
-        "calibration response");
-    if (findResponseFrameOffset(response, 2U) == std::string::npos) {
-        enterFault("PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" +
-                   previewBytesHex(response));
-        return;
-    }
+    // Write acknowledgment has a complete 14-byte header and LRC, no payload.
+    transact(*serial_, frame, 0U, std::chrono::milliseconds(response_timeout_ms_));
     calibration_performed_ = true;
 }
 
@@ -584,117 +617,51 @@ void PX6AXGen3::refreshSensorData() {
 
 void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_resultant) {
     if (!read_distributed && !read_resultant) {
-        CMVR_LOG(ERROR) << "PX6AXGen3 refreshSensorData requires at least one data type to read.";
-        return;
+        throw std::invalid_argument("PX6AXGen3 refresh requires at least one data type.");
     }
 
     std::lock_guard<std::mutex> refresh_lock(refresh_mutex_);
-    const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant);
-
     try {
         ensureConnected();
-        if (!isOperationalState(state())) {
-            return;
-        }
-
-        std::vector<TactilePoint> tactile_points;
-        int rows = 0;
-        int cols = 0;
-        bool tactile_valid = false;
-
-        if (read_distributed) {
-            const auto distributed_frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_);
-            if (!serial_->flushInput()) {
-                handleRefreshFailure("Failed to flush serial input before distributed-force read: " + serial_->lastError(), had_valid_snapshot);
-                return;
-            }
-            if (!serial_->write(distributed_frame)) {
-                handleRefreshFailure("Failed to send distributed-force command: " + serial_->lastError(), had_valid_snapshot);
-                return;
-            }
-            const size_t distributed_frame_bytes =
-                static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(distributed_length_);
-            const auto distributed_response = readFramedResponse(
-                *serial_,
-                distributed_frame_bytes,
-                distributed_frame.size(),
-                std::chrono::milliseconds(response_timeout_ms_),
-                "distributed tactile response");
-            const auto distributed_frame_offset =
-                findResponseFrameOffset(distributed_response, distributed_frame_bytes);
-            if (distributed_frame_offset == std::string::npos) {
-                handleRefreshFailure("Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response), had_valid_snapshot);
-                return;
-            }
-
-            tactile_points = parseDistributedPayload(extractPayload(
-                distributed_response,
-                distributed_frame_offset,
-                static_cast<size_t>(response_header_bytes_),
-                static_cast<size_t>(distributed_length_)));
-            std::tie(rows, cols) = resolveMatrixShape(tactile_rows_, tactile_cols_, tactile_points.size());
-            tactile_valid = !tactile_points.empty();
-        }
-
-        std::array<int, 3> resultant_force_tenths{};
-        bool resultant_valid = false;
+        // Publish force first: a slower distributed read must not postpone a
+        // contact measurement. Each channel has its own request timestamp.
         if (read_resultant) {
-            try {
-                const auto resultant_frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_);
-                if (!serial_->flushInput()) {
-                    handleRefreshFailure("Failed to flush serial input before resultant-force read: " + serial_->lastError(), had_valid_snapshot);
-                    return;
+            const auto frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_);
+            const auto request_time = std::chrono::steady_clock::now();
+            const auto payload = transact(*serial_, frame, resultant_length_,
+                                          std::chrono::milliseconds(response_timeout_ms_));
+            if (!isSampleFresh(request_time)) {
+                throw std::runtime_error("PX6AXGen3 resultant-force response arrived too late.");
             }
-                if (!serial_->write(resultant_frame)) {
-                    handleRefreshFailure("Failed to send resultant-force command: " + serial_->lastError(), had_valid_snapshot);
-                    return;
-                }
-                const size_t resultant_frame_bytes =
-                    static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(resultant_length_);
-                const auto resultant_response = readFramedResponse(
-                    *serial_,
-                    resultant_frame_bytes,
-                    resultant_frame.size(),
-                    std::chrono::milliseconds(response_timeout_ms_),
-                    "resultant-force response");
-                const auto resultant_frame_offset =
-                    findResponseFrameOffset(resultant_response, resultant_frame_bytes);
-                if (resultant_frame_offset != std::string::npos) {
-                    resultant_force_tenths = parseResultantPayload(extractPayload(
-                        resultant_response,
-                        resultant_frame_offset,
-                        static_cast<size_t>(response_header_bytes_),
-                        static_cast<size_t>(resultant_length_)));
-                    resultant_valid = true;
-                } else {
-                    handleRefreshFailure("Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response), had_valid_snapshot);
-                    return;
-                }
-            } catch (const std::exception& e) {
-                if (!read_distributed) {
-                    handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot);
-                    return;
-                }
-                CMVR_LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what();
-            }
-        }
-
+            const auto force = parseResultantPayload(payload);
             std::lock_guard<std::mutex> lock(snapshot_mutex_);
+            latest_snapshot_.resultant_force_tenths = force;
+            latest_snapshot_.resultant_request_time = request_time;
+            latest_snapshot_.resultant_valid = true;
+        }
         if (read_distributed) {
-            latest_snapshot_.tactile_points = std::move(tactile_points);
+            const auto frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_);
+            const auto request_time = std::chrono::steady_clock::now();
+            const auto payload = transact(*serial_, frame, distributed_length_,
+                                          std::chrono::milliseconds(response_timeout_ms_));
+            if (!isSampleFresh(request_time)) {
+                throw std::runtime_error("PX6AXGen3 distributed-force response arrived too late.");
+            }
+            auto points = parseDistributedPayload(payload);
+            const auto [rows, cols] = resolveMatrixShape(tactile_rows_, tactile_cols_, points.size());
+            std::lock_guard<std::mutex> lock(snapshot_mutex_);
+            latest_snapshot_.tactile_points = std::move(points);
             latest_snapshot_.rows = rows;
             latest_snapshot_.cols = cols;
-            latest_snapshot_.tactile_valid = tactile_valid;
+            latest_snapshot_.tactile_request_time = request_time;
+            latest_snapshot_.tactile_valid = true;
         }
-        if (read_resultant) {
-            latest_snapshot_.resultant_force_tenths = resultant_force_tenths;
-            latest_snapshot_.resultant_valid = resultant_valid;
-        }
-
         clearOperationalError();
     } catch (const std::exception& e) {
-        handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot);
-        return;
+        handleRefreshFailure(e.what());
+        // Let initialization retries and synchronous callers see the failure.
+        // The polling thread catches it and continues reconnecting in background.
+        throw;
     }
 }
 
@@ -715,7 +682,7 @@ void PX6AXGen3::refreshSensorDataWithRetry(const int max_attempts,
         }
     }
 
-    handleRefreshFailure(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error, isSnapshotReady(true, true));
+    throw std::runtime_error(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error);
 }
 
 void PX6AXGen3::pollingLoop() {
@@ -754,18 +721,21 @@ void PX6AXGen3::pollingLoop() {
 void PX6AXGen3::ensureSensorReady(const bool allow_background,
                                   const bool require_tactile,
                                   const bool require_resultant) {
+    const auto lifecycle = state();
+    if (!config_valid_ || lifecycle == Status::STOPPED || lifecycle == Status::FAULT) {
+        throw std::runtime_error("PX6AXGen3 is not operational: " + lastError());
+    }
     const auto [polls_tactile, polls_resultant] = resolvePollingReadSelection();
     const bool background_covers_request =
         (!require_tactile || polls_tactile) &&
         (!require_resultant || polls_resultant);
-    const bool background_ready = allow_background &&
-        background_covers_request &&
-        polling_thread_running_.load(std::memory_order_acquire) &&
-        isSnapshotReady(require_tactile, require_resultant);
-
-    if (!background_ready) {
-        refreshSensorData(require_tactile, require_resultant);
+    if (allow_background && background_covers_request &&
+        polling_thread_running_.load(std::memory_order_acquire)) {
+        // The getter checks freshness while copying under snapshot_mutex_.
+        // Never block the control loop on serial I/O to replace a stale sample.
+        return;
     }
+    refreshSensorData(require_tactile, require_resultant);
 }
 
 bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion region) const {
@@ -774,8 +744,15 @@ bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion r
 
 bool PX6AXGen3::isSnapshotReady(const bool require_tactile, const bool require_resultant) const {
     std::lock_guard<std::mutex> lock(snapshot_mutex_);
-    return (!require_tactile || latest_snapshot_.tactile_valid) &&
-           (!require_resultant || latest_snapshot_.resultant_valid);
+    return (!require_tactile || (latest_snapshot_.tactile_valid &&
+                                isSampleFresh(latest_snapshot_.tactile_request_time))) &&
+           (!require_resultant || (latest_snapshot_.resultant_valid &&
+                                  isSampleFresh(latest_snapshot_.resultant_request_time)));
+}
+
+bool PX6AXGen3::isSampleFresh(const std::chrono::steady_clock::time_point request_time) const {
+    return request_time != std::chrono::steady_clock::time_point{} &&
+           std::chrono::steady_clock::now() - request_time <= max_sample_age_;
 }
 
 TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const {
@@ -785,9 +762,8 @@ TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const {
 
     {
         std::lock_guard<std::mutex> lock(snapshot_mutex_);
-        if (!latest_snapshot_.tactile_valid) {
-            CMVR_LOG(ERROR) << "PX6AXGen3 tactile snapshot is not ready.";
-            return {};
+        if (!latest_snapshot_.tactile_valid || !isSampleFresh(latest_snapshot_.tactile_request_time)) {
+            throw std::runtime_error("PX6AXGen3 tactile sample is unavailable or stale.");
         }
         *snapshot = latest_snapshot_.tactile_points;
         rows = latest_snapshot_.rows;
@@ -823,21 +799,15 @@ void PX6AXGen3::clearOperationalError() {
     }
 }
 
-void PX6AXGen3::handleRefreshFailure(const std::string& error, const bool had_valid_snapshot) {
-    closeConnection();
-
-    if (had_valid_snapshot) {
+void PX6AXGen3::handleRefreshFailure(const std::string& error) {
+    // Invalidate before any close/reconnect work. Keep the worker alive so a
+    // subsequent valid transaction can restore service, but never expose old data.
+    initializeSnapshot();
     {
         std::lock_guard<std::mutex> lock(lifecycle_mutex_);
-            if (lifecycle_state_ == Status::INITIALIZED || lifecycle_state_ == Status::STREAMING) {
         last_error_ = error;
     }
-        }
-        CMVR_LOG(WARNING) << error;
-        return;
-    }
-
-    enterFault(error);
+    closeConnection();
 }
 
 void PX6AXGen3::transitionTo(const Status next_state) {
@@ -857,6 +827,7 @@ void PX6AXGen3::enterFault(const std::string& error) {
 
     polling_thread_running_.store(false, std::memory_order_release);
     polling_cv_.notify_all();
+    initializeSnapshot();
     closeConnection();
 
     CMVR_LOG(ERROR) << error;

cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp

@@ -0,0 +1,201 @@
+#include "../include/px_6ax_gen3.h"
+#include "hardware/include/posix_serial_transport.h"
+
+#include <algorithm>
+#include <cmath>
+#include <csignal>
+#include <cstdlib>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <stdexcept>
+
+namespace {
+using Clock = std::chrono::steady_clock;
+volatile std::sig_atomic_t interrupted = 0;
+void interrupt(int) { interrupted = 1; }
+
+// Wrap the actual POSIX serial transport; optionally capture every transmitted
+// and received byte to diagnose framing/USB latency without a second reader.
+class TraceTransport final : public cmvr::AbstractSerialTransport {
+public:
+    explicit TraceTransport(const std::string& path) {
+        if (!path.empty()) {
+            trace_.open(path);
+            if (!trace_) throw std::runtime_error("Cannot open raw log: " + path);
+        }
+    }
+    bool open(const Config& cfg) override { return serial_.open(cfg); }
+    bool close() override { return serial_.close(); }
+    bool isOpen() const override { return serial_.isOpen(); }
+    bool flushInput() override { return serial_.flushInput(); }
+    bool write(const std::vector<uint8_t>& data) override {
+        // Only permit sensor read requests, even if driver defaults change.
+        if (data.size() != 14 || data[6] != 0xFB) {
+            throw std::runtime_error("Real test only permits force read commands.");
+        }
+        const bool ok = serial_.write(data);
+        record(ok ? "TX" : "TX_FAILED", data);
+        return ok;
+    }
+    bool read(size_t n, std::chrono::milliseconds timeout, std::vector<uint8_t>& data) override {
+        const bool ok = serial_.read(n, timeout, data);
+        record(ok ? "RX" : "RX_FAILED", data);
+        return ok;
+    }
+    std::string lastError() const override { return serial_.lastError(); }
+private:
+    void record(const char* direction, const std::vector<uint8_t>& data) {
+        if (!trace_.is_open()) return;
+        trace_ << std::fixed << std::setprecision(3)
+               << std::chrono::duration<double, std::milli>(Clock::now() - begin_).count()
+               << " ms " << direction;
+        for (const auto byte : data) {
+            trace_ << ' ' << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte);
+        }
+        trace_ << std::dec << std::setfill(' ') << '\n';
+        trace_.flush();
+    }
+    cmvr::PosixSerialTransport serial_;
+    std::ofstream trace_;
+    Clock::time_point begin_{Clock::now()};
+};
+
+struct Options {
+    std::string port{"/dev/ttyACM0"};
+    std::string mode{"sync"};
+    std::string csv;
+    std::string raw_log;
+    int module_id{2};
+    int poll_ms{5};
+    int max_age_ms{50};
+    int timeout_ms{200};
+    double duration_s{10.0};
+};
+
+void usage() {
+    std::cout << "Usage: px_6ax_gen3_real_test [--port /dev/ttyACM0] [--module-id 2]\n"
+                 "  [--duration-s 10] [--mode sync|stream] [--poll-ms 5]\n"
+                 "  [--max-age-ms 50] [--timeout-ms 200] [--csv samples.csv]\n"
+                 "  [--raw-log frames.log]\n"
+                 "sync: each read sends a new force request; latency is request/response time.\n"
+                 "stream: exercise background polling and cached getters used by the task;\n"
+                 "        getter counts include repeated samples, not sensor update frequency.\n"
+                 "Only reads the sensor. No calibration or robot commands. Ctrl-C stops.\n";
+}
+
+Options parseOptions(int argc, char** argv) {
+    Options o;
+    for (int i = 1; i < argc; ++i) {
+        const std::string arg = argv[i];
+        if (arg == "--help") { usage(); std::exit(0); }
+        if (++i >= argc) throw std::invalid_argument("Missing value for " + arg);
+        const std::string value = argv[i];
+        if (arg == "--port") o.port = value;
+        else if (arg == "--mode") o.mode = value;
+        else if (arg == "--module-id") o.module_id = std::stoi(value);
+        else if (arg == "--duration-s") o.duration_s = std::stod(value);
+        else if (arg == "--poll-ms") o.poll_ms = std::stoi(value);
+        else if (arg == "--max-age-ms") o.max_age_ms = std::stoi(value);
+        else if (arg == "--timeout-ms") o.timeout_ms = std::stoi(value);
+        else if (arg == "--csv") o.csv = value;
+        else if (arg == "--raw-log") o.raw_log = value;
+        else throw std::invalid_argument("Unknown option: " + arg);
+    }
+    if ((o.mode != "sync" && o.mode != "stream") || !std::isfinite(o.duration_s) ||
+        o.duration_s <= 0 || o.poll_ms <= 0 || o.max_age_ms <= 0 || o.timeout_ms <= 0) {
+        throw std::invalid_argument("Invalid mode, duration or timing option");
+    }
+    return o;
+}
+
+double percentile(const std::vector<double>& sorted, double fraction) {
+    return sorted.empty() ? 0.0 : sorted[static_cast<size_t>((sorted.size() - 1) * fraction)];
+}
+}
+
+int main(int argc, char** argv) {
+    try {
+        const auto options = parseOptions(argc, argv);
+        std::signal(SIGINT, interrupt);
+        std::signal(SIGTERM, interrupt);
+        cmvr::config::PX6AXGen3 cfg;
+        cfg.set_serial_port(options.port);
+        cfg.set_module_id(options.module_id);
+        cfg.set_baud_rate(921600);
+        cfg.set_distributed_length(153);
+        cfg.set_resultant_length(3);
+        cfg.set_poll_interval_ms(options.poll_ms);
+        cfg.set_response_timeout_ms(options.timeout_ms);
+        cfg.set_max_sample_age_ms(options.max_age_ms);
+        cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE);
+        cfg.set_auto_calibrate(false);
+        cmvr::device::PX6AXGen3 sensor(cfg, std::make_unique<TraceTransport>(options.raw_log));
+        std::ofstream csv;
+        if (!options.csv.empty()) {
+            csv.open(options.csv);
+            if (!csv) throw std::runtime_error("Cannot open CSV: " + options.csv);
+            csv << "elapsed_ms,read_ms,valid,fx_N,fy_N,fz_N\n";
+        }
+        std::cout << "port=" << options.port << " module_id=" << options.module_id
+                  << " device_address=" << options.module_id + 1 << " mode=" << options.mode
+                  << " max_sample_age_ms=" << options.max_age_ms << '\n';
+        if (!sensor.init()) throw std::runtime_error("Sensor init failed: " + sensor.lastError());
+        if (options.mode == "stream" && !sensor.start()) {
+            throw std::runtime_error("Sensor start failed: " + sensor.lastError());
+        }
+        const auto begin = Clock::now();
+        auto next = begin;
+        auto next_print = begin;
+        std::vector<double> durations;
+        size_t failures = 0;
+        double max_fz = 0.0;
+        while (!interrupted && std::chrono::duration<double>(Clock::now() - begin).count() < options.duration_s) {
+            const auto read_begin = Clock::now();
+            cmvr::device::AbstractDexHand::ForceNewtons force;
+            bool valid = true;
+            std::string error;
+            try {
+                force = sensor.getResultantForceNewtons(cmvr::device::AbstractDexHand::FingerType::INDEX,
+                                                        cmvr::device::AbstractDexHand::TactileRegion::TIP);
+            } catch (const std::exception& e) {
+                valid = false;
+                error = e.what();
+                ++failures;
+            }
+            const auto now = Clock::now();
+            const double read_ms = std::chrono::duration<double, std::milli>(now - read_begin).count();
+            const double elapsed_ms = std::chrono::duration<double, std::milli>(now - begin).count();
+            if (valid) { durations.push_back(read_ms); max_fz = std::max(max_fz, force.fz); }
+            if (csv.is_open()) {
+                csv << std::fixed << std::setprecision(3) << elapsed_ms << ',' << read_ms << ',' << valid;
+                if (valid) csv << ',' << force.fx << ',' << force.fy << ',' << force.fz;
+                else csv << ",,,";
+                csv << '\n';
+            }
+            if (now >= next_print) {
+                std::cout << std::fixed << std::setprecision(3) << "t_ms=" << elapsed_ms
+                          << " read_ms=" << read_ms;
+                if (valid) std::cout << " force_N=[" << force.fx << ',' << force.fy << ',' << force.fz
+                                     << "] fz_N=" << force.fz;
+                else std::cout << " UNAVAILABLE: " << error;
+                std::cout << std::endl;
+                next_print = now + std::chrono::milliseconds(100);
+            }
+            next += std::chrono::milliseconds(options.poll_ms);
+            if (next < now) next = now;
+            std::this_thread::sleep_until(next);
+        }
+        const double elapsed_s = std::chrono::duration<double>(Clock::now() - begin).count();
+        sensor.stop();
+        std::sort(durations.begin(), durations.end());
+        std::cout << "SUMMARY mode=" << options.mode << " valid_reads=" << durations.size()
+                  << " unavailable_reads=" << failures << " valid_reads_per_s=" << durations.size() / elapsed_s
+                  << " p50_ms=" << percentile(durations, .5) << " p95_ms=" << percentile(durations, .95)
+                  << " max_ms=" << percentile(durations, 1) << " max_fz_N=" << max_fz << '\n';
+        return durations.empty() || failures != 0 ? 1 : 0;
+    } catch (const std::exception& e) {
+        std::cerr << e.what() << '\n';
+        return 1;
+    }
+}

cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp

@@ -0,0 +1,318 @@
+#include "../include/px_6ax_gen3.h"
+
+#include <gtest/gtest.h>
+#include <algorithm>
+#include <atomic>
+#include <functional>
+#include <stdexcept>
+
+namespace {
+using Sensor = cmvr::device::PX6AXGen3;
+using Bytes = std::vector<uint8_t>;
+using namespace std::chrono_literals;
+
+void checksum(Bytes& frame) {
+    unsigned sum = 0;
+    for (size_t i = 0; i + 1 < frame.size(); ++i) sum += frame[i];
+    frame.back() = static_cast<uint8_t>(-sum);
+}
+
+Bytes reply(const Bytes& request) {
+    const bool is_write = request[6] == 0x79;
+    const size_t n = is_write ? 0U : request[11] | (size_t(request[12]) << 8U);
+    Bytes frame{0xAA, 0x55, static_cast<uint8_t>((n + 10U) & 0xffU),
+                static_cast<uint8_t>((n + 10U) >> 8U)};
+    frame.insert(frame.end(), request.begin() + 4, request.begin() + 13);
+    // Real device returns status=1 on reads; zero is only the write ACK status.
+    frame.push_back(is_write ? 0 : 1);
+    for (size_t i = 0; i < n; ++i) {
+        frame.push_back(i % 3U == 0 ? 0x80 : (i % 3U == 1 ? 0x7f : 10));
+    }
+    frame.push_back(0);
+    checksum(frame);
+    return frame;
+}
+
+class ScriptedTransport final : public cmvr::AbstractSerialTransport {
+public:
+    bool open(const Config&) override { opened = true; return true; }
+    bool close() override { opened = false; return true; }
+    bool isOpen() const override { return opened; }
+    bool flushInput() override { pending.clear(); return true; }
+    bool write(const Bytes& request) override {
+        ++writes;
+        unsigned sum = 0;
+        for (auto byte : request) sum += byte;
+        EXPECT_EQ(sum % 256, 0U);
+        pending = dropping.load() ? Bytes{} : make_reply(request);
+        return true;
+    }
+    bool read(size_t, std::chrono::milliseconds timeout, Bytes& out) override {
+        out.clear();
+        if (pending.empty()) {
+            waiting.store(true);
+            std::this_thread::sleep_for(timeout);
+            waiting.store(false);
+            return false;
+        }
+        const size_t n = std::min(chunk_size, pending.size());
+        out.assign(pending.begin(), pending.begin() + n);
+        pending.erase(pending.begin(), pending.begin() + n);
+        return true;
+    }
+    std::string lastError() const override { return "scripted timeout"; }
+    std::function<Bytes(const Bytes&)> make_reply{reply};
+    size_t chunk_size{256};
+    std::atomic<int> writes{0};
+    std::atomic<bool> dropping{false};
+    std::atomic<bool> waiting{false};
+private:
+    bool opened{false};
+    Bytes pending;
+};
+
+cmvr::config::PX6AXGen3 config() {
+    cmvr::config::PX6AXGen3 cfg;
+    cfg.set_serial_port("scripted");
+    cfg.set_module_id(2);
+    cfg.set_distributed_length(153);
+    cfg.set_poll_interval_ms(5);
+    cfg.set_response_timeout_ms(5);
+    cfg.set_max_sample_age_ms(50);
+    cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE);
+    return cfg;
+}
+
+Sensor::ResultantForce force(Sensor& sensor) {
+    return sensor.getResultantForce(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
+}
+
+Sensor::ForceNewtons forceNewtons(cmvr::device::AbstractDexHand& sensor) {
+    return sensor.getResultantForceNewtons(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
+}
+
+bool waitFor(const std::function<bool()>& predicate) {
+    const auto deadline = std::chrono::steady_clock::now() + 1s;
+    do {
+        if (predicate()) return true;
+        std::this_thread::sleep_for(1ms);
+    } while (std::chrono::steady_clock::now() < deadline);
+    return predicate();
+}
+
+TEST(PX6AXGen3, ParsesRealReadStatusAndSignedForcesAcrossByteFragments) {
+    auto serial = std::make_unique<ScriptedTransport>();
+    serial->chunk_size = 1;
+    Sensor sensor(config(), std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    const auto f = force(sensor);
+    EXPECT_EQ(f.fx, -128);
+    EXPECT_EQ(f.fy, 127);
+    EXPECT_EQ(f.fz, 10);
+}
+
+TEST(PX6AXGen3, NewtonInterfacePreservesSignsAndComputesPhysicalMagnitude) {
+    auto serial = std::make_unique<ScriptedTransport>();
+    auto* transport = serial.get();
+    serial->make_reply = [](const Bytes& request) {
+        auto frame = reply(request);
+        frame[14] = 0xfd; // -3 raw = -0.3 N
+        frame[15] = 4;
+        frame[16] = 12;
+        checksum(frame);
+        return frame;
+    };
+    Sensor sensor(config(), std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    const auto writes_before = transport->writes.load();
+    const auto f = forceNewtons(sensor);
+    EXPECT_EQ(transport->writes.load(), writes_before + 1);
+    EXPECT_DOUBLE_EQ(f.fx, -0.3);
+    EXPECT_DOUBLE_EQ(f.fy, 0.4);
+    EXPECT_DOUBLE_EQ(f.fz, 1.2);
+    EXPECT_DOUBLE_EQ(f.magnitude(), 1.3);
+    EXPECT_EQ(force(sensor).fz, 12); // Raw interface remains unchanged.
+}
+
+class NewtonForce : public testing::TestWithParam<int> {};
+TEST_P(NewtonForce, ConvertsRawFzOnceAndPreservesPointOneNewtonTrigger) {
+    auto serial = std::make_unique<ScriptedTransport>();
+    const int raw_fz = GetParam();
+    serial->make_reply = [raw_fz](const Bytes& request) {
+        auto frame = reply(request);
+        frame[14] = 0;
+        frame[15] = 0;
+        frame[16] = static_cast<uint8_t>(raw_fz);
+        checksum(frame);
+        return frame;
+    };
+    Sensor sensor(config(), std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    const auto f = forceNewtons(sensor);
+    EXPECT_DOUBLE_EQ(f.fz, static_cast<double>(raw_fz) / 10.0);
+    EXPECT_DOUBLE_EQ(f.magnitude(), f.fz);
+    EXPECT_EQ(f.fz >= 0.1, raw_fz >= 1);
+    EXPECT_EQ(force(sensor).fz, raw_fz);
+}
+INSTANTIATE_TEST_SUITE_P(PhysicalUnits, NewtonForce, testing::Values(0, 1, 10, 108, 255));
+
+TEST(PX6AXGen3, ResynchronizesAfterEchoNoiseAndInvalidFrame) {
+    auto serial = std::make_unique<ScriptedTransport>();
+    serial->chunk_size = 7;
+    serial->make_reply = [](const Bytes& request) {
+        auto bad = reply(request);
+        bad.back() ^= 1;
+        Bytes frames{0xAA, 0x00};
+        frames.insert(frames.end(), request.begin(), request.end());
+        frames.insert(frames.end(), bad.begin(), bad.end());
+        const auto valid = reply(request);
+        frames.insert(frames.end(), valid.begin(), valid.end());
+        return frames;
+    };
+    Sensor sensor(config(), std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    EXPECT_EQ(force(sensor).fz, 10);
+}
+
+class InvalidFrame : public testing::TestWithParam<int> {};
+TEST_P(InvalidFrame, RejectsResponseAndFailsInitializationAfterRetries) {
+    auto serial = std::make_unique<ScriptedTransport>();
+    auto* transport = serial.get();
+    const int kind = GetParam();
+    serial->make_reply = [kind](const Bytes& request) {
+        auto frame = reply(request);
+        if (kind == 0) { frame.pop_back(); return frame; } // missing LRC
+        if (kind == 1) { frame.back() ^= 1; return frame; } // bad LRC
+        // Frame size, device, reserved, function, register, returned byte count.
+        const int offsets[]{2, 4, 5, 6, 7, 11};
+        frame[offsets[kind - 2]] ^= 1;
+        checksum(frame); // Valid LRC must not bypass request matching.
+        return frame;
+    };
+    Sensor sensor(config(), std::move(serial));
+    EXPECT_FALSE(sensor.init());
+    EXPECT_EQ(transport->writes.load(), 5);
+    EXPECT_EQ(sensor.state(), Sensor::Status::FAULT);
+    EXPECT_FALSE(sensor.lastError().empty());
+    EXPECT_THROW(force(sensor), std::runtime_error);
+}
+INSTANTIATE_TEST_SUITE_P(ProtocolValidation, InvalidFrame, testing::Range(0, 8));
+
+TEST(PX6AXGen3, RetriesTransientStartupFailure) {
+    auto serial = std::make_unique<ScriptedTransport>();
+    int count = 0;
+    serial->make_reply = [&count](const Bytes& request) {
+        ++count;
+        return count < 3 ? Bytes{} : reply(request);
+    };
+    Sensor sensor(config(), std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    EXPECT_EQ(count, 3);
+    EXPECT_TRUE(sensor.lastError().empty());
+}
+
+TEST(PX6AXGen3, StaleGetterFailsWithoutWaitingForSerialAndRecovers) {
+    auto cfg = config();
+    cfg.set_response_timeout_ms(200);
+    cfg.set_max_sample_age_ms(30);
+    auto serial = std::make_unique<ScriptedTransport>();
+    auto* transport = serial.get();
+    Sensor sensor(cfg, std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    ASSERT_TRUE(sensor.start());
+    ASSERT_EQ(force(sensor).fz, 10);
+    transport->dropping.store(true);
+    ASSERT_TRUE(waitFor([&] { return transport->waiting.load(); }));
+    std::this_thread::sleep_for(40ms);
+    const auto begin = std::chrono::steady_clock::now();
+    EXPECT_THROW(force(sensor), std::runtime_error);
+    EXPECT_THROW(forceNewtons(sensor), std::runtime_error);
+    EXPECT_LT(std::chrono::steady_clock::now() - begin, 50ms);
+    cmvr::device::DexHandState state;
+    sensor.getState(state);
+    EXPECT_FALSE(state.is_initialized);
+    EXPECT_FALSE(state.hands[0].error_message.empty());
+    transport->dropping.store(false);
+    ASSERT_TRUE(waitFor([&] {
+        try { return force(sensor).fz == 10; }
+        catch (const std::exception&) { return false; }
+    }));
+    sensor.stop();
+    const auto writes = transport->writes.load();
+    EXPECT_THROW(force(sensor), std::runtime_error);
+    EXPECT_THROW(forceNewtons(sensor), std::runtime_error);
+    EXPECT_EQ(transport->writes.load(), writes);
+}
+
+TEST(PX6AXGen3, ReadFailureInvalidatesPreviouslyValidSample) {
+    auto serial = std::make_unique<ScriptedTransport>();
+    auto* transport = serial.get();
+    Sensor sensor(config(), std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    transport->dropping.store(true);
+    EXPECT_THROW(force(sensor), std::runtime_error);
+    cmvr::device::DexHandState state;
+    sensor.getState(state);
+    EXPECT_FALSE(state.is_initialized);
+    transport->dropping.store(false);
+    EXPECT_EQ(force(sensor).fz, 10);
+}
+
+TEST(PX6AXGen3, RejectsLateResponseEvenWithValidChecksum) {
+    auto cfg = config();
+    cfg.set_max_sample_age_ms(2);
+    auto serial = std::make_unique<ScriptedTransport>();
+    serial->make_reply = [](const Bytes& request) {
+        std::this_thread::sleep_for(5ms);
+        return reply(request);
+    };
+    Sensor sensor(cfg, std::move(serial));
+    EXPECT_FALSE(sensor.init());
+    EXPECT_NE(sensor.lastError().find("too late"), std::string::npos);
+}
+
+TEST(PX6AXGen3, DistributedDataIsValidatedAndParsed) {
+    auto cfg = config();
+    cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE);
+    auto serial = std::make_unique<ScriptedTransport>();
+    serial->chunk_size = 5;
+    Sensor sensor(cfg, std::move(serial));
+    ASSERT_TRUE(sensor.init());
+    auto data = sensor.getSensorData(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
+    ASSERT_TRUE(data.valid());
+    ASSERT_EQ(data.view.pointCount(), 51);
+    EXPECT_EQ(data.view.at(0, 50).fx, -128);
+    EXPECT_EQ(data.view.at(0, 50).fz, 10);
+}
+
+TEST(PX6AXGen3, CalibrationRequiresFullSuccessfulWriteAcknowledgment) {
+    auto cfg = config();
+    cfg.set_auto_calibrate(true);
+    auto serial = std::make_unique<ScriptedTransport>();
+    serial->make_reply = [](const Bytes& request) {
+        if (request[6] == 0x79) return Bytes{0xAA, 0x55};
+        return reply(request);
+    };
+    Sensor sensor(cfg, std::move(serial));
+    EXPECT_FALSE(sensor.init());
+}
+
+TEST(PX6AXGen3, CalibrationAcceptsFullWriteAcknowledgment) {
+    auto cfg = config();
+    cfg.set_auto_calibrate(true);
+    Sensor sensor(cfg, std::make_unique<ScriptedTransport>());
+    ASSERT_TRUE(sensor.init());
+    EXPECT_EQ(force(sensor).fz, 10);
+}
+
+TEST(PX6AXGen3, InvalidConfigurationCannotBeResurrectedByInitOrStart) {
+    auto cfg = config();
+    cfg.set_response_header_bytes(15);
+    auto serial = std::make_unique<ScriptedTransport>();
+    auto* transport = serial.get();
+    Sensor sensor(cfg, std::move(serial));
+    EXPECT_FALSE(sensor.init());
+    EXPECT_FALSE(sensor.start());
+    EXPECT_EQ(transport->writes.load(), 0);
+}
+}

cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h

@@ -38,6 +38,7 @@ public:
     std::vector<TactileRegionData> getSensorData() override;
     TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
     ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
+    ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override;
 
 private:
     TactileRegionData makeRegionData(FingerType finger, TactileRegion region);

cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp

@@ -83,6 +83,11 @@ ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce(
     return TactilePoint::fromFz(0);
 }
 
+ZeroSimTouchDexHand::ForceNewtons ZeroSimTouchDexHand::getResultantForceNewtons(
+    const FingerType, const TactileRegion) {
+    return {};
+}
+
 ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData(
     const FingerType finger, const TactileRegion region) {
     tactile_points_[0] = TactilePoint::fromFz(0);

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

@@ -95,6 +95,7 @@ public:
 
     int targetU() const;
     int targetV() const;
+    // Selected force criterion summed over requested tactile regions, in N.
     double lastTouchPressureSum() const;
     int lastTouchNonzeroCount() const;
     int lastActiveTagId() const;
@@ -183,8 +184,8 @@ private:
     int pbvs_debug_count_{0};
     int last_active_tag_id_{-1};
 
-    double last_touch_pressure_sum_{0.0};
-    double last_touch_resultant_fz_{0.0};
+    double last_touch_pressure_sum_{0.0}; // N
+    double last_touch_resultant_fz_{0.0}; // N
     int last_touch_nonzero_count_{0};
     Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
     Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
@@ -199,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

@@ -116,15 +116,15 @@ bool isTouchTriggered(const TouchScreenTaskConfig& config,
     return resultant_force_value >= config.touch().tactile().force_threshold();
 }
 
-double tactileForceValue(const device::AbstractDexHand::TactilePoint& point,
+double tactileForceValue(const device::AbstractDexHand::ForceNewtons& point,
                          const cmvr::config::TouchScreenTactileCriterion criterion) {
     switch (criterion) {
         case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ:
-            return static_cast<double>(point.fz);
+            return point.fz;
         case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE:
             return point.magnitude();
     }
-    return static_cast<double>(point.fz);
+    return point.fz;
 }
 
 Eigen::Matrix3d rotationFromTargetEuler(const double rx,
@@ -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_);
-    if (!retract_start_position_valid_) {
-        CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] cannot read TCP start pose";
-        return false;
-    }
+    retract_start_position_valid_ = false;
+    retract_direction_base_.setZero();
+    max_forward_after_retract_m_ = 0.0;
     const auto retract_cmd = toCartesianVelocity(
         cmvr::common::math::toEigenVec6(retract.twist_tool()));
-    CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
-                   << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
-                   << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
-                   << "], acceleration=" << retract.acceleration()
-                   << ", distance_m=" << retract.distance_m()
-                   << ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
-                   << retract_start_position_base_.y() << ", "
-                   << retract_start_position_base_.z() << "]";
-
+    device::SpeedLOptions options;
+    options.acceleration = retract.acceleration();
+    if (retract.has_linear_jerk()) options.linear_jerk = retract.linear_jerk();
+    options.capture_reference = true;
     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;
@@ -2022,9 +2073,10 @@ bool TouchScreenTask::updateTouchPressure() {
     double resultant_fz = 0.0;
     try {
         for (const auto& tactile_region : tactile_regions) {
-            const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second);
+            const auto resultant_force = dexhand_->getResultantForceNewtons(
+                tactile_region.first, tactile_region.second);
             resultant_value += tactileForceValue(resultant_force, tactile.criterion());
-            resultant_fz += static_cast<double>(resultant_force.fz);
+            resultant_fz += resultant_force.fz;
         }
     } catch (...) {
         return false;
@@ -2043,9 +2095,9 @@ void TouchScreenTask::logTouchPressure(const bool force) {
         return;
     }
     last_touch_pressure_log_time_ = now;
-    CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << last_touch_resultant_fz_
-                    << ", criterion_value=" << last_touch_pressure_sum_
-                    << ", threshold=" << config_.touch().tactile().force_threshold()
+    CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz_N=" << last_touch_resultant_fz_
+                    << ", criterion_value_N=" << last_touch_pressure_sum_
+                    << ", threshold_N=" << config_.touch().tactile().force_threshold()
                     << ", triggered=" << isTouchTriggered(config_, last_touch_pressure_sum_);
 }
 

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/dexhand_config/dexhand_config.proto

@@ -36,6 +36,9 @@ message PX6AXGen3{
     string tactile_region = 16;
     string sensor_name = 17;
     PX6AXGen3PollingReadMode polling_read_mode = 18;
+    // Maximum age since the sample's request was sent. 0 uses 50 ms.
+    // Independent of response_timeout_ms: stale cached data must fail promptly.
+    int32 max_sample_age_ms = 19;
 }
 
 message ZeroSimTouchDexHand {

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 {
@@ -112,6 +115,8 @@ message TouchScreenTactileTriggerConfig {
   optional TouchScreenFingerType finger = 1;
   optional TouchScreenTactileRegion region = 2;
   optional TouchScreenTactileCriterion criterion = 3;
+  // Threshold in newtons (N), applied to the selected force criterion summed
+  // over the requested tactile regions. Equality also triggers contact.
   optional double force_threshold = 4;
 }
 
@@ -127,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_px_6ax_gen3.sh

@@ -0,0 +1,12 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+# Real USB sensor test. The executable only sends force-read requests.
+# Example: ./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --duration-s 20 --mode sync
+repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
+build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}"
+cmake --build "$build_dir" --target px_6ax_gen3_real_test -j 4
+sensor_build_dir="$build_dir/cmvr-es/devices/dexhand/px_6ax_gen3"
+# Put build libraries ahead of installed ones so this tests the current driver/proto.
+exec env LD_LIBRARY_PATH="$build_dir:$sensor_build_dir:$build_dir/cmvr-es/hardware:$repo_root/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \
+    "$sensor_build_dir/px_6ax_gen3_real_test" "$@"

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