Merge branch 'lgv_dev' into dev

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.

CMakeLists.txt

@@ -18,7 +18,6 @@ set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE)
 set(CMAKE_BUILD_RPATH "\$ORIGIN:\$ORIGIN/../lib")
 set(CMAKE_INSTALL_RPATH "\$ORIGIN:\$ORIGIN/../lib")
 
-# Use RUNPATH (new dtags) generally preferable
 set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF)
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 

MUJOCO_LOG.TXT

@@ -0,0 +1,12 @@
+Fri Jul 24 15:39:05 2026
+ERROR: could not create window
+
+Fri Jul 24 15:40:37 2026
+ERROR: could not create window
+
+Fri Sep 11 13:10:38 2026
+ERROR: could not initialize GLFW
+
+Fri Sep 11 14:13:09 2026
+ERROR: could not initialize GLFW
+

README.md

@@ -1,23 +1,25 @@
 # CMVR-ES
 
-## Overview
+## 简介
 
-## Installation
+CMVR-ES 工程。
 
-### 1. Git submodules install
+## 安装
+
+### 1. 拉取 Git 子模块
 
 ```
 git submodule update --init --recursive
 ```
 
-### 2. Dependency install
+### 2. 安装系统依赖
 
 ```shell
-# basic
+# 基础工具
 sudo apt-get update
 sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev
 
-# opencv
+# OpenCV
 sudo apt install -y \
   libjpeg-dev libpng-dev libtiff-dev \
   libavcodec-dev libavformat-dev libswscale-dev \
@@ -39,7 +41,7 @@ sudo apt-get install libassimp-dev
 # visp
 sudo apt-get install -y libx11-dev liblapack-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev
 
-# realsense
+# RealSense
 sudo apt-get install -y \
   libusb-1.0-0-dev libudev-dev \
   libglu1-mesa-dev
@@ -48,4 +50,91 @@ sudo apt-get install -y \
 sudo apt install gnuplot-qt
 ```
 
+### 3. 配置 IgH EtherCAT
 
+工程已内置 IgH EtherCAT 1.7.0 的 userspace 文件：
+
+```text
+dependency/x86/third_party/ethercat/v1.7.0
+```
+
+先设置本机路径：
+
+```shell
+export CMVR_ES_ROOT=/path/to/cmvr-es
+export IGH_ETHERCAT_ROOT=$CMVR_ES_ROOT/dependency/x86/third_party/ethercat/v1.7.0
+```
+
+安装当前内核的 header：
+
+```shell
+sudo apt-get update
+sudo apt-get install -y linux-headers-$(uname -r)
+```
+
+如果内置目录里已经有当前内核版本的 EtherCAT 内核模块，直接安装到系统：
+
+```shell
+sudo mkdir -p /lib/modules/$(uname -r)/ethercat
+sudo cp -r $IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)/ethercat/* /lib/modules/$(uname -r)/ethercat/
+sudo depmod
+```
+
+内置内核模块只适用于相同内核版本。如果 `$IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)` 不存在，说明这台机器的内核版本不匹配，需要在这台机器上重新编译安装 IgH EtherCAT：
+
+```shell
+sudo apt-get update
+sudo apt-get install -y \
+  build-essential autoconf automake libtool pkg-config git \
+  linux-headers-$(uname -r)
+
+cd /tmp
+git clone --branch stable-1.7 --depth 1 https://gitlab.com/etherlab.org/ethercat.git ethercat-stable-1.7
+cd ethercat-stable-1.7
+
+./bootstrap
+./configure \
+  --prefix=$IGH_ETHERCAT_ROOT \
+  --libdir=$IGH_ETHERCAT_ROOT/lib \
+  --includedir=$IGH_ETHERCAT_ROOT/include \
+  --sysconfdir=$IGH_ETHERCAT_ROOT/etc \
+  --with-systemdsystemunitdir=$IGH_ETHERCAT_ROOT/lib/systemd/system \
+  --enable-generic \
+  --with-linux-dir=/lib/modules/$(uname -r)/build
+
+make -j$(nproc) all modules
+make install
+sudo make modules_install
+sudo depmod
+```
+
+启动 EtherCAT。`eno1` 换成实际连接 EtherCAT 从站的网卡：
+
+```shell
+sudo script/ethercat/start_ethercat.sh eno1
+```
+
+脚本会写入内置 IgH 配置文件：
+
+```text
+$IGH_ETHERCAT_ROOT/etc/ethercat.conf
+```
+
+脚本会把该网卡的 MAC 写到 `MASTER0_DEVICE`，使用 `DEVICE_MODULES="generic"`，把网卡从 NetworkManager 断开，并通过 `ethercatctl -c` 启动 IgH master。
+
+查看状态：
+
+```shell
+script/ethercat/status_ethercat.sh
+
+$IGH_ETHERCAT_ROOT/bin/ethercat master
+$IGH_ETHERCAT_ROOT/bin/ethercat slaves
+$IGH_ETHERCAT_ROOT/bin/ethercat pdos
+```
+
+停止 EtherCAT：
+
+```shell
+sudo script/ethercat/stop_ethercat.sh eno1
+sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
+```

cmake/FindExternalLib.cmake

@@ -55,7 +55,17 @@ function(setup_external_libs ARCH)
 
         # ---- library dirs ----
         if(EXISTS "${FULL_PATH}/lib")
+            file(GLOB _BUNDLED_LIBSTDCXX_FILES
+                    "${FULL_PATH}/lib/libstdc++.so"
+                    "${FULL_PATH}/lib/libstdc++.so.*"
+            )
+            if(_BUNDLED_LIBSTDCXX_FILES)
+                message(STATUS
+                        "${LIB_NAME}: excluding vendor lib directory from global "
+                        "link paths because it contains a private libstdc++")
+            else()
                 list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib")
+            endif()
             set(HAS_LIB TRUE)
 
             # Collect shared libs for install: *.so and *.so.*
@@ -128,6 +138,10 @@ function(setup_external_libs ARCH)
         link_directories(${LIBRARY_DIRS})
     endif()
 
+    # Use the system/toolchain C++ runtime. Vendor SDKs (e.g. Aubo) may bundle
+    # an older libstdc++ that cannot satisfy the rest of the application's ABI.
+    list(FILTER INSTALL_SO_FILES EXCLUDE REGEX "/libstdc\\+\\+\\.so(\\..*)?$")
+
     # ---- install third-party shared libs into <prefix>/lib ----
     if(INSTALL_SO_FILES)
         list(REMOVE_DUPLICATES INSTALL_SO_FILES)

cmvr-es/algorithms/CMakeLists.txt

@@ -2,3 +2,4 @@ add_subdirectory(motion_planner)
 add_subdirectory(kinematics/ik_solver)
 add_subdirectory(perception)
 add_subdirectory(controllers)
+add_subdirectory(collision_detection)

cmvr-es/algorithms/collision_detection/CMakeLists.txt

@@ -0,0 +1,48 @@
+add_library(self_collision_checker SHARED
+        self_collision/src/self_collision_checker.cpp
+        self_collision/src/distance_sampling_policy.cpp
+)
+
+target_include_directories(self_collision_checker PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_compile_definitions(self_collision_checker PRIVATE
+        PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
+        PINOCCHIO_WITH_HPP_FCL
+        COAL_DISABLE_HPP_FCL_WARNINGS
+)
+
+target_link_libraries(self_collision_checker PUBLIC
+        pinocchio_default
+        pinocchio_parsers
+        pinocchio_collision
+        coal
+)
+
+add_library(cmvr_es::self_collision_checker ALIAS self_collision_checker)
+
+add_executable(self_collision_checker_test
+        self_collision/test/self_collision_checker_test.cpp
+)
+target_link_libraries(self_collision_checker_test PRIVATE
+        cmvr_es::self_collision_checker
+        gtest
+        gtest_main
+        pthread
+)
+target_compile_definitions(self_collision_checker_test PRIVATE
+        CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}"
+)
+
+add_executable(self_collision_benchmark
+        self_collision/benchmark/self_collision_benchmark.cpp
+)
+target_link_libraries(self_collision_benchmark PRIVATE
+        cmvr_es::self_collision_checker
+)
+target_compile_definitions(self_collision_benchmark PRIVATE
+        CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}"
+)
+
+install(TARGETS self_collision_checker LIBRARY DESTINATION lib)

cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp

@@ -0,0 +1,53 @@
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+int main()
+{
+    const std::string urdf_path = std::string(CMVR_ES_SOURCE_DIR) +
+                                  "/model/xiaoyan_description/dual_arm_collision.urdf";
+    const std::vector<std::string> joint_names{
+        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R",
+        "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
+    };
+
+    cmvr::SelfCollisionChecker checker;
+    std::string error;
+    if (!checker.init(urdf_path, joint_names, {}, &error)) {
+        std::cerr << "Initialization failed: " << error << '\n';
+        return 1;
+    }
+
+    constexpr std::size_t kIterations = 2000;
+    std::vector<double> samples_us;
+    samples_us.reserve(kIterations);
+    std::vector<double> q(joint_names.size(), 0.0);
+    for (std::size_t iteration = 0; iteration < kIterations; ++iteration) {
+        q[0] = 0.2 * static_cast<double>(iteration % 100) / 100.0;
+        const auto begin = std::chrono::steady_clock::now();
+        const auto result = checker.check(q);
+        const auto end = std::chrono::steady_clock::now();
+        if (!result.valid) {
+            std::cerr << "Collision check failed: " << result.error << '\n';
+            return 1;
+        }
+        samples_us.push_back(std::chrono::duration<double, std::micro>(end - begin).count());
+    }
+
+    std::sort(samples_us.begin(), samples_us.end());
+    double total_us = 0.0;
+    for (const double sample : samples_us) {
+        total_us += sample;
+    }
+    const std::size_t p99_index = static_cast<std::size_t>(0.99 * (samples_us.size() - 1));
+    std::cout << "active_pairs=" << checker.activePairCount() << '\n'
+              << "iterations=" << samples_us.size() << '\n'
+              << "average_us=" << total_us / samples_us.size() << '\n'
+              << "p99_us=" << samples_us[p99_index] << '\n'
+              << "max_us=" << samples_us.back() << '\n';
+    return 0;
+}

cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h

@@ -0,0 +1,46 @@
+#ifndef CMVR_ES_DISTANCE_SAMPLING_POLICY_H
+#define CMVR_ES_DISTANCE_SAMPLING_POLICY_H
+
+#include <chrono>
+#include <string>
+
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+namespace cmvr {
+
+struct DistanceSamplingOptions {
+    double max_geometry_displacement_m{0.002};
+    double max_check_period_s{0.01};
+};
+
+class DistanceSamplingPolicy {
+public:
+    using Clock = std::chrono::steady_clock;
+
+    bool configure(const DistanceSamplingOptions& options,
+                   std::string* error = nullptr);
+
+    bool shouldCheck(const CollisionGeometrySnapshot& current,
+                     Clock::time_point now) const;
+
+    void markChecked(const CollisionGeometrySnapshot& current,
+                     Clock::time_point now);
+
+    void reset();
+
+    double displacementSinceLastCheck(
+        const CollisionGeometrySnapshot& current) const;
+
+    bool hasBaseline() const { return has_baseline_; }
+
+private:
+    DistanceSamplingOptions options_{};
+    CollisionGeometrySnapshot last_checked_{};
+    Clock::time_point last_check_time_{};
+    bool configured_{false};
+    bool has_baseline_{false};
+};
+
+} // namespace cmvr
+
+#endif // CMVR_ES_DISTANCE_SAMPLING_POLICY_H

cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h

@@ -0,0 +1,82 @@
+#ifndef CMVR_ES_SELF_COLLISION_CHECKER_H
+#define CMVR_ES_SELF_COLLISION_CHECKER_H
+
+#include <cstddef>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+namespace cmvr {
+
+struct CollisionPair {
+    std::string first;
+    std::string second;
+};
+
+struct SelfCollisionOptions {
+    std::vector<CollisionPair> ignored_pairs;
+};
+
+struct CollisionObjectPose {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    std::size_t geometry_index{0};
+    Eigen::Vector3d position{Eigen::Vector3d::Zero()};
+    Eigen::Quaterniond orientation{Eigen::Quaterniond::Identity()};
+    double bounding_radius_m{0.0};
+};
+
+struct CollisionGeometrySnapshot {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    std::vector<CollisionObjectPose, Eigen::aligned_allocator<CollisionObjectPose>> objects;
+};
+
+struct SelfCollisionResult {
+    bool valid{false};
+    bool in_collision{false};
+    double minimum_distance_m{0.0};
+    std::string first;
+    std::string second;
+    std::string error;
+};
+
+// Instances cache Pinocchio work data and are not thread-safe.
+class SelfCollisionChecker {
+public:
+    SelfCollisionChecker();
+    ~SelfCollisionChecker();
+
+    SelfCollisionChecker(SelfCollisionChecker&&) noexcept;
+    SelfCollisionChecker& operator=(SelfCollisionChecker&&) noexcept;
+
+    SelfCollisionChecker(const SelfCollisionChecker&) = delete;
+    SelfCollisionChecker& operator=(const SelfCollisionChecker&) = delete;
+
+    bool init(const std::string& urdf_path,
+              const std::vector<std::string>& active_joint_names,
+              const SelfCollisionOptions& options,
+              std::string* error = nullptr);
+
+    bool makeSnapshot(const std::vector<double>& joint_positions,
+                      CollisionGeometrySnapshot* snapshot,
+                      std::string* error = nullptr);
+
+    SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot);
+    SelfCollisionResult check(const std::vector<double>& joint_positions);
+
+    bool initialized() const;
+    std::size_t dof() const;
+    std::size_t activePairCount() const;
+    const std::vector<std::string>& jointNames() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> impl_;
+};
+
+} // namespace cmvr
+
+#endif // CMVR_ES_SELF_COLLISION_CHECKER_H

cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp

@@ -0,0 +1,100 @@
+#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+namespace cmvr {
+namespace {
+
+void setError(std::string* error, const std::string& message)
+{
+    if (error) {
+        *error = message;
+    }
+}
+
+double rotationAngle(const Eigen::Quaterniond& first,
+                     const Eigen::Quaterniond& second)
+{
+    const double dot = std::clamp(
+        std::abs(first.normalized().dot(second.normalized())), 0.0, 1.0);
+    return 2.0 * std::acos(dot);
+}
+
+} // namespace
+
+bool DistanceSamplingPolicy::configure(const DistanceSamplingOptions& options,
+                                       std::string* error)
+{
+    if (!std::isfinite(options.max_geometry_displacement_m) ||
+        options.max_geometry_displacement_m <= 0.0) {
+        setError(error, "max_geometry_displacement_m must be finite and positive");
+        return false;
+    }
+    if (!std::isfinite(options.max_check_period_s) ||
+        options.max_check_period_s <= 0.0) {
+        setError(error, "max_check_period_s must be finite and positive");
+        return false;
+    }
+    options_ = options;
+    configured_ = true;
+    reset();
+    if (error) {
+        error->clear();
+    }
+    return true;
+}
+
+bool DistanceSamplingPolicy::shouldCheck(const CollisionGeometrySnapshot& current,
+                                         const Clock::time_point now) const
+{
+    if (!configured_ || !has_baseline_) {
+        return true;
+    }
+    const double elapsed_s = std::chrono::duration<double>(now - last_check_time_).count();
+    if (elapsed_s >= options_.max_check_period_s) {
+        return true;
+    }
+    return displacementSinceLastCheck(current) >= options_.max_geometry_displacement_m;
+}
+
+void DistanceSamplingPolicy::markChecked(const CollisionGeometrySnapshot& current,
+                                         const Clock::time_point now)
+{
+    last_checked_ = current;
+    last_check_time_ = now;
+    has_baseline_ = true;
+}
+
+void DistanceSamplingPolicy::reset()
+{
+    last_checked_.objects.clear();
+    last_check_time_ = Clock::time_point{};
+    has_baseline_ = false;
+}
+
+double DistanceSamplingPolicy::displacementSinceLastCheck(
+    const CollisionGeometrySnapshot& current) const
+{
+    if (!has_baseline_ || current.objects.size() != last_checked_.objects.size()) {
+        return std::numeric_limits<double>::infinity();
+    }
+
+    double maximum_displacement = 0.0;
+    for (std::size_t index = 0; index < current.objects.size(); ++index) {
+        const auto& previous = last_checked_.objects[index];
+        const auto& now = current.objects[index];
+        if (previous.geometry_index != now.geometry_index) {
+            return std::numeric_limits<double>::infinity();
+        }
+        const double translation = (now.position - previous.position).norm();
+        const double radius = std::max(previous.bounding_radius_m, now.bounding_radius_m);
+        const double swept_distance =
+            translation + radius * rotationAngle(previous.orientation, now.orientation);
+        maximum_displacement = std::max(maximum_displacement, swept_distance);
+    }
+    return maximum_displacement;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp

@@ -0,0 +1,403 @@
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+#include <algorithm>
+#include <cmath>
+#include <filesystem>
+#include <limits>
+#include <set>
+#include <sstream>
+#include <unordered_set>
+#include <utility>
+
+#include <pinocchio/algorithm/geometry.hpp>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/collision/distance.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+
+namespace cmvr {
+namespace {
+
+using LinkPairKey = std::pair<std::string, std::string>;
+
+LinkPairKey canonicalPair(std::string first, std::string second)
+{
+    if (second < first) {
+        std::swap(first, second);
+    }
+    return {std::move(first), std::move(second)};
+}
+
+void setError(std::string* error, const std::string& message)
+{
+    if (error) {
+        *error = message;
+    }
+}
+
+} // namespace
+
+class SelfCollisionChecker::Impl {
+public:
+    bool init(const std::string& urdf_path,
+              const std::vector<std::string>& active_joint_names,
+              const SelfCollisionOptions& options,
+              std::string* error)
+    {
+        reset();
+        if (urdf_path.empty()) {
+            setError(error, "URDF path is empty");
+            return false;
+        }
+        if (!std::filesystem::is_regular_file(urdf_path)) {
+            setError(error, "URDF file does not exist: " + urdf_path);
+            return false;
+        }
+        if (active_joint_names.empty()) {
+            setError(error, "Active joint list is empty");
+            return false;
+        }
+
+        try {
+            pinocchio::urdf::buildModel(urdf_path, model_);
+            pinocchio::urdf::buildGeom(
+                model_, urdf_path, pinocchio::COLLISION, geometry_model_);
+        } catch (const std::exception& exception) {
+            setError(error, "Failed to load collision URDF: " + std::string(exception.what()));
+            reset();
+            return false;
+        }
+
+        if (geometry_model_.ngeoms == 0) {
+            setError(error, "URDF contains no collision geometry: " + urdf_path);
+            reset();
+            return false;
+        }
+
+        std::unordered_set<pinocchio::JointIndex> active_joint_ids;
+        std::unordered_set<std::string> unique_joint_names;
+        joint_names_.reserve(active_joint_names.size());
+        joint_q_indices_.reserve(active_joint_names.size());
+        for (const auto& joint_name : active_joint_names) {
+            if (joint_name.empty() || !unique_joint_names.insert(joint_name).second) {
+                setError(error, "Active joint names must be non-empty and unique");
+                reset();
+                return false;
+            }
+            if (!model_.existJointName(joint_name)) {
+                setError(error, "Joint not found in URDF: " + joint_name);
+                reset();
+                return false;
+            }
+            const pinocchio::JointIndex joint_id = model_.getJointId(joint_name);
+            const auto& joint = model_.joints[joint_id];
+            if (joint.nq() != 1) {
+                setError(error, "Only one-DoF active joints are supported: " + joint_name);
+                reset();
+                return false;
+            }
+            active_joint_ids.insert(joint_id);
+            joint_names_.push_back(joint_name);
+            joint_q_indices_.push_back(joint.idx_q());
+        }
+
+        geometry_link_names_.resize(geometry_model_.ngeoms);
+        std::unordered_set<std::string> selected_link_names;
+        for (pinocchio::GeomIndex geometry_id = 0;
+             geometry_id < geometry_model_.ngeoms;
+             ++geometry_id) {
+            auto& geometry = geometry_model_.geometryObjects[geometry_id];
+            const std::string link_name = geometry.parentFrame < model_.frames.size()
+                                              ? model_.frames[geometry.parentFrame].name
+                                              : geometry.name;
+            geometry_link_names_[geometry_id] = link_name;
+
+            const bool is_static = geometry.parentJoint == 0;
+            const bool belongs_to_active_arm = active_joint_ids.count(geometry.parentJoint) != 0;
+            if (!is_static && !belongs_to_active_arm) {
+                continue;
+            }
+
+            if (!geometry.geometry) {
+                setError(error, "Collision geometry is null for link: " + link_name);
+                reset();
+                return false;
+            }
+            geometry.geometry->computeLocalAABB();
+            selected_geometry_indices_.push_back(geometry_id);
+            selected_link_names.insert(link_name);
+        }
+
+        if (selected_geometry_indices_.size() < 2) {
+            setError(error, "Fewer than two collision geometries remain after arm filtering");
+            reset();
+            return false;
+        }
+
+        std::set<LinkPairKey> ignored_pairs;
+        for (const auto& pair : options.ignored_pairs) {
+            if (pair.first.empty() || pair.second.empty() || pair.first == pair.second) {
+                setError(error, "Ignored collision pairs require two different non-empty links");
+                reset();
+                return false;
+            }
+            if (!selected_link_names.count(pair.first) || !selected_link_names.count(pair.second)) {
+                setError(error,
+                         "Ignored collision pair references an inactive or unknown link: " +
+                             pair.first + ", " + pair.second);
+                reset();
+                return false;
+            }
+            ignored_pairs.insert(canonicalPair(pair.first, pair.second));
+        }
+
+        geometry_model_.removeAllCollisionPairs();
+        for (std::size_t first_index = 0;
+             first_index < selected_geometry_indices_.size();
+             ++first_index) {
+            const auto first_geometry_id = selected_geometry_indices_[first_index];
+            const auto& first_geometry = geometry_model_.geometryObjects[first_geometry_id];
+            for (std::size_t second_index = first_index + 1;
+                 second_index < selected_geometry_indices_.size();
+                 ++second_index) {
+                const auto second_geometry_id = selected_geometry_indices_[second_index];
+                const auto& second_geometry = geometry_model_.geometryObjects[second_geometry_id];
+
+                if (first_geometry.parentJoint == second_geometry.parentJoint) {
+                    continue;
+                }
+                if (model_.parents[first_geometry.parentJoint] == second_geometry.parentJoint ||
+                    model_.parents[second_geometry.parentJoint] == first_geometry.parentJoint) {
+                    continue;
+                }
+
+                const auto link_pair = canonicalPair(
+                    geometry_link_names_[first_geometry_id],
+                    geometry_link_names_[second_geometry_id]);
+                if (ignored_pairs.count(link_pair)) {
+                    continue;
+                }
+                geometry_model_.addCollisionPair(
+                    pinocchio::CollisionPair(first_geometry_id, second_geometry_id));
+            }
+        }
+
+        if (geometry_model_.collisionPairs.empty()) {
+            setError(error, "No active collision pairs remain after filtering");
+            reset();
+            return false;
+        }
+
+        data_ = std::make_unique<pinocchio::Data>(model_);
+        geometry_data_ = std::make_unique<pinocchio::GeometryData>(geometry_model_);
+        for (auto& request : geometry_data_->distanceRequests) {
+            request.enable_signed_distance = true;
+        }
+        neutral_q_ = pinocchio::neutral(model_);
+        initialized_ = true;
+        if (error) {
+            error->clear();
+        }
+        return true;
+    }
+
+    bool makeSnapshot(const std::vector<double>& joint_positions,
+                      CollisionGeometrySnapshot* snapshot,
+                      std::string* error)
+    {
+        if (!initialized_) {
+            setError(error, "SelfCollisionChecker is not initialized");
+            return false;
+        }
+        if (!snapshot) {
+            setError(error, "Collision snapshot output is null");
+            return false;
+        }
+        if (joint_positions.size() != joint_names_.size()) {
+            std::ostringstream stream;
+            stream << "Joint position size mismatch: expected " << joint_names_.size()
+                   << ", got " << joint_positions.size();
+            setError(error, stream.str());
+            return false;
+        }
+
+        Eigen::VectorXd q = neutral_q_;
+        for (std::size_t index = 0; index < joint_positions.size(); ++index) {
+            if (!std::isfinite(joint_positions[index])) {
+                setError(error, "Joint position contains a non-finite value: " + joint_names_[index]);
+                return false;
+            }
+            q[joint_q_indices_[index]] = joint_positions[index];
+        }
+
+        try {
+            pinocchio::updateGeometryPlacements(
+                model_, *data_, geometry_model_, *geometry_data_, q);
+        } catch (const std::exception& exception) {
+            setError(error, "Failed to update collision geometry: " + std::string(exception.what()));
+            return false;
+        }
+
+        snapshot->objects.clear();
+        snapshot->objects.reserve(selected_geometry_indices_.size());
+        for (const auto geometry_id : selected_geometry_indices_) {
+            const auto& placement = geometry_data_->oMg[geometry_id];
+            const auto& geometry = geometry_model_.geometryObjects[geometry_id];
+            CollisionObjectPose pose;
+            pose.geometry_index = geometry_id;
+            pose.position = placement.translation();
+            pose.orientation = Eigen::Quaterniond(placement.rotation()).normalized();
+            pose.bounding_radius_m = std::max(0.0, geometry.geometry->aabb_radius);
+            snapshot->objects.push_back(std::move(pose));
+        }
+        if (error) {
+            error->clear();
+        }
+        return true;
+    }
+
+    SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot)
+    {
+        SelfCollisionResult result;
+        if (!initialized_) {
+            result.error = "SelfCollisionChecker is not initialized";
+            return result;
+        }
+        if (snapshot.objects.size() != selected_geometry_indices_.size()) {
+            result.error = "Collision snapshot size does not match initialized geometry";
+            return result;
+        }
+
+        for (std::size_t index = 0; index < snapshot.objects.size(); ++index) {
+            const auto& pose = snapshot.objects[index];
+            if (pose.geometry_index != selected_geometry_indices_[index] ||
+                pose.geometry_index >= geometry_data_->oMg.size()) {
+                result.error = "Collision snapshot geometry order is invalid";
+                return result;
+            }
+            if (!pose.position.allFinite() || !pose.orientation.coeffs().allFinite() ||
+                pose.orientation.norm() <= std::numeric_limits<double>::epsilon()) {
+                result.error = "Collision snapshot contains an invalid pose";
+                return result;
+            }
+            geometry_data_->oMg[pose.geometry_index] = pinocchio::SE3(
+                pose.orientation.normalized().toRotationMatrix(), pose.position);
+        }
+
+        try {
+            const std::size_t pair_index =
+                pinocchio::computeDistances(geometry_model_, *geometry_data_);
+            if (pair_index >= geometry_model_.collisionPairs.size()) {
+                result.error = "Collision distance computation returned no active pair";
+                return result;
+            }
+            const auto& pair = geometry_model_.collisionPairs[pair_index];
+            result.minimum_distance_m = geometry_data_->distanceResults[pair_index].min_distance;
+            result.first = geometry_link_names_[pair.first];
+            result.second = geometry_link_names_[pair.second];
+            result.in_collision = result.minimum_distance_m <= 0.0;
+            result.valid = std::isfinite(result.minimum_distance_m);
+            if (!result.valid) {
+                result.error = "Collision distance is not finite";
+            }
+        } catch (const std::exception& exception) {
+            result.error = "Collision distance computation failed: " + std::string(exception.what());
+        }
+        return result;
+    }
+
+    SelfCollisionResult check(const std::vector<double>& joint_positions)
+    {
+        CollisionGeometrySnapshot snapshot;
+        std::string error;
+        if (!makeSnapshot(joint_positions, &snapshot, &error)) {
+            SelfCollisionResult result;
+            result.error = std::move(error);
+            return result;
+        }
+        return check(snapshot);
+    }
+
+    void reset()
+    {
+        initialized_ = false;
+        joint_names_.clear();
+        joint_q_indices_.clear();
+        selected_geometry_indices_.clear();
+        geometry_link_names_.clear();
+        geometry_data_.reset();
+        data_.reset();
+        model_ = pinocchio::Model{};
+        geometry_model_ = pinocchio::GeometryModel{};
+        neutral_q_.resize(0);
+    }
+
+    bool initialized_{false};
+    std::vector<std::string> joint_names_;
+    std::vector<int> joint_q_indices_;
+    std::vector<pinocchio::GeomIndex> selected_geometry_indices_;
+    std::vector<std::string> geometry_link_names_;
+    pinocchio::Model model_;
+    pinocchio::GeometryModel geometry_model_;
+    std::unique_ptr<pinocchio::Data> data_;
+    std::unique_ptr<pinocchio::GeometryData> geometry_data_;
+    Eigen::VectorXd neutral_q_;
+};
+
+SelfCollisionChecker::SelfCollisionChecker()
+    : impl_(std::make_unique<Impl>())
+{
+}
+
+SelfCollisionChecker::~SelfCollisionChecker() = default;
+SelfCollisionChecker::SelfCollisionChecker(SelfCollisionChecker&&) noexcept = default;
+SelfCollisionChecker& SelfCollisionChecker::operator=(SelfCollisionChecker&&) noexcept = default;
+
+bool SelfCollisionChecker::init(const std::string& urdf_path,
+                                const std::vector<std::string>& active_joint_names,
+                                const SelfCollisionOptions& options,
+                                std::string* error)
+{
+    return impl_->init(urdf_path, active_joint_names, options, error);
+}
+
+bool SelfCollisionChecker::makeSnapshot(const std::vector<double>& joint_positions,
+                                        CollisionGeometrySnapshot* snapshot,
+                                        std::string* error)
+{
+    return impl_->makeSnapshot(joint_positions, snapshot, error);
+}
+
+SelfCollisionResult SelfCollisionChecker::check(const CollisionGeometrySnapshot& snapshot)
+{
+    return impl_->check(snapshot);
+}
+
+SelfCollisionResult SelfCollisionChecker::check(const std::vector<double>& joint_positions)
+{
+    return impl_->check(joint_positions);
+}
+
+bool SelfCollisionChecker::initialized() const
+{
+    return impl_->initialized_;
+}
+
+std::size_t SelfCollisionChecker::dof() const
+{
+    return impl_->joint_names_.size();
+}
+
+std::size_t SelfCollisionChecker::activePairCount() const
+{
+    return impl_->geometry_model_.collisionPairs.size();
+}
+
+const std::vector<std::string>& SelfCollisionChecker::jointNames() const
+{
+    return impl_->joint_names_;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp

@@ -0,0 +1,236 @@
+#include <chrono>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h"
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+namespace cmvr {
+namespace {
+
+const std::vector<std::string> kRightArmJoints{
+    "R_SHOULDER_P",
+    "R_SHOULDER_R",
+    "R_SHOULDER_Y",
+    "R_ELBOW_R",
+    "R_WRIST_P",
+    "R_WRIST_Y",
+    "R_WRIST_R",
+};
+
+const std::vector<std::string> kGen2RightArmJoints{
+    "right_arm_J1",
+    "right_arm_J2",
+    "right_arm_J3",
+    "right_arm_J4",
+    "right_arm_J5",
+    "right_arm_J6",
+    "right_arm_J7",
+};
+
+const std::vector<double> kGen2SetupPose{
+    0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0,
+};
+
+const std::vector<double> kGen2WarningPose{
+    2.45028525340088,
+    0.413065394330014,
+    -1.78610031118294,
+    2.3232081721811,
+    -2.96828882895788,
+    -1.59350098130002,
+    0.582912411114367,
+};
+
+const std::vector<double> kGen2StopPose{
+    2.13758633436379,
+    1.61835160165575,
+    -2.3836142221041,
+    0.964538527544213,
+    -0.00382525077004825,
+    1.74586899135531,
+    -0.336868659266887,
+};
+
+const std::vector<double> kGen2CollisionPose{
+    -0.42656969579233,
+    1.41426471041774,
+    -2.67949400419915,
+    2.45814854129954,
+    -2.35907388079205,
+    1.14125209449898,
+    1.53232912981414,
+};
+
+const std::vector<double> kGen2TorsoCollisionPose{
+    1.57607137794121,
+    2.06613762981425,
+    -1.76915077905899,
+    0.959251437141443,
+    -0.725973209527894,
+    1.79390262120717,
+    0.2223354372144,
+};
+
+std::string collisionUrdfPath()
+{
+    return std::string(CMVR_ES_SOURCE_DIR) +
+           "/model/xiaoyan_description/dual_arm_collision.urdf";
+}
+
+std::string gen2CollisionUrdfPath()
+{
+    return std::string(CMVR_ES_SOURCE_DIR) +
+           "/model/gen2/collision/robot_collision.urdf";
+}
+
+SelfCollisionOptions gen2CollisionOptions()
+{
+    SelfCollisionOptions options;
+    options.ignored_pairs.push_back({"arm_link_5_2", "arm_link_7_2"});
+    options.ignored_pairs.push_back({"body_link", "arm_link_2_2"});
+    return options;
+}
+
+CollisionGeometrySnapshot singleObjectSnapshot(double x,
+                                                double angle,
+                                                double radius)
+{
+    CollisionGeometrySnapshot snapshot;
+    CollisionObjectPose pose;
+    pose.geometry_index = 1;
+    pose.position = Eigen::Vector3d(x, 0.0, 0.0);
+    pose.orientation = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
+    pose.bounding_radius_m = radius;
+    snapshot.objects.push_back(pose);
+    return snapshot;
+}
+
+TEST(SelfCollisionCheckerTest, LoadsRightArmFromDualArmUrdf)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error;
+    EXPECT_EQ(checker.dof(), 7U);
+    EXPECT_GT(checker.activePairCount(), 0U);
+
+    CollisionGeometrySnapshot snapshot;
+    ASSERT_TRUE(checker.makeSnapshot(std::vector<double>(7, 0.0), &snapshot, &error)) << error;
+    EXPECT_EQ(snapshot.objects.size(), 11U);
+
+    const SelfCollisionResult result = checker.check(snapshot);
+    ASSERT_TRUE(result.valid) << result.error;
+    EXPECT_TRUE(result.first.rfind("L_", 0) != 0);
+    EXPECT_TRUE(result.second.rfind("L_", 0) != 0);
+}
+
+TEST(SelfCollisionCheckerTest, RejectsWrongJointVectorSize)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error;
+
+    CollisionGeometrySnapshot snapshot;
+    EXPECT_FALSE(checker.makeSnapshot(std::vector<double>(6, 0.0), &snapshot, &error));
+    EXPECT_NE(error.find("size mismatch"), std::string::npos);
+}
+
+TEST(SelfCollisionCheckerTest, RemovesConfiguredIgnoredPair)
+{
+    SelfCollisionChecker baseline;
+    SelfCollisionChecker filtered;
+    std::string error;
+    ASSERT_TRUE(baseline.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error;
+
+    SelfCollisionOptions options;
+    options.ignored_pairs.push_back({"base_link", "R_ELBOW_R_S"});
+    ASSERT_TRUE(filtered.init(collisionUrdfPath(), kRightArmJoints, options, &error)) << error;
+    EXPECT_EQ(filtered.activePairCount() + 1, baseline.activePairCount());
+}
+
+TEST(SelfCollisionCheckerTest, LoadsGen2RightArmCollisionModel)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(
+        gen2CollisionUrdfPath(),
+        kGen2RightArmJoints,
+        gen2CollisionOptions(),
+        &error)) << error;
+    EXPECT_EQ(checker.dof(), 7U);
+    EXPECT_EQ(checker.activePairCount(), 19U);
+
+    CollisionGeometrySnapshot snapshot;
+    ASSERT_TRUE(checker.makeSnapshot(kGen2SetupPose, &snapshot, &error)) << error;
+    EXPECT_EQ(snapshot.objects.size(), 8U);
+
+    const SelfCollisionResult setup_result = checker.check(snapshot);
+    ASSERT_TRUE(setup_result.valid) << setup_result.error;
+    EXPECT_FALSE(setup_result.in_collision);
+    EXPECT_GT(setup_result.minimum_distance_m, 0.02);
+}
+
+TEST(SelfCollisionCheckerTest, ClassifiesGen2SafetyDistances)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(
+        gen2CollisionUrdfPath(),
+        kGen2RightArmJoints,
+        gen2CollisionOptions(),
+        &error)) << error;
+
+    const SelfCollisionResult warning_result = checker.check(kGen2WarningPose);
+    ASSERT_TRUE(warning_result.valid) << warning_result.error;
+    EXPECT_FALSE(warning_result.in_collision);
+    EXPECT_GT(warning_result.minimum_distance_m, 0.005);
+    EXPECT_LE(warning_result.minimum_distance_m, 0.02);
+
+    const SelfCollisionResult stop_result = checker.check(kGen2StopPose);
+    ASSERT_TRUE(stop_result.valid) << stop_result.error;
+    EXPECT_FALSE(stop_result.in_collision);
+    EXPECT_GT(stop_result.minimum_distance_m, 0.0);
+    EXPECT_LE(stop_result.minimum_distance_m, 0.005);
+
+    const SelfCollisionResult collision_result = checker.check(kGen2CollisionPose);
+    ASSERT_TRUE(collision_result.valid) << collision_result.error;
+    EXPECT_TRUE(collision_result.in_collision);
+    EXPECT_LE(collision_result.minimum_distance_m, 0.0);
+
+    const SelfCollisionResult torso_result =
+        checker.check(kGen2TorsoCollisionPose);
+    ASSERT_TRUE(torso_result.valid) << torso_result.error;
+    EXPECT_TRUE(torso_result.in_collision);
+    EXPECT_LE(torso_result.minimum_distance_m, 0.0);
+    EXPECT_TRUE(torso_result.first == "body_link" ||
+                torso_result.second == "body_link");
+}
+
+TEST(DistanceSamplingPolicyTest, SamplesByAccumulatedGeometryDisplacement)
+{
+    DistanceSamplingPolicy policy;
+    DistanceSamplingOptions options;
+    options.max_geometry_displacement_m = 0.002;
+    options.max_check_period_s = 0.01;
+    std::string error;
+    ASSERT_TRUE(policy.configure(options, &error)) << error;
+
+    const auto start = DistanceSamplingPolicy::Clock::now();
+    const auto initial = singleObjectSnapshot(0.0, 0.0, 0.2);
+    EXPECT_TRUE(policy.shouldCheck(initial, start));
+    policy.markChecked(initial, start);
+
+    EXPECT_FALSE(policy.shouldCheck(
+        singleObjectSnapshot(0.001, 0.0, 0.2), start + std::chrono::milliseconds(1)));
+    EXPECT_TRUE(policy.shouldCheck(
+        singleObjectSnapshot(0.0021, 0.0, 0.2), start + std::chrono::milliseconds(2)));
+    EXPECT_TRUE(policy.shouldCheck(
+        singleObjectSnapshot(0.0, 0.011, 0.2), start + std::chrono::milliseconds(2)));
+    EXPECT_TRUE(policy.shouldCheck(
+        initial, start + std::chrono::milliseconds(10)));
+}
+
+} // namespace
+} // namespace cmvr

cmvr-es/algorithms/controllers/CMakeLists.txt

@@ -12,7 +12,7 @@ add_subdirectory(arm_control)
 #)  其他动态库类似
 file(GLOB SRC
         ${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/pbvs/src/pbvs_controller.cpp
 
 )
 
@@ -58,3 +58,13 @@ target_link_libraries(controller PUBLIC
 add_library(cmvr_es::algorithms::controller ALIAS controller)
 
 install(TARGETS controller LIBRARY DESTINATION lib)
+
+add_executable(pbvs_controller_test
+        pbvs/src/pbvs_controller_test.cpp
+)
+
+target_link_libraries(pbvs_controller_test PRIVATE
+        cmvr_es::algorithms::controller
+        gtest
+        gtest_main
+)

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

@@ -25,6 +25,7 @@ public:
         double stop_command_velocity_norm{1e-3};
         double stop_measured_velocity_norm{1e-2};
         double stop_acceleration{0.5};
+        double stop_timeout_s{2.0};
     };
 
     using ReadStateCallback = std::function<bool(std::vector<double>& q, std::vector<double>& qd)>;
@@ -45,14 +46,20 @@ 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(); }
+    double stopTimeoutS() const { return config_.stop_timeout_s; }
     CartesianVelocity getCommandTwistBase() const;
 
 private:
     void ensureWorkerStarted_();
     void workerLoop_();
+    void requestStop_(std::optional<double> acceleration = std::nullopt);
+    void abortCommand_();
     void sendZero_();
 
     static double velocityNorm_(const std::vector<double>& velocity);
@@ -73,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

@@ -29,6 +29,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController:
     if (config.stop_acceleration <= 0.0) {
         config.stop_acceleration = defaults.stop_acceleration;
     }
+    if (!std::isfinite(config.stop_timeout_s) || config.stop_timeout_s <= 0.0) {
+        config.stop_timeout_s = defaults.stop_timeout_s;
+    }
     return config;
 }
 
@@ -58,12 +61,27 @@ 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()) && busy_.exchange(true)) {
+    if (!worker_ || !worker_->joinable()) {
+        if (busy_.exchange(true)) {
             return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
         }
+    }
 
     ensureWorkerStarted_();
 
@@ -73,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();
@@ -103,15 +124,13 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        target_twist_ = {};
-        target_frame_ = FrameType::Base;
-        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
-        command_active_ = true;
-        ++command_version_;
+    // A completed speedL command leaves the worker thread joinable but idle.
+    // Do not turn that idle worker into a new command just because a caller
+    // requests a stop during a task transition.
+    if (!busy_.load()) {
+        return Result::success();
     }
-    cv_.notify_all();
+    requestStop_(acceleration);
     return Result::success();
 }
 
@@ -137,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_()
@@ -161,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, [&]() {
@@ -189,42 +215,55 @@ 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 (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
-                    std::lock_guard<std::mutex> lock(mutex_);
-                    command_active_ = false;
-                    sendZero_();
-                    busy_.store(false);
+            if (!planner_->updateSpeedLLimits(options)) {
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
                     break;
                 }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
-                sendZero_();
-                busy_.store(false);
-                return;
+                requestStop_();
+                continue;
             }
 
             std::vector<double> q_now;
             std::vector<double> qd_now;
             if (!read_state_(q_now, qd_now)) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
-                sendZero_();
-                busy_.store(false);
-                return;
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             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 << ", "
                                 << target_twist.vz << ", " << target_twist.wx << ", "
                                 << target_twist.wy << ", " << target_twist.wz
                                 << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
-                sendZero_();
-                busy_.store(false);
-                return;
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             JointVelocityCommand velocity_command;
@@ -233,9 +272,25 @@ void CartesianVelocityController::workerLoop_()
             if (!send_result.ok()) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
                                 << send_result.message;
-                sendZero_();
-                busy_.store(false);
-                return;
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
+                    break;
+                }
+                requestStop_();
+                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 &&
@@ -243,10 +298,14 @@ void CartesianVelocityController::workerLoop_()
                 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;
             }
 
@@ -260,6 +319,34 @@ void CartesianVelocityController::workerLoop_()
     busy_.store(false);
 }
 
+void CartesianVelocityController::requestStop_(const std::optional<double> acceleration)
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_twist_ = {};
+        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_;
+    }
+    cv_.notify_all();
+}
+
+void CartesianVelocityController::abortCommand_()
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        command_active_ = false;
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+        sendZero_();
+        busy_.store(false);
+    }
+}
+
 void CartesianVelocityController::sendZero_()
 {
     if (!send_velocity_) {

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

@@ -0,0 +1,131 @@
+#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_reversal.store(options.continuous_linear_reversal.value_or(false));
+        applied_jerk.store(options.linear_jerk.value_or(10.0)); return true;
+    }
+    bool captureSpeedLReference(const std::vector<double>&, const CartesianVelocity& target,
+                               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};
+    std::atomic<bool> applied_reversal{false};
+};
+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.continuous_linear_reversal = true;
+    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);
+    EXPECT_TRUE(planner->applied_reversal.load());
+    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/controllers/pbvs/include/pbvs_controller.h

@@ -0,0 +1,453 @@
+#pragma once
+
+#ifndef CMVR_PBVS_CONTROLLER_H
+#define CMVR_PBVS_CONTROLLER_H
+
+#include <array>
+
+#include <Eigen/Dense>
+
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr {
+
+/**
+ * @brief 基于相对 3D 位姿的 PBVS 控制器。
+ *
+ * 坐标系：
+ *
+ *   G : Screen Tag 坐标系，同时作为屏幕参考坐标系
+ *   P : TCP / 触控点坐标系
+ *
+ * 输入：
+ *
+ *   ^G T_P_des : TCP 相对于 Screen Tag 的目标位姿
+ *   ^G T_P_cur : TCP 相对于 Screen Tag 的当前位姿
+ *
+ * 输出：
+ *
+ *   ^G V_P =
+ *
+ *   [ vx ]
+ *   [ vy ]
+ *   [ vz ]
+ *   [ wx ]
+ *   [ wy ]
+ *   [ wz ]
+ *
+ * 即 TCP 在 Screen Tag 坐标系 G 下表达的 6D Cartesian Twist。
+ *
+ * 位置误差：
+ *
+ *   e_p = p_des - p_cur
+ *
+ * 姿态误差：
+ *
+ *   R_err = R_des * R_cur^T
+ *
+ *   e_R = Log(R_err)^vee
+ *
+ * 控制律：
+ *
+ *   v = Kp * e_p
+ *   w = Kr * e_R
+ *
+ * 本类只负责视觉伺服 6D Twist 计算，不负责：
+ *
+ * - AprilTag 检测
+ * - RobotArm
+ * - G -> Base 坐标转换
+ * - IK
+ * - speedL 下发
+ */
+class PbvsController {
+public:
+    enum class ComputeStatus {
+        OK = 0,
+        TARGET_NOT_SET,
+        INVALID_INPUT,
+        INVALID_DT,
+        INVALID_TARGET,
+        INVALID_CURRENT_POSE
+    };
+
+    struct Output {
+        // 当前位置误差，单位 m
+        Eigen::Vector3d position_error_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 当前姿态误差 rotation-vector，单位 rad
+        Eigen::Vector3d rotation_error_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // PBVS 计算得到的原始线速度，单位 m/s
+        Eigen::Vector3d raw_linear_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // PBVS 计算得到的原始角速度，单位 rad/s
+        Eigen::Vector3d raw_angular_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 经过限幅 / 加速度 / 滤波后的线速度
+        Eigen::Vector3d linear_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 经过限幅 / 加速度 / 滤波后的角速度
+        Eigen::Vector3d angular_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 可直接取出的 CartesianVelocity。
+        //
+        // 注意：
+        // 这里仍然是在 G / ScreenTag frame 下表达。
+        device::CartesianVelocity twist_G{};
+
+        bool position_reached{false};
+        bool orientation_reached{false};
+        bool reached{false};
+
+        bool valid{false};
+    };
+
+public:
+    PbvsController();
+
+    /**
+     * @brief 设置完整目标位姿。
+     *
+     * @param T_G_P_des TCP(P) 相对于 ScreenTag(G) 的目标位姿。
+     */
+    bool setTargetPose(
+        const Eigen::Matrix4d& T_G_P_des);
+
+    /**
+     * @brief 使用目标位置 + 目标姿态设置期望位姿。
+     */
+    bool setTargetPose(
+        const Eigen::Vector3d& position_G,
+        const Eigen::Matrix3d& rotation_G_P);
+
+    /**
+     * @brief 当前是否已经设置有效目标。
+     */
+    bool hasTarget() const {
+        return target_valid_;
+    }
+
+    /**
+     * @brief 获取目标位姿。
+     */
+    const Eigen::Matrix4d& targetPose() const {
+        return T_G_P_des_;
+    }
+
+    /**
+     * @brief 根据当前 TCP 位姿计算 PBVS 速度命令。
+     *
+     * @param T_G_P_cur 当前 TCP 相对于 Screen Tag 的位姿。
+     * @param dt        控制周期，单位 s。
+     * @param output    输出结果。
+     *
+     * @return 成功返回 true。
+     */
+    bool compute(
+        const Eigen::Matrix4d& T_G_P_cur,
+        double dt,
+        Output& output);
+
+    /**
+     * @brief 便捷接口，只输出 CartesianVelocity。
+     *
+     * 注意输出仍然是 G frame。
+     */
+    bool compute(
+        const Eigen::Matrix4d& T_G_P_cur,
+        double dt,
+        device::CartesianVelocity& twist_G_out);
+
+    /**
+     * @brief 设置位置比例增益。
+     *
+     * x/y/z 分别控制。
+     */
+    void setPositionGain(
+        const Eigen::Vector3d& kp);
+
+    /**
+     * @brief 设置姿态比例增益。
+     *
+     * rx/ry/rz 分别控制。
+     */
+    void setRotationGain(
+        const Eigen::Vector3d& kr);
+
+    /**
+     * @brief 设置六维速度上限。
+     *
+     * [vx vy vz wx wy wz]
+     *
+     * 前三维 m/s；
+     * 后三维 rad/s。
+     */
+    void setVelocityLimit6(
+        const std::array<double, 6>& vmax6);
+
+    /**
+     * @brief 设置六维加速度限制。
+     *
+     * 前三维 m/s^2；
+     * 后三维 rad/s^2。
+     *
+     * <= 0 表示对应维度不限制。
+     */
+    void setAccelerationLimit6(
+        const std::array<double, 6>& amax6);
+
+    /**
+     * @brief 设置六维误差阈值。
+     *
+     * [x y z rx ry rz]
+     *
+     * 前三维 m；
+     * 后三维 rad。
+     */
+    void setTolerance6(
+        const std::array<double, 6>& tolerance6);
+
+    /**
+     * @brief 设置一阶低通 alpha。
+     *
+     * alpha = 1：
+     * 不滤波。
+     *
+     * 0 < alpha < 1：
+     *
+     * cmd =
+     * alpha * current
+     * +
+     * (1-alpha) * previous
+     */
+    void setTwistFilterAlpha(double alpha);
+
+    /**
+     * @brief 是否启用每个轴。
+     *
+     * 默认 6DoF 全部开启。
+     *
+     * 例如以后如果不希望控制 yaw：
+     *
+     * enabled[5] = false;
+     */
+    void setAxisEnabled(
+        const std::array<bool, 6>& enabled);
+
+    /**
+     * @brief 清空速度历史，但保留目标。
+     */
+    void resetTwistCommandState();
+
+    /**
+     * @brief 清空整个 PBVS 状态和目标。
+     */
+    void reset();
+
+    ComputeStatus lastComputeStatus() const {
+        return last_compute_status_;
+    }
+
+    static const char* statusToString(
+        ComputeStatus status);
+
+    const Eigen::Vector3d& lastPositionError() const {
+        return last_position_error_G_;
+    }
+
+    const Eigen::Vector3d& lastRotationError() const {
+        return last_rotation_error_G_;
+    }
+
+    const Eigen::Vector3d& positionGain() const {
+        return kp_position_;
+    }
+
+    const Eigen::Vector3d& rotationGain() const {
+        return kp_rotation_;
+    }
+
+    const std::array<double, 6>& velocityLimit6() const {
+        return vmax6_;
+    }
+
+    const std::array<double, 6>& accelerationLimit6() const {
+        return amax6_;
+    }
+
+    const std::array<double, 6>& tolerance6() const {
+        return tolerance6_;
+    }
+
+    double twistFilterAlpha() const {
+        return twist_lpf_alpha_;
+    }
+
+    const Eigen::Matrix<double, 6, 1>&
+    lastTwistCommandG() const {
+        return last_twist_cmd_G_;
+    }
+
+    bool lastReached() const {
+        return last_reached_;
+    }
+
+private:
+    static bool isFiniteTransform(
+        const Eigen::Matrix4d& T);
+
+    static bool hasValidBottomRow(
+        const Eigen::Matrix4d& T,
+        double tolerance = 1e-6);
+
+    static bool isValidTransform(
+        const Eigen::Matrix4d& T);
+
+    /**
+     * @brief 将可能有微小数值误差的旋转矩阵投影到 SO(3)。
+     */
+    static Eigen::Matrix3d projectToSO3(
+        const Eigen::Matrix3d& R);
+
+    /**
+     * @brief 计算空间旋转误差，在 G frame 表达。
+     *
+     * R_err = R_des * R_cur^T
+     *
+     * e_R = Log(R_err)^vee
+     */
+    static Eigen::Vector3d rotationError(
+        const Eigen::Matrix3d& R_des,
+        const Eigen::Matrix3d& R_cur);
+
+    static double clampValue(
+        double value,
+        double lower,
+        double upper);
+
+    static device::CartesianVelocity
+    toCartesianVelocity(
+        const Eigen::Matrix<double, 6, 1>& twist);
+
+private:
+    // ---------------- target ----------------
+
+    Eigen::Matrix4d T_G_P_des_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    bool target_valid_{false};
+
+    // ---------------- gains ----------------
+
+    Eigen::Vector3d kp_position_{
+        2.0,
+        2.0,
+        1.5
+    };
+
+    Eigen::Vector3d kp_rotation_{
+        1.5,
+        1.5,
+        1.5
+    };
+
+    // ---------------- velocity limits ----------------
+
+    //
+    // [vx vy vz wx wy wz]
+    //
+    std::array<double, 6> vmax6_{{
+        0.10,
+        0.10,
+        0.05,
+        0.50,
+        0.50,
+        0.50
+    }};
+
+    // ---------------- acceleration limits ----------------
+
+    std::array<double, 6> amax6_{{
+        0.50,
+        0.50,
+        0.30,
+        2.0,
+        2.0,
+        2.0
+    }};
+
+    // ---------------- tolerance ----------------
+
+    std::array<double, 6> tolerance6_{{
+        0.0015,  // x  1.5 mm
+        0.0015,  // y  1.5 mm
+        0.0020,  // z  2.0 mm
+
+        0.05,    // rx ~2.9 deg
+        0.05,    // ry
+        0.05     // rz
+    }};
+
+    // ---------------- axis enable ----------------
+
+    std::array<bool, 6> axis_enabled_{{
+        true,
+        true,
+        true,
+        true,
+        true,
+        true
+    }};
+
+    // ---------------- LPF ----------------
+
+    double twist_lpf_alpha_{1.0};
+
+    // ---------------- command history ----------------
+
+    Eigen::Matrix<double, 6, 1>
+        previous_twist_cmd_G_{
+            Eigen::Matrix<double, 6, 1>::Zero()
+        };
+
+    bool has_previous_twist_{false};
+
+    // ---------------- last output ----------------
+
+    Eigen::Vector3d last_position_error_G_{
+        Eigen::Vector3d::Zero()
+    };
+
+    Eigen::Vector3d last_rotation_error_G_{
+        Eigen::Vector3d::Zero()
+    };
+
+    Eigen::Matrix<double, 6, 1>
+        last_twist_cmd_G_{
+            Eigen::Matrix<double, 6, 1>::Zero()
+        };
+
+    bool last_reached_{false};
+
+    ComputeStatus last_compute_status_{
+        ComputeStatus::TARGET_NOT_SET
+    };
+};
+
+} // namespace cmvr
+
+#endif // CMVR_PBVS_CONTROLLER_H

cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp

@@ -0,0 +1,774 @@
+#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Geometry>
+#include <Eigen/SVD>
+
+namespace cmvr {
+
+PbvsController::PbvsController()
+{
+    resetTwistCommandState();
+}
+
+bool PbvsController::setTargetPose(
+    const Eigen::Matrix4d& T_G_P_des)
+{
+    if (!isValidTransform(T_G_P_des)) {
+        target_valid_ = false;
+        last_compute_status_ =
+            ComputeStatus::INVALID_TARGET;
+        return false;
+    }
+
+    T_G_P_des_ = T_G_P_des;
+
+    //
+    // 视觉位姿可能有微小数值误差。
+    // 强制把 rotation 投影到 SO(3)。
+    //
+    T_G_P_des_.block<3, 3>(0, 0) =
+        projectToSO3(
+            T_G_P_des.block<3, 3>(0, 0));
+
+    target_valid_ = true;
+    last_compute_status_ =
+        ComputeStatus::OK;
+
+    return true;
+}
+
+bool PbvsController::setTargetPose(
+    const Eigen::Vector3d& position_G,
+    const Eigen::Matrix3d& rotation_G_P)
+{
+    if (!position_G.allFinite() ||
+        !rotation_G_P.allFinite()) {
+
+        target_valid_ = false;
+        last_compute_status_ =
+            ComputeStatus::INVALID_TARGET;
+
+        return false;
+    }
+
+    Eigen::Matrix4d T =
+        Eigen::Matrix4d::Identity();
+
+    T.block<3, 3>(0, 0) =
+        projectToSO3(rotation_G_P);
+
+    T.block<3, 1>(0, 3) =
+        position_G;
+
+    return setTargetPose(T);
+}
+
+bool PbvsController::compute(
+    const Eigen::Matrix4d& T_G_P_cur,
+    const double dt,
+    Output& output)
+{
+    output = Output{};
+
+    last_reached_ = false;
+    last_position_error_G_.setZero();
+    last_rotation_error_G_.setZero();
+    last_twist_cmd_G_.setZero();
+
+    if (!target_valid_) {
+        last_compute_status_ =
+            ComputeStatus::TARGET_NOT_SET;
+
+        return false;
+    }
+
+    if (!std::isfinite(dt) ||
+        dt <= 0.0) {
+
+        last_compute_status_ =
+            ComputeStatus::INVALID_DT;
+
+        return false;
+    }
+
+    if (!isValidTransform(T_G_P_cur)) {
+        last_compute_status_ =
+            ComputeStatus::INVALID_CURRENT_POSE;
+
+        return false;
+    }
+
+    // =====================================================
+    // 1. Current pose
+    // =====================================================
+
+    const Eigen::Vector3d p_cur_G =
+        T_G_P_cur.block<3, 1>(0, 3);
+
+    const Eigen::Matrix3d R_cur_G_P =
+        projectToSO3(
+            T_G_P_cur.block<3, 3>(0, 0));
+
+    // =====================================================
+    // 2. Desired pose
+    // =====================================================
+
+    const Eigen::Vector3d p_des_G =
+        T_G_P_des_.block<3, 1>(0, 3);
+
+    const Eigen::Matrix3d R_des_G_P =
+        T_G_P_des_.block<3, 3>(0, 0);
+
+    // =====================================================
+    // 3. Position error
+    //
+    // e_p = p_des - p_cur
+    //
+    // expressed in G frame.
+    // =====================================================
+
+    Eigen::Vector3d e_pos_G =
+        p_des_G -
+        p_cur_G;
+
+    // =====================================================
+    // 4. Rotation error
+    //
+    // R_err =
+    //     R_des * R_cur^T
+    //
+    // e_R =
+    //     Log(R_err)^vee
+    //
+    // e_R is also expressed in G frame.
+    // =====================================================
+
+    Eigen::Vector3d e_rot_G =
+        rotationError(
+            R_des_G_P,
+            R_cur_G_P);
+
+    if (!e_pos_G.allFinite() ||
+        !e_rot_G.allFinite()) {
+
+        last_compute_status_ =
+            ComputeStatus::INVALID_INPUT;
+
+        return false;
+    }
+
+    // =====================================================
+    // 5. Disabled axes
+    // =====================================================
+
+    for (int i = 0; i < 3; ++i) {
+        if (!axis_enabled_[i]) {
+            e_pos_G[i] = 0.0;
+        }
+
+        if (!axis_enabled_[i + 3]) {
+            e_rot_G[i] = 0.0;
+        }
+    }
+
+    last_position_error_G_ =
+        e_pos_G;
+
+    last_rotation_error_G_ =
+        e_rot_G;
+
+    output.position_error_G =
+        e_pos_G;
+
+    output.rotation_error_G =
+        e_rot_G;
+
+    // =====================================================
+    // 6. Reached check
+    // =====================================================
+
+    bool position_reached = true;
+    bool orientation_reached = true;
+
+    for (int i = 0; i < 3; ++i) {
+
+        if (axis_enabled_[i] &&
+            std::abs(e_pos_G[i]) >
+                tolerance6_[i]) {
+
+            position_reached = false;
+        }
+
+        if (axis_enabled_[i + 3] &&
+            std::abs(e_rot_G[i]) >
+                tolerance6_[i + 3]) {
+
+            orientation_reached = false;
+        }
+    }
+
+    const bool reached =
+        position_reached &&
+        orientation_reached;
+
+    output.position_reached =
+        position_reached;
+
+    output.orientation_reached =
+        orientation_reached;
+
+    output.reached =
+        reached;
+
+    last_reached_ =
+        reached;
+
+    // =====================================================
+    // 7. PBVS P control
+    //
+    // v = Kp * e_pos
+    //
+    // w = Kr * e_rot
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_raw_G =
+            Eigen::Matrix<double, 6, 1>::Zero();
+
+    twist_raw_G.head<3>() =
+        kp_position_.cwiseProduct(
+            e_pos_G);
+
+    twist_raw_G.tail<3>() =
+        kp_rotation_.cwiseProduct(
+            e_rot_G);
+
+    // =====================================================
+    // 8. Dead zone
+    //
+    // 某个轴已经进入误差阈值，则这个轴不再主动运动。
+    // =====================================================
+
+    for (int i = 0; i < 6; ++i) {
+
+        if (!axis_enabled_[i]) {
+            twist_raw_G[i] = 0.0;
+            continue;
+        }
+
+        const double error_value =
+            i < 3
+                ? e_pos_G[i]
+                : e_rot_G[i - 3];
+
+        if (std::abs(error_value) <=
+            tolerance6_[i]) {
+
+            twist_raw_G[i] = 0.0;
+        }
+    }
+
+    output.raw_linear_velocity_G =
+        twist_raw_G.head<3>();
+
+    output.raw_angular_velocity_G =
+        twist_raw_G.tail<3>();
+
+    // =====================================================
+    // 9. Velocity limits
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_vel_limited =
+            twist_raw_G;
+
+    for (int i = 0; i < 6; ++i) {
+
+        const double vmax =
+            vmax6_[i];
+
+        if (!std::isfinite(vmax) ||
+            vmax <= 0.0) {
+
+            twist_vel_limited[i] = 0.0;
+            continue;
+        }
+
+        twist_vel_limited[i] =
+            clampValue(
+                twist_vel_limited[i],
+                -vmax,
+                vmax);
+    }
+
+    // =====================================================
+    // 10. Reached:
+    //
+    // 进入完整目标阈值后直接输出 0。
+    //
+    // 上层可以随后调用 RobotArm::stopL()。
+    // =====================================================
+
+    if (reached) {
+
+        previous_twist_cmd_G_.setZero();
+        has_previous_twist_ = true;
+
+        last_twist_cmd_G_.setZero();
+
+        output.linear_velocity_G.setZero();
+        output.angular_velocity_G.setZero();
+
+        output.twist_G =
+            device::CartesianVelocity{};
+
+        output.valid = true;
+
+        last_compute_status_ =
+            ComputeStatus::OK;
+
+        return true;
+    }
+
+    // =====================================================
+    // 11. Acceleration limits
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_acc_limited;
+
+    if (!has_previous_twist_) {
+
+        previous_twist_cmd_G_.setZero();
+        has_previous_twist_ = true;
+    }
+
+    twist_acc_limited =
+        previous_twist_cmd_G_;
+
+    for (int i = 0; i < 6; ++i) {
+
+        if (!axis_enabled_[i]) {
+            twist_acc_limited[i] = 0.0;
+            continue;
+        }
+
+        const double amax =
+            amax6_[i];
+
+        // <=0：不做加速度限制
+        if (!std::isfinite(amax) ||
+            amax <= 0.0) {
+
+            twist_acc_limited[i] =
+                twist_vel_limited[i];
+
+            continue;
+        }
+
+        const double dv_max =
+            amax * dt;
+
+        const double dv_des =
+            twist_vel_limited[i]
+            -
+            previous_twist_cmd_G_[i];
+
+        const double dv =
+            clampValue(
+                dv_des,
+                -dv_max,
+                dv_max);
+
+        twist_acc_limited[i] =
+            previous_twist_cmd_G_[i]
+            +
+            dv;
+    }
+
+    // =====================================================
+    // 12. First-order LPF
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_filtered =
+            twist_acc_limited;
+
+    const double alpha =
+        std::clamp(
+            twist_lpf_alpha_,
+            0.0,
+            1.0);
+
+    if (alpha > 0.0 &&
+        alpha < 1.0) {
+
+        twist_filtered =
+            alpha *
+            twist_acc_limited
+            +
+            (1.0 - alpha) *
+            previous_twist_cmd_G_;
+    }
+
+    // =====================================================
+    // 13. Make sure disabled axis is zero
+    // =====================================================
+
+    for (int i = 0; i < 6; ++i) {
+        if (!axis_enabled_[i]) {
+            twist_filtered[i] = 0.0;
+        }
+    }
+
+    // =====================================================
+    // 14. Save state
+    // =====================================================
+
+    previous_twist_cmd_G_ =
+        twist_filtered;
+
+    last_twist_cmd_G_ =
+        twist_filtered;
+
+    // =====================================================
+    // 15. Output
+    // =====================================================
+
+    output.linear_velocity_G =
+        twist_filtered.head<3>();
+
+    output.angular_velocity_G =
+        twist_filtered.tail<3>();
+
+    output.twist_G =
+        toCartesianVelocity(
+            twist_filtered);
+
+    output.valid = true;
+
+    last_compute_status_ =
+        ComputeStatus::OK;
+
+    return true;
+}
+
+bool PbvsController::compute(
+    const Eigen::Matrix4d& T_G_P_cur,
+    const double dt,
+    device::CartesianVelocity& twist_G_out)
+{
+    Output output;
+
+    if (!compute(
+            T_G_P_cur,
+            dt,
+            output)) {
+
+        twist_G_out =
+            device::CartesianVelocity{};
+
+        return false;
+    }
+
+    twist_G_out =
+        output.twist_G;
+
+    return true;
+}
+
+void PbvsController::setPositionGain(
+    const Eigen::Vector3d& kp)
+{
+    for (int i = 0; i < 3; ++i) {
+        if (std::isfinite(kp[i]) &&
+            kp[i] >= 0.0) {
+
+            kp_position_[i] =
+                kp[i];
+        }
+    }
+}
+
+void PbvsController::setRotationGain(
+    const Eigen::Vector3d& kr)
+{
+    for (int i = 0; i < 3; ++i) {
+        if (std::isfinite(kr[i]) &&
+            kr[i] >= 0.0) {
+
+            kp_rotation_[i] =
+                kr[i];
+        }
+    }
+}
+
+void PbvsController::setVelocityLimit6(
+    const std::array<double, 6>& vmax6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(vmax6[i]) &&
+            vmax6[i] >= 0.0) {
+
+            vmax6_[i] =
+                vmax6[i];
+        }
+    }
+}
+
+void PbvsController::setAccelerationLimit6(
+    const std::array<double, 6>& amax6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(amax6[i])) {
+            amax6_[i] =
+                amax6[i];
+        }
+    }
+}
+
+void PbvsController::setTolerance6(
+    const std::array<double, 6>& tolerance6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(tolerance6[i]) &&
+            tolerance6[i] >= 0.0) {
+
+            tolerance6_[i] =
+                tolerance6[i];
+        }
+    }
+}
+
+void PbvsController::setTwistFilterAlpha(
+    const double alpha)
+{
+    if (!std::isfinite(alpha)) {
+        return;
+    }
+
+    twist_lpf_alpha_ =
+        std::clamp(
+            alpha,
+            0.0,
+            1.0);
+}
+
+void PbvsController::setAxisEnabled(
+    const std::array<bool, 6>& enabled)
+{
+    axis_enabled_ =
+        enabled;
+}
+
+void PbvsController::resetTwistCommandState()
+{
+    previous_twist_cmd_G_.setZero();
+    last_twist_cmd_G_.setZero();
+
+    has_previous_twist_ = false;
+}
+
+void PbvsController::reset()
+{
+    T_G_P_des_.setIdentity();
+
+    target_valid_ = false;
+
+    last_position_error_G_.setZero();
+    last_rotation_error_G_.setZero();
+
+    last_reached_ = false;
+
+    resetTwistCommandState();
+
+    last_compute_status_ =
+        ComputeStatus::TARGET_NOT_SET;
+}
+
+const char* PbvsController::statusToString(
+    const ComputeStatus status)
+{
+    switch (status) {
+
+        case ComputeStatus::OK:
+            return "ok";
+
+        case ComputeStatus::TARGET_NOT_SET:
+            return "target_not_set";
+
+        case ComputeStatus::INVALID_INPUT:
+            return "invalid_input";
+
+        case ComputeStatus::INVALID_DT:
+            return "invalid_dt";
+
+        case ComputeStatus::INVALID_TARGET:
+            return "invalid_target";
+
+        case ComputeStatus::INVALID_CURRENT_POSE:
+            return "invalid_current_pose";
+
+        default:
+            return "unknown";
+    }
+}
+
+bool PbvsController::isFiniteTransform(
+    const Eigen::Matrix4d& T)
+{
+    return T.allFinite();
+}
+
+bool PbvsController::hasValidBottomRow(
+    const Eigen::Matrix4d& T,
+    const double tolerance)
+{
+    return
+        std::abs(T(3, 0)) <= tolerance &&
+        std::abs(T(3, 1)) <= tolerance &&
+        std::abs(T(3, 2)) <= tolerance &&
+        std::abs(T(3, 3) - 1.0) <= tolerance;
+}
+
+bool PbvsController::isValidTransform(
+    const Eigen::Matrix4d& T)
+{
+    if (!isFiniteTransform(T)) {
+        return false;
+    }
+
+    if (!hasValidBottomRow(T)) {
+        return false;
+    }
+
+    return true;
+}
+
+Eigen::Matrix3d PbvsController::projectToSO3(
+    const Eigen::Matrix3d& R)
+{
+    if (!R.allFinite()) {
+        return Eigen::Matrix3d::Identity();
+    }
+
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(
+        R,
+        Eigen::ComputeFullU |
+        Eigen::ComputeFullV);
+
+    Eigen::Matrix3d U =
+        svd.matrixU();
+
+    const Eigen::Matrix3d V =
+        svd.matrixV();
+
+    Eigen::Matrix3d R_projected =
+        U * V.transpose();
+
+    //
+    // 确保 det = +1，而不是 reflection。
+    //
+    if (R_projected.determinant() < 0.0) {
+
+        U.col(2) *= -1.0;
+
+        R_projected =
+            U * V.transpose();
+    }
+
+    return R_projected;
+}
+
+Eigen::Vector3d PbvsController::rotationError(
+    const Eigen::Matrix3d& R_des,
+    const Eigen::Matrix3d& R_cur)
+{
+    const Eigen::Matrix3d R_d =
+        projectToSO3(R_des);
+
+    const Eigen::Matrix3d R_c =
+        projectToSO3(R_cur);
+
+    //
+    // 空间旋转误差，在 G frame 表达。
+    //
+    // 当前为 I，目标绕 +Z 旋转 theta：
+    //
+    // R_err = R_des
+    //
+    // 得到 +theta Z，
+    // 因此角速度方向正确。
+    //
+    const Eigen::Matrix3d R_err =
+        R_d *
+        R_c.transpose();
+
+    Eigen::AngleAxisd aa(
+        R_err);
+
+    const double angle =
+        aa.angle();
+
+    if (!std::isfinite(angle) ||
+        std::abs(angle) <= 1e-12) {
+
+        return Eigen::Vector3d::Zero();
+    }
+
+    const Eigen::Vector3d axis =
+        aa.axis();
+
+    if (!axis.allFinite()) {
+        return Eigen::Vector3d::Zero();
+    }
+
+    return axis * angle;
+}
+
+double PbvsController::clampValue(
+    const double value,
+    const double lower,
+    const double upper)
+{
+    return std::max(
+        lower,
+        std::min(
+            value,
+            upper));
+}
+
+device::CartesianVelocity
+PbvsController::toCartesianVelocity(
+    const Eigen::Matrix<double, 6, 1>& twist)
+{
+    device::CartesianVelocity velocity;
+
+    velocity.vx =
+        twist[0];
+
+    velocity.vy =
+        twist[1];
+
+    velocity.vz =
+        twist[2];
+
+    velocity.wx =
+        twist[3];
+
+    velocity.wy =
+        twist[4];
+
+    velocity.wz =
+        twist[5];
+
+    return velocity;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp

@@ -0,0 +1,61 @@
+#include "gtest/gtest.h"
+
+#include <array>
+
+#include <Eigen/Geometry>
+
+#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
+
+namespace {
+
+TEST(PbvsControllerTest, AppliesConfigurationSetters) {
+    cmvr::PbvsController controller;
+
+    const Eigen::Vector3d position_gain(1.0, 2.0, 3.0);
+    const Eigen::Vector3d rotation_gain(4.0, 5.0, 6.0);
+    const std::array<double, 6> vmax{{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}};
+    const std::array<double, 6> amax{{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}};
+    const std::array<double, 6> tolerance{{0.001, 0.002, 0.003, 0.01, 0.02, 0.03}};
+
+    controller.setPositionGain(position_gain);
+    controller.setRotationGain(rotation_gain);
+    controller.setVelocityLimit6(vmax);
+    controller.setAccelerationLimit6(amax);
+    controller.setTolerance6(tolerance);
+    controller.setTwistFilterAlpha(0.75);
+
+    EXPECT_TRUE(controller.positionGain().isApprox(position_gain));
+    EXPECT_TRUE(controller.rotationGain().isApprox(rotation_gain));
+    EXPECT_EQ(controller.velocityLimit6(), vmax);
+    EXPECT_EQ(controller.accelerationLimit6(), amax);
+    EXPECT_EQ(controller.tolerance6(), tolerance);
+    EXPECT_DOUBLE_EQ(controller.twistFilterAlpha(), 0.75);
+}
+
+TEST(PbvsControllerTest, CommandsTowardPositionAndRotationError) {
+    cmvr::PbvsController controller;
+    controller.setPositionGain(Eigen::Vector3d::Ones());
+    controller.setRotationGain(Eigen::Vector3d::Ones());
+    controller.setVelocityLimit6({{1.0, 1.0, 1.0, 1.0, 1.0, 1.0}});
+    controller.setAccelerationLimit6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
+    controller.setTolerance6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
+    controller.setTwistFilterAlpha(1.0);
+
+    Eigen::Matrix4d target = Eigen::Matrix4d::Identity();
+    target.block<3, 3>(0, 0) =
+        Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+    target.block<3, 1>(0, 3) = Eigen::Vector3d(0.1, -0.2, 0.3);
+    ASSERT_TRUE(controller.setTargetPose(target));
+
+    cmvr::PbvsController::Output output;
+    ASSERT_TRUE(controller.compute(Eigen::Matrix4d::Identity(), 0.01, output));
+    ASSERT_TRUE(output.valid);
+    EXPECT_GT(output.linear_velocity_G.x(), 0.0);
+    EXPECT_LT(output.linear_velocity_G.y(), 0.0);
+    EXPECT_GT(output.linear_velocity_G.z(), 0.0);
+    EXPECT_NEAR(output.angular_velocity_G.x(), 0.0, 1e-12);
+    EXPECT_NEAR(output.angular_velocity_G.y(), 0.0, 1e-12);
+    EXPECT_GT(output.angular_velocity_G.z(), 0.0);
+}
+
+} // namespace

cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt

@@ -26,3 +26,14 @@ target_link_libraries(ik_solver PUBLIC
 add_library(cmvr_es::ik_solver ALIAS ik_solver)
 
 install(TARGETS ik_solver LIBRARY DESTINATION lib)
+
+add_executable(pinocchio_qp_ik_solver_test
+        pinocchio/src/pinocchio_qp_ik_solver_test.cpp
+)
+
+target_link_libraries(pinocchio_qp_ik_solver_test PRIVATE
+        cmvr_es::ik_solver
+        cmvr_es::proto
+        gtest
+        gtest_main
+)

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h

@@ -45,6 +45,11 @@ public:
                            std::vector<double>& qdot_out,
                            double qdot_abs_max = std::numeric_limits<double>::infinity()) const override;
 
+    // Projects a secondary joint velocity into the Cartesian task null space.
+    static Eigen::VectorXd projectJointLimitAvoidanceToNullspace(
+        const Eigen::MatrixXd& jacobian_base,
+        const Eigen::VectorXd& qdot_avoid);
+
     /// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
     void setVelocityLimits(const Eigen::VectorXd &qd_max);
     void setAccelerationLimits(const Eigen::VectorXd &qdd_max);

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp

@@ -164,7 +164,9 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
 
     const double margin_ratio = positiveOr(config.margin_ratio(), 0.08);
     const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02);
-    Eigen::VectorXd limited = qdot;
+    // Apply one common scale factor instead of changing individual joints.
+    // Per-joint scaling changes J*qdot and can disturb the Cartesian task.
+    double scale = 1.0;
     for (Eigen::Index i = 0; i < q_chain.size(); ++i) {
         const double lower = joint_pos_lower_limits_[i];
         const double upper = joint_pos_upper_limits_[i];
@@ -174,21 +176,15 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
 
         const double span = upper - lower;
         const double margin = std::max(min_margin_rad, margin_ratio * span);
-        if (limited[i] < 0.0 && q_chain[i] < lower + margin) {
+        if (qdot[i] < 0.0 && q_chain[i] < lower + margin) {
             const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
-            if (q_chain[i] <= lower) {
-                limited[i] = std::max(0.0, limited[i]);
-            }
-        } else if (limited[i] > 0.0 && q_chain[i] > upper - margin) {
+            scale = std::min(scale, ratio);
+        } else if (qdot[i] > 0.0 && q_chain[i] > upper - margin) {
             const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
-            if (q_chain[i] >= upper) {
-                limited[i] = std::min(0.0, limited[i]);
+            scale = std::min(scale, ratio);
         }
     }
-    }
-    return limited;
+    return scale * qdot;
 }
 
 void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp

@@ -13,11 +13,66 @@
 #include <pinocchio/spatial/explog.hpp>
 
 #include <algorithm>   // std::clamp, std::max, std::min
+#include <atomic>
 #include <cmath>       // std::sqrt
+#include <cstdint>
 #include <limits>
+#include <sstream>
 #include <unordered_map>
+#include <Eigen/SVD>
 
 namespace cmvr {
+    namespace {
+
+    Eigen::MatrixXd moorePenrosePseudoInverse(const Eigen::MatrixXd& matrix)
+    {
+        if (matrix.rows() == 0 || matrix.cols() == 0 || !matrix.allFinite()) {
+            return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
+        }
+
+        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+            matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
+        if (svd.info() != Eigen::Success) {
+            return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
+        }
+
+        const Eigen::VectorXd singular_values = svd.singularValues();
+        const double max_singular = singular_values.size() > 0
+                                        ? singular_values.maxCoeff()
+                                        : 0.0;
+        const double tolerance =
+            std::numeric_limits<double>::epsilon() *
+            static_cast<double>(std::max(matrix.rows(), matrix.cols())) *
+            std::max(1.0, max_singular);
+        Eigen::VectorXd inverse_singular = singular_values;
+        for (Eigen::Index i = 0; i < inverse_singular.size(); ++i) {
+            inverse_singular[i] = singular_values[i] > tolerance
+                                      ? 1.0 / singular_values[i]
+                                      : 0.0;
+        }
+
+        const Eigen::Index rank_dimension = singular_values.size();
+        return svd.matrixV().leftCols(rank_dimension) *
+               inverse_singular.asDiagonal() *
+               svd.matrixU().leftCols(rank_dimension).transpose();
+    }
+
+    std::string vectorToString(const Eigen::VectorXd& value)
+    {
+        std::ostringstream stream;
+        stream << '[';
+        for (Eigen::Index i = 0; i < value.size(); ++i) {
+            if (i > 0) {
+                stream << ' ';
+            }
+            stream << value[i];
+        }
+        stream << ']';
+        return stream.str();
+    }
+
+    } // namespace
+
     using Eigen::Matrix4d;
     using Eigen::VectorXd;
     using Eigen::MatrixXd;
@@ -208,6 +263,24 @@ namespace cmvr {
         }
     }
 
+    Eigen::VectorXd PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
+        const Eigen::MatrixXd& jacobian_base,
+        const Eigen::VectorXd& qdot_avoid)
+    {
+        if (jacobian_base.cols() != qdot_avoid.size() ||
+            jacobian_base.rows() == 0 || jacobian_base.cols() == 0 ||
+            !jacobian_base.allFinite() || !qdot_avoid.allFinite()) {
+            return Eigen::VectorXd::Zero(qdot_avoid.size());
+        }
+
+        const Eigen::MatrixXd jacobian_pinv =
+            moorePenrosePseudoInverse(jacobian_base);
+        const Eigen::MatrixXd nullspace =
+            Eigen::MatrixXd::Identity(jacobian_base.cols(), jacobian_base.cols()) -
+            jacobian_pinv * jacobian_base;
+        return nullspace * qdot_avoid;
+    }
+
     bool PinocchioQpIKSolver::ik(const Matrix4d &target_pose,
                                  std::vector<double> &joints_angle,
                                  bool is_tcp) {
@@ -389,7 +462,7 @@ namespace cmvr {
         const int dof = chain_v_dof_;
         const auto& avoidance = jointLimitPolicy().avoidance();
         const bool use_joint_limit_avoidance =
-            !jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0;
+            !jointLimitsDisabled() && avoidance.enable() && avoidance.gain() > 0.0;
         const int avoidance_rows = use_joint_limit_avoidance ? dof : 0;
 
         MatrixXd cost(6 + dof + avoidance_rows, dof);
@@ -406,7 +479,7 @@ namespace cmvr {
         VectorXd upper(dof);
         const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
         if (use_joint_limit_avoidance) {
-            const VectorXd qdot_avoid =
+            const VectorXd qdot_avoid_raw =
                 cmvr::kinematics::computeJointLimitAvoidanceVelocity(
                     q_chain,
                     joint_pos_lower_limits_,
@@ -415,10 +488,32 @@ namespace cmvr {
                     positiveOr(avoidance.gain(), 0.2),
                     positiveOr(avoidance.margin_ratio(), 0.15),
                     positiveOr(avoidance.max_push(), 0.25));
-            const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05));
+            const Eigen::MatrixXd jacobian_pinv =
+                moorePenrosePseudoInverse(jacobian_base);
+            const bool jacobian_pinv_valid =
+                jacobian_pinv.rows() == dof && jacobian_pinv.cols() == 6 &&
+                jacobian_pinv.allFinite();
+            Eigen::MatrixXd nullspace = MatrixXd::Zero(dof, dof);
+            if (jacobian_pinv_valid) {
+                nullspace = MatrixXd::Identity(dof, dof) -
+                            jacobian_pinv * jacobian_base;
+            }
+            const VectorXd qdot_avoid_null = nullspace * qdot_avoid_raw;
             cost.middleRows(6 + dof, dof) =
-                sqrt_weight * MatrixXd::Identity(dof, dof);
-            target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid;
+                nullspace;
+            target.segment(6 + dof, dof) = qdot_avoid_null;
+
+            static std::atomic<std::uint64_t> avoidance_debug_counter{0};
+            const auto debug_index =
+                avoidance_debug_counter.fetch_add(1, std::memory_order_relaxed);
+            if (debug_index % 1000 == 0) {
+                CMVR_LOG(DEBUG)
+                    << "[PinocchioQpIKSolver][JOINT_LIMIT_AVOIDANCE]"
+                    << " qdot_avoid_raw=" << vectorToString(qdot_avoid_raw)
+                    << " qdot_avoid_null=" << vectorToString(qdot_avoid_null)
+                    << " norm(J*qdot_avoid_null)="
+                    << (jacobian_base * qdot_avoid_null).norm();
+            }
         }
         for (int i = 0; i < dof; ++i) {
             double limit = std::numeric_limits<double>::infinity();
@@ -452,6 +547,35 @@ namespace cmvr {
                     }
                 }
             }
+
+            const auto& soft_limit = jointLimitPolicy().soft_limit();
+            if (!jointLimitsDisabled() && soft_limit.enable() &&
+                joint_pos_lower_limits_.size() == dof &&
+                joint_pos_upper_limits_.size() == dof) {
+                const double q_min = joint_pos_lower_limits_[i];
+                const double q_max = joint_pos_upper_limits_[i];
+                if (std::isfinite(q_min) && std::isfinite(q_max) && q_max > q_min) {
+                    const double span = q_max - q_min;
+                    const double margin = std::max(
+                        positiveOr(soft_limit.min_margin_rad(), 0.02),
+                        positiveOr(soft_limit.margin_ratio(), 0.08) * span);
+                    if (q_chain[i] < q_min + margin) {
+                        const double ratio = std::clamp(
+                            (q_chain[i] - q_min) / margin, 0.0, 1.0);
+                        lower[i] = std::max(lower[i], -limit * ratio);
+                        if (q_chain[i] <= q_min) {
+                            lower[i] = std::max(0.0, lower[i]);
+                        }
+                    } else if (q_chain[i] > q_max - margin) {
+                        const double ratio = std::clamp(
+                            (q_max - q_chain[i]) / margin, 0.0, 1.0);
+                        upper[i] = std::min(upper[i], limit * ratio);
+                        if (q_chain[i] >= q_max) {
+                            upper[i] = std::min(0.0, upper[i]);
+                        }
+                    }
+                }
+            }
         }
 
         QPSolver solver;
@@ -475,7 +599,6 @@ namespace cmvr {
         if (qdot.size() != dof) {
             return false;
         }
-        qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
         qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
         return true;
     }

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp

@@ -0,0 +1,104 @@
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
+
+#include <filesystem>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "common/io/proto_file_io.h"
+#include "cmvr/config/arm_config/arm_config.pb.h"
+
+namespace cmvr {
+namespace {
+
+std::filesystem::path findProjectRoot()
+{
+    std::filesystem::path current = std::filesystem::current_path();
+    while (!current.empty()) {
+        if (std::filesystem::exists(
+                current / "model/xiaoyan_description/dual_arm.urdf")) {
+            return current;
+        }
+        const auto parent = current.parent_path();
+        if (parent == current) {
+            break;
+        }
+        current = parent;
+    }
+    return {};
+}
+
+config::PinocchioQpIKConfig loadQpConfig(const std::filesystem::path& root)
+{
+    config::ArmRootConfig root_config;
+    const auto config_path =
+        root / "cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt";
+    EXPECT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(config_path.string(), &root_config));
+    EXPECT_GT(root_config.arm().robot_arms_size(), 0);
+    auto solver_config =
+        root_config.arm().robot_arms(0).kinematics().pinocchio_qp_ik_solver();
+    solver_config.set_urdf_path(
+        (root / "model/xiaoyan_description/dual_arm.urdf").string());
+    return solver_config;
+}
+
+TEST(PinocchioQpIKSolverTest, JointLimitAvoidanceIsInCartesianNullspace)
+{
+    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
+    jacobian.leftCols(6).setIdentity();
+    jacobian.col(6) << 0.3, -0.2, 0.4, -0.1, 0.25, 0.15;
+    Eigen::VectorXd qdot_avoid(7);
+    qdot_avoid << 0.4, -0.3, 0.2, 0.1, -0.5, 0.6, -0.7;
+
+    const Eigen::VectorXd qdot_null =
+        PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
+            jacobian, qdot_avoid);
+
+    ASSERT_EQ(qdot_null.size(), 7);
+    EXPECT_LT((jacobian * qdot_null).norm(), 1e-12);
+    EXPECT_GT(qdot_null.norm(), 0.0);
+}
+
+TEST(PinocchioQpIKSolverTest, AvoidanceDoesNotDisturbReachableCartesianTwist)
+{
+    const auto root = findProjectRoot();
+    ASSERT_FALSE(root.empty());
+
+    auto disabled_config = loadQpConfig(root);
+    disabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(false);
+    auto enabled_config = disabled_config;
+    enabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(true);
+
+    PinocchioQpIKSolver solver_disabled(disabled_config);
+    PinocchioQpIKSolver solver_enabled(enabled_config);
+    ASSERT_TRUE(solver_disabled.init());
+    ASSERT_TRUE(solver_enabled.init());
+
+    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
+    jacobian.leftCols(6).setIdentity();
+    Eigen::Matrix<double, 6, 1> target_twist;
+    target_twist << 0.15, -0.10, 0.08, 0.05, -0.04, 0.03;
+
+    // The last joint is inside its configured soft-limit margin, while the
+    // first six columns fully span the Cartesian task.
+    const std::vector<double> q_chain = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.56};
+    std::vector<double> qdot_disabled;
+    std::vector<double> qdot_enabled;
+    ASSERT_TRUE(solver_disabled.solveVelocityBase(
+        jacobian, target_twist, q_chain, qdot_disabled, 10.0));
+    ASSERT_TRUE(solver_enabled.solveVelocityBase(
+        jacobian, target_twist, q_chain, qdot_enabled, 10.0));
+
+    const Eigen::Map<const Eigen::VectorXd> qdot_disabled_eigen(
+        qdot_disabled.data(), static_cast<Eigen::Index>(qdot_disabled.size()));
+    const Eigen::Map<const Eigen::VectorXd> qdot_enabled_eigen(
+        qdot_enabled.data(), static_cast<Eigen::Index>(qdot_enabled.size()));
+    const Eigen::VectorXd achieved_disabled = jacobian * qdot_disabled_eigen;
+    const Eigen::VectorXd achieved_enabled = jacobian * qdot_enabled_eigen;
+
+    EXPECT_LT((achieved_enabled - achieved_disabled).norm(), 1e-6);
+    EXPECT_LT((achieved_enabled - target_twist).norm(), 5e-5);
+}
+
+} // namespace
+} // namespace cmvr

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

@@ -14,3 +14,22 @@ target_link_libraries(arm_motion
 add_library(cmvr_es::arm_motion ALIAS arm_motion)
 add_library(cmvr_es::algorithms::arm_motion ALIAS arm_motion)
 install(TARGETS arm_motion LIBRARY DESTINATION lib)
+
+add_executable(toppra_joint_motion_planner_test
+        joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp
+)
+
+target_link_libraries(toppra_joint_motion_planner_test
+        PRIVATE
+        cmvr_es::algorithms::arm_motion
+        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,12 @@ public:
                             FrameType frame) = 0;
 
     virtual bool updateSpeedLAcceleration(double acceleration) = 0;
+    virtual bool updateSpeedLLimits(const SpeedLOptions& options) {
+        return !options.linear_jerk && !options.continuous_linear_reversal.value_or(false) &&
+               updateSpeedLAcceleration(options.acceleration);
+    }
+    virtual bool captureSpeedLReference(const std::vector<double>&,
+                                       const CartesianVelocity&, FrameType, SpeedLReference&) { return false; }
     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,9 +99,16 @@ 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};
+
+    // speedL 是否已经进入停止阶段。
+    // 停止阶段不要每 1 ms 用 measured twist 重新点燃 Cartesian planner。
+    bool speedl_stop_active_{false};
+
     bool speedl_configured_{false};
 };
 

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

@@ -121,13 +121,16 @@ 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();
     speedl_line_check_active_ = false;
     speedl_line_deviation_warned_ = false;
     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;
 }
@@ -894,15 +897,44 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         return false;
     }
 
-    const Eigen::Matrix<double, 6, 1> target_twist = common::math::velocityToVector(target_velocity);
-    const bool is_stop_command = target_twist.squaredNorm() <= 1e-12;
+    const Eigen::Matrix<double, 6, 1> target_twist =
+        common::math::velocityToVector(target_velocity);
+
+    const bool is_stop_command =
+        target_twist.squaredNorm() <= 1e-12;
+
     if (is_stop_command) {
-        twist_limiter_.synchronize(measured_twist_base, dt, true);
-    } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
-        twist_limiter_.initialize(Eigen::Matrix<double, 6, 1>::Zero());
+        if (!speedl_stop_active_) {
+            // 只在 stop 边沿执行一次。
+            //
+            // 非常重要：
+            // 不再调用
+            // twist_limiter_.synchronize(measured_twist_base, dt, true);
+            //
+            // 停止应当从“上一拍已经发送出去的 command twist”
+            // 连续规划到 0，而不是每 1 ms 被 measured twist 重新点燃。
+            speedl_stop_active_ = true;
+
+            twist_limiter_.stop();
         }
-    twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame));
-    speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool);
+    } else {
+        // 收到新的非零 speedL，退出停止状态。
+        speedl_stop_active_ = false;
+
+        // 从静止开始一个新的 speedL command。
+        if (speedl_command_twist_base_.squaredNorm() <= 1e-12 &&
+            (!twist_limiter_.continuousLinearReversalEnabled() || !twist_limiter_.isMoving())) {
+            twist_limiter_.initialize(
+                Eigen::Matrix<double, 6, 1>::Zero());
+        }
+
+        twist_limiter_.setTargetTwist(
+            target_twist,
+            common::math::toPlannerFrame(frame));
+    }
+
+    speedl_command_twist_base_ =
+        twist_limiter_.update(dt, base_R_tool);
     if (!updateAndValidateSpeedLLineDeviation_(q_measured,
                                                is_stop_command,
                                                speedl_command_twist_base_.head<3>())) {
@@ -932,7 +964,11 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         qdot = applyJointAccelerationLimits_(qdot, reference, dt);
     }
     const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
+    // During a stop, the limiter intentionally commands a near-zero residual
+    // twist while the measured arm can still be moving in a different direction.
+    // Direction and speed-ratio checks are not meaningful for that transient.
+    if (!is_stop_command &&
+        !validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
                                                      achieved_twist_base,
                                                      toEigenVector(q_measured),
                                                      qdot)) {
@@ -1216,18 +1252,64 @@ 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);
+    // Apply the command's motion policy even when acceleration/jerk are
+    // unchanged. PBVS streams directions; contact retraction locks an axis.
+    twist_limiter_.setContinuousLinearReversal(options.continuous_linear_reversal.value_or(
+        !speedl_config_.has_continuous_linear_reversal() || speedl_config_.continuous_linear_reversal()));
+    if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9 &&
+        std::abs(speedl_applied_angular_acceleration_ - angular_acceleration) <= 1e-9 &&
+        std::abs(speedl_applied_linear_jerk_ - jerk) <= 1e-9) {
         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,213 @@
+#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));
+    }
+}
+
+TEST_F(PinocchioSpeedLLimits, StreamingAlignmentOverridesGlobalReversalPolicy) {
+    for (const double jerk : {10.0, 30.0}) {
+        SCOPED_TRACE(jerk);
+        limits_.set_continuous_linear_reversal(true);
+        limits_.set_linear_acceleration_max(5);
+        limits_.set_linear_jerk_max(jerk);
+        ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
+        SpeedLOptions options;
+        options.acceleration = .5;
+        CartesianVelocity target;
+        target.vy = .02;
+        sample(target, options, false, 300);
+        ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10);
+
+        // Same acceleration/jerk: the policy override must bypass their cache.
+        options.continuous_linear_reversal = false;
+        double min_speed = .02;
+        double max_error = 0;
+        for (int i = 0; i < 2000; ++i) {
+            const double angle = (i / 20) * .01;
+            target.vx = .02 * std::sin(angle);
+            target.vy = .02 * std::cos(angle);
+            ASSERT_TRUE(planner_->updateSpeedLLimits(options));
+            ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base));
+            const Twist actual = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase());
+            min_speed = std::min(min_speed, actual.head<3>().norm());
+            max_error = std::max(max_error, (actual - common::math::velocityToVector(target)).norm());
+        }
+        EXPECT_NEAR(min_speed, .02, 1e-10);
+        EXPECT_LT(max_error, 1e-10);
+    }
+}
+
+TEST_F(PinocchioSpeedLLimits, ContactReversalStillCrossesZeroAfterStreamingAlignment) {
+    limits_.set_linear_acceleration_max(1);
+    limits_.set_linear_jerk_max(4);
+    ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
+    SpeedLOptions options;
+    options.acceleration = 1;
+    options.continuous_linear_reversal = false;
+    CartesianVelocity target;
+    target.vy = .02;
+    sample(target, options, false, 300);
+    ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10);
+
+    options.continuous_linear_reversal = true;
+    target.vy = -.02;
+    for (int i = 0; i < 100; ++i) {
+        ASSERT_TRUE(planner_->updateSpeedLLimits(options));
+        ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base));
+    }
+    EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-10);
+    ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base));
+    EXPECT_LT(planner_->getSpeedLCommandTwistBase().vy, -.0003);
+}
+} // namespace
+} // namespace cmvr::device

cmvr-es/algorithms/motion_planner/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/arm_motion/joint_motion/joint_motion_planner.h

@@ -1,18 +1,15 @@
 #ifndef CMVR_ES_JOINT_MOTION_PLANNER_H
 #define CMVR_ES_JOINT_MOTION_PLANNER_H
 
+#include <algorithm>
+#include <cmath>
 #include <vector>
 
+#include "common/base/logging/logger.h"
 #include "common/types/arm/arm_types.h"
 
 namespace cmvr::device {
 
-struct JointTrajectorySample {
-    double t{0.0};
-    std::vector<double> position;
-    std::vector<double> velocity;
-};
-
 class JointMotionPlanner {
 public:
     virtual ~JointMotionPlanner() = default;
@@ -23,9 +20,137 @@ public:
                            const JointPositionCommand& target,
                            const MotionOptions& options,
                            double speed_scaling,
-                           std::vector<JointTrajectorySample>& samples) = 0;
+                           JointTrajectory& trajectory) = 0;
+
+    virtual bool planReplay(const std::vector<double>& current_position,
+                            const JointTrajectory& recorded_trajectory,
+                            const MotionOptions& options,
+                            JointTrajectory& replay_trajectory) = 0;
+
+    bool validateJointTrajectory(const JointTrajectory& trajectory,
+                                 std::size_t expected_dof,
+                                 const MotionOptions& limits) const;
 };
 
+inline bool JointMotionPlanner::validateJointTrajectory(
+    const JointTrajectory& trajectory,
+    const std::size_t expected_dof,
+    const MotionOptions& limits) const
+{
+    if (trajectory.size() < 2 || expected_dof == 0) {
+        CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory must contain at least "
+                           "two points and have a non-zero DOF";
+        return false;
+    }
+    if (!std::isfinite(limits.velocity) || limits.velocity <= 0.0 ||
+        !std::isfinite(limits.acceleration) || limits.acceleration <= 0.0) {
+        CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity or acceleration limit is invalid";
+        return false;
+    }
+    if (!limits.joint_velocity_limits.empty() &&
+        limits.joint_velocity_limits.size() != expected_dof) {
+        CMVR_LOG(ERROR) << "[JointMotionPlanner] joint velocity limit count does not match DOF";
+        return false;
+    }
+
+    constexpr double kVelocityTolerance = 1e-6;
+    constexpr double kAccelerationTolerance = 1e-3;
+    double maximum_velocity = 0.0;
+    double maximum_acceleration = 0.0;
+    double maximum_position_velocity = 0.0;
+    double maximum_position_acceleration = 0.0;
+    double maximum_jerk = 0.0;
+    std::vector<double> previous_position_velocity(expected_dof, 0.0);
+    std::vector<double> previous_acceleration(expected_dof, 0.0);
+
+    for (std::size_t i = 0; i < trajectory.size(); ++i) {
+        const auto& point = trajectory[i];
+        if (!std::isfinite(point.time_s) ||
+            point.position.size() != expected_dof ||
+            point.velocity.size() != expected_dof) {
+            CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid trajectory point at index=" << i;
+            return false;
+        }
+
+        double dt = 0.0;
+        if (i > 0) {
+            dt = point.time_s - trajectory[i - 1].time_s;
+            if (!std::isfinite(dt) || dt <= 0.0) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory time is not increasing at index="
+                                << i;
+                return false;
+            }
+        }
+
+        for (std::size_t joint = 0; joint < expected_dof; ++joint) {
+            if (!std::isfinite(point.position[joint]) ||
+                !std::isfinite(point.velocity[joint])) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] non-finite trajectory value at point="
+                                << i << ", joint=" << joint;
+                return false;
+            }
+
+            const double velocity = std::abs(point.velocity[joint]);
+            const double velocity_limit = limits.joint_velocity_limits.empty()
+                                              ? limits.velocity
+                                              : limits.joint_velocity_limits[joint];
+            if (!std::isfinite(velocity_limit) || velocity_limit <= 0.0) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid velocity limit for joint="
+                                << joint;
+                return false;
+            }
+            maximum_velocity = std::max(maximum_velocity, velocity);
+            if (velocity > velocity_limit + kVelocityTolerance) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity limit exceeded at point="
+                                << i << ", joint=" << joint
+                                << ", actual=" << velocity
+                                << ", limit=" << velocity_limit;
+                return false;
+            }
+
+            if (i > 0) {
+                const double position_velocity =
+                    (point.position[joint] - trajectory[i - 1].position[joint]) / dt;
+                const double acceleration =
+                    (point.velocity[joint] - trajectory[i - 1].velocity[joint]) / dt;
+                maximum_position_velocity = std::max(
+                    maximum_position_velocity, std::abs(position_velocity));
+                maximum_acceleration = std::max(
+                    maximum_acceleration, std::abs(acceleration));
+                if (std::abs(acceleration) >
+                    limits.acceleration + kAccelerationTolerance) {
+                    CMVR_LOG(ERROR) << "[JointMotionPlanner] acceleration limit exceeded at point="
+                                    << i << ", joint=" << joint
+                                    << ", actual=" << std::abs(acceleration)
+                                    << ", limit=" << limits.acceleration;
+                    return false;
+                }
+                if (i > 1) {
+                    maximum_position_acceleration = std::max(
+                        maximum_position_acceleration,
+                        std::abs(position_velocity -
+                                 previous_position_velocity[joint]) / dt);
+                    maximum_jerk = std::max(
+                        maximum_jerk,
+                        std::abs(acceleration - previous_acceleration[joint]) / dt);
+                }
+                previous_position_velocity[joint] = position_velocity;
+                previous_acceleration[joint] = acceleration;
+            }
+        }
+    }
+
+    CMVR_LOG(INFO) << "[JointMotionPlanner] trajectory validated"
+                   << ", points=" << trajectory.size()
+                   << ", max_velocity_rad_s=" << maximum_velocity
+                   << ", max_discrete_acceleration_rad_s2=" << maximum_acceleration
+                   << ", max_position_velocity_rad_s=" << maximum_position_velocity
+                   << ", max_position_acceleration_rad_s2="
+                   << maximum_position_acceleration
+                   << ", max_discrete_jerk_rad_s3=" << maximum_jerk;
+    return true;
+}
+
 } // namespace cmvr::device
 
 #endif // CMVR_ES_JOINT_MOTION_PLANNER_H

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h

@@ -21,9 +21,19 @@ public:
                    const JointPositionCommand& target,
                    const MotionOptions& options,
                    double speed_scaling,
-                   std::vector<JointTrajectorySample>& samples) override;
+                   JointTrajectory& trajectory) override;
+
+    bool planReplay(const std::vector<double>& current_position,
+                    const JointTrajectory& recorded_trajectory,
+                    const MotionOptions& options,
+                    JointTrajectory& replay_trajectory) override;
 
 private:
+    bool sampleTrajectory_(
+        const std::shared_ptr<cmvr::JointTrajectoryPlanner>& planner,
+        const cmvr::TrajPtr& raw_trajectory,
+        JointTrajectory& trajectory) const;
+
     std::shared_ptr<cmvr::JointTrajectoryPlanner> planner_;
     cmvr::PathType path_type_{cmvr::PathType::Quintic};
     double sample_period_s_{0.001};

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp

@@ -1,6 +1,9 @@
 #include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h"
 
+#include <cmath>
+
 #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
+#include "common/base/logging/logger.h"
 
 namespace cmvr::device {
 
@@ -39,36 +42,215 @@ bool ToppraJointMotionPlanner::init()
     return true;
 }
 
+bool ToppraJointMotionPlanner::sampleTrajectory_(
+    const std::shared_ptr<cmvr::JointTrajectoryPlanner>& planner,
+    const cmvr::TrajPtr& raw_trajectory,
+    JointTrajectory& trajectory) const
+{
+    const auto raw_samples = planner->sampleTrajectory(
+        raw_trajectory, sample_period_s_);
+    if (raw_samples.size() < 2) {
+        CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] trajectory sampling returned fewer than "
+                           "two points: count="
+                        << raw_samples.size();
+        return false;
+    }
+    trajectory.clear();
+    trajectory.reserve(raw_samples.size());
+    for (std::size_t i = 0; i < raw_samples.size(); ++i) {
+        const auto& sample = raw_samples[i];
+        if (!std::isfinite(sample.t) || !sample.q.allFinite() ||
+            !sample.qd.allFinite()) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] sampled trajectory contains "
+                               "a non-finite value at point="
+                            << i;
+            trajectory.clear();
+            return false;
+        }
+        JointTrajectoryPoint point;
+        point.time_s = sample.t;
+        point.position = toStdVector(sample.q);
+        point.velocity = toStdVector(sample.qd);
+        trajectory.push_back(std::move(point));
+    }
+    return true;
+}
+
 bool ToppraJointMotionPlanner::planMoveJ(const std::vector<double>& start,
                                          const JointPositionCommand& target,
                                          const MotionOptions& options,
                                          const double speed_scaling,
-                                         std::vector<JointTrajectorySample>& samples)
+                                         JointTrajectory& trajectory)
 {
-    samples.clear();
+    trajectory.clear();
     if (!planner_ || start.empty() || start.size() != target.position.size() ||
         options.velocity <= 0.0 || options.acceleration <= 0.0) {
         return false;
     }
 
-    cmvr::TrajPtr trajectory;
+    cmvr::TrajPtr raw_trajectory;
     planner_->setPathType(path_type_);
     planner_->setGridSizes(grid_size_, high_grid_size_);
     planner_->setSymmetricLimits(
         std::vector<double>(start.size(), options.velocity * speed_scaling),
         std::vector<double>(start.size(), options.acceleration));
-    if (!planner_->plan(start, target.position, trajectory)) {
+    if (!planner_->plan(start, target.position, raw_trajectory)) {
         return false;
     }
 
-    const auto raw_samples = planner_->sampleTrajectory(trajectory, sample_period_s_);
-    samples.reserve(raw_samples.size());
-    for (const auto& sample : raw_samples) {
-        JointTrajectorySample dst;
-        dst.t = sample.t;
-        dst.position = toStdVector(sample.q);
-        dst.velocity = toStdVector(sample.qd);
-        samples.push_back(std::move(dst));
+    return sampleTrajectory_(planner_, raw_trajectory, trajectory);
+}
+
+bool ToppraJointMotionPlanner::planReplay(
+    const std::vector<double>& current_position,
+    const JointTrajectory& recorded_trajectory,
+    const MotionOptions& options,
+    JointTrajectory& replay_trajectory)
+{
+    replay_trajectory.clear();
+    if (current_position.empty() || recorded_trajectory.size() < 2 ||
+        options.velocity <= 0.0 ||
+        options.acceleration <= 0.0 || !std::isfinite(options.velocity) ||
+        !std::isfinite(options.acceleration)) {
+        CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid trajectory or options";
+        return false;
+    }
+
+    const std::size_t dof = current_position.size();
+    if (!options.joint_velocity_limits.empty() &&
+        options.joint_velocity_limits.size() != dof) {
+        CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid DOF or joint limits";
+        return false;
+    }
+    for (const double position : current_position) {
+        if (!std::isfinite(position)) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite current position";
+            return false;
+        }
+    }
+    for (std::size_t i = 0; i < recorded_trajectory.size(); ++i) {
+        const auto& point = recorded_trajectory[i];
+        if (!std::isfinite(point.time_s) || point.position.size() != dof ||
+            (i > 0 && point.time_s <= recorded_trajectory[i - 1].time_s)) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid recorded point: "
+                            << i;
+            return false;
+        }
+        for (std::size_t joint = 0; joint < dof; ++joint) {
+            if (!std::isfinite(point.position[joint])) {
+                CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite recorded point: "
+                                << i;
+                return false;
+            }
+        }
+    }
+
+    std::vector<double> velocity_limits = options.joint_velocity_limits;
+    if (velocity_limits.empty()) {
+        velocity_limits.assign(dof, options.velocity);
+    }
+    for (std::size_t joint = 0; joint < velocity_limits.size(); ++joint) {
+        const double limit = velocity_limits[joint];
+        if (!std::isfinite(limit) || limit <= 0.0) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid joint velocity limit";
+            return false;
+        }
+    }
+
+    const double ramp_duration_s = std::max(
+        sample_period_s_, options.velocity / options.acceleration);
+    replay_trajectory.reserve(recorded_trajectory.size() + 2);
+    replay_trajectory.push_back(JointTrajectoryPoint{
+        0.0, current_position, std::vector<double>(dof, 0.0)});
+
+    double replay_time_s = ramp_duration_s;
+    replay_trajectory.push_back(JointTrajectoryPoint{
+        replay_time_s,
+        recorded_trajectory.back().position,
+        std::vector<double>(dof, 0.0)});
+    for (std::size_t i = recorded_trajectory.size() - 1; i > 0; --i) {
+        replay_time_s += recorded_trajectory[i].time_s -
+                         recorded_trajectory[i - 1].time_s;
+        replay_trajectory.push_back(JointTrajectoryPoint{
+            replay_time_s,
+            recorded_trajectory[i - 1].position,
+            std::vector<double>(dof, 0.0)});
+    }
+    replay_time_s += ramp_duration_s;
+    replay_trajectory.push_back(JointTrajectoryPoint{
+        replay_time_s,
+        recorded_trajectory.front().position,
+        std::vector<double>(dof, 0.0)});
+
+    const auto update_velocities = [&] {
+        for (auto& point : replay_trajectory) {
+            std::fill(point.velocity.begin(), point.velocity.end(), 0.0);
+        }
+        for (std::size_t i = 1; i + 1 < replay_trajectory.size(); ++i) {
+            const double dt = replay_trajectory[i + 1].time_s -
+                              replay_trajectory[i - 1].time_s;
+            for (std::size_t joint = 0; joint < dof; ++joint) {
+                replay_trajectory[i].velocity[joint] =
+                    (replay_trajectory[i + 1].position[joint] -
+                     replay_trajectory[i - 1].position[joint]) / dt;
+            }
+        }
+    };
+
+    for (int iteration = 0; iteration < 3; ++iteration) {
+        update_velocities();
+        double required_scale = 1.0;
+        std::vector<double> previous_position_velocity(dof, 0.0);
+        for (std::size_t i = 0; i < replay_trajectory.size(); ++i) {
+            const auto& point = replay_trajectory[i];
+            for (std::size_t joint = 0; joint < dof; ++joint) {
+                required_scale = std::max(
+                    required_scale,
+                    std::abs(point.velocity[joint]) / velocity_limits[joint]);
+                if (i == 0) {
+                    continue;
+                }
+
+                const double dt = point.time_s -
+                                  replay_trajectory[i - 1].time_s;
+                const double position_velocity =
+                    (point.position[joint] -
+                     replay_trajectory[i - 1].position[joint]) / dt;
+                const double acceleration =
+                    (point.velocity[joint] -
+                     replay_trajectory[i - 1].velocity[joint]) / dt;
+                required_scale = std::max(
+                    required_scale,
+                    std::abs(position_velocity) / velocity_limits[joint]);
+                required_scale = std::max(
+                    required_scale,
+                    std::sqrt(std::abs(acceleration) /
+                              options.acceleration));
+                if (i > 1) {
+                    const double position_acceleration =
+                        (position_velocity -
+                         previous_position_velocity[joint]) / dt;
+                    required_scale = std::max(
+                        required_scale,
+                        std::sqrt(std::abs(position_acceleration) /
+                                  options.acceleration));
+                }
+                previous_position_velocity[joint] = position_velocity;
+            }
+        }
+
+        if (required_scale <= 1.0 + 1e-9) {
+            break;
+        }
+        required_scale *= 1.001;
+        for (auto& point : replay_trajectory) {
+            point.time_s *= required_scale;
+        }
+    }
+    update_velocities();
+    if (!validateJointTrajectory(replay_trajectory, dof, options)) {
+        replay_trajectory.clear();
+        return false;
     }
     return true;
 }

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp

@@ -0,0 +1,139 @@
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "joint_motion/toppra/include/toppra_joint_motion_planner.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr std::size_t kDof = 7;
+
+JointTrajectory makeRecordedTrajectory(const std::size_t point_count)
+{
+    JointTrajectory trajectory;
+    trajectory.reserve(point_count);
+    for (std::size_t i = 0; i < point_count; ++i) {
+        const double s = static_cast<double>(i) /
+                         static_cast<double>(point_count - 1);
+        JointTrajectoryPoint point;
+        point.time_s = static_cast<double>(i) * 0.002;
+        point.position = {
+            0.40 * s,
+            -0.25 * s + 0.03 * std::sin(3.141592653589793 * s),
+            0.20 * s * s,
+            0.30 * std::sin(1.5707963267948966 * s),
+            -0.12 * s,
+            0.15 * s,
+            -0.08 * std::sin(3.141592653589793 * s),
+        };
+        point.velocity.assign(kDof, 0.0);
+        trajectory.push_back(std::move(point));
+    }
+    return trajectory;
+}
+
+double maximumPositionError(const std::vector<double>& lhs,
+                            const std::vector<double>& rhs)
+{
+    if (lhs.size() != rhs.size()) {
+        return std::numeric_limits<double>::infinity();
+    }
+    double maximum = 0.0;
+    for (std::size_t i = 0; i < lhs.size(); ++i) {
+        maximum = std::max(maximum, std::abs(lhs[i] - rhs[i]));
+    }
+    return maximum;
+}
+
+TEST(ToppraJointMotionPlannerTest, PlansBoundedReverseReplay)
+{
+    ToppraJointMotionPlanner planner(
+        cmvr::PathType::Quintic, 0.001, 150, 300);
+    ASSERT_TRUE(planner.init());
+
+    const JointTrajectory recorded = makeRecordedTrajectory(300);
+    MotionOptions options;
+    options.velocity = 0.15;
+    options.acceleration = 5.0;
+
+    JointTrajectory replay;
+    ASSERT_TRUE(planner.planReplay(
+        recorded.back().position, recorded, options, replay));
+    ASSERT_EQ(replay.size(), recorded.size() + 2);
+    EXPECT_LT(maximumPositionError(
+                  replay.front().position, recorded.back().position),
+              1e-9);
+    EXPECT_LT(maximumPositionError(
+                  replay.back().position, recorded.front().position),
+              1e-9);
+    for (std::size_t i = 0; i < recorded.size(); ++i) {
+        EXPECT_LT(maximumPositionError(
+                      replay[i + 1].position,
+                      recorded[recorded.size() - 1 - i].position),
+                  1e-9);
+    }
+
+    double maximum_velocity = 0.0;
+    double maximum_acceleration = 0.0;
+    for (std::size_t i = 0; i < replay.size(); ++i) {
+        ASSERT_EQ(replay[i].position.size(), kDof);
+        ASSERT_EQ(replay[i].velocity.size(), kDof);
+        for (std::size_t joint = 0; joint < kDof; ++joint) {
+            maximum_velocity = std::max(
+                maximum_velocity, std::abs(replay[i].velocity[joint]));
+            if (i > 0) {
+                const double dt = replay[i].time_s - replay[i - 1].time_s;
+                ASSERT_GT(dt, 0.0);
+                maximum_acceleration = std::max(
+                    maximum_acceleration,
+                    std::abs(replay[i].velocity[joint] -
+                             replay[i - 1].velocity[joint]) / dt);
+            }
+        }
+    }
+    EXPECT_LE(maximum_velocity, options.velocity + 1e-6);
+    EXPECT_LE(maximum_acceleration, options.acceleration + 1e-3);
+}
+
+TEST(ToppraJointMotionPlannerTest, RejectsNonIncreasingRecordedTime)
+{
+    ToppraJointMotionPlanner planner(
+        cmvr::PathType::Quintic, 0.001, 150, 300);
+    ASSERT_TRUE(planner.init());
+
+    JointTrajectory recorded = makeRecordedTrajectory(10);
+    recorded[5].time_s = recorded[4].time_s;
+    MotionOptions options;
+    options.velocity = 0.15;
+    options.acceleration = 5.0;
+
+    JointTrajectory replay;
+    EXPECT_FALSE(planner.planReplay(
+        recorded.back().position, recorded, options, replay));
+    EXPECT_TRUE(replay.empty());
+}
+
+TEST(ToppraJointMotionPlannerTest, ValidationRejectsInvalidOutputTrajectory)
+{
+    ToppraJointMotionPlanner planner(
+        cmvr::PathType::Quintic, 0.001, 150, 300);
+    MotionOptions options;
+    options.velocity = 0.15;
+    options.acceleration = 5.0;
+
+    JointTrajectory trajectory = makeRecordedTrajectory(10);
+    trajectory[5].velocity[2] = options.velocity + 0.01;
+    EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options));
+
+    trajectory[5].velocity[2] = 0.0;
+    trajectory[5].time_s = trajectory[4].time_s;
+    EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options));
+}
+
+} // namespace
+} // namespace cmvr::device

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

@@ -20,4 +20,30 @@ 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
+        joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp
+)
+
+target_link_libraries(toppra_multi_waypoint_test
+        PRIVATE
+        cmvr_es::base_motion
+        gtest
+        gtest_main
+)
+
+add_executable(s_curve_velocity_planner_stop_test
+        motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp
+)
+
+target_link_libraries(s_curve_velocity_planner_stop_test
+        PRIVATE
+        cmvr_es::base_motion
+        gtest
+        gtest_main
+)

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,11 @@ public:
   void setLinearReverseSwitchPolicy(double cos_threshold,
                                     double switch_speed_threshold);
 
+  // Same-axis reversal uses a signed velocity without resetting acceleration
+  // at zero. Disable for streaming direction tracking (e.g. visual alignment).
+  void setContinuousLinearReversal(bool enabled);
+  bool continuousLinearReversalEnabled() const { return continuous_linear_reversal_; }
+
   /**
    * @brief 初始化当前 twist 状态
    *
@@ -120,7 +126,7 @@ public:
    *
    * 作用：
    * - 更新当前执行方向
-   * - 用测得模长与模长加速度同步两个 planner
+   * - 线速度使用固定轴投影（保留正负号），角速度使用模长
    * - keep_target=true 时：
    *   - 若测量状态仍贴着当前 profile，则保持当前 profile
    *   - 否则由 planner 内部从测量状态重规划到当前目标
@@ -178,6 +184,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

@@ -108,6 +108,25 @@ void CartesianTwistLimiter::setLinearReverseSwitchPolicy(double cos_threshold,
   linear_reverse_switch_speed_threshold_ = std::max(0.0, switch_speed_threshold);
 }
 
+void CartesianTwistLimiter::setContinuousLinearReversal(bool enabled)
+{
+  if (continuous_linear_reversal_ == enabled) return;
+
+  if (!enabled) {
+    const double velocity = linear_norm_planner_.getVelocity();
+    const double acceleration = linear_norm_planner_.getAcceleration();
+    // The streaming policy stores a speed magnitude and a physical direction.
+    // Convert a negative signed state without reversing its physical motion or
+    // dropping its acceleration when a command changes policy mid-retraction.
+    if (velocity < 0.0 || (std::abs(velocity) <= EPSILON && acceleration < 0.0)) {
+      current_linear_dir_base_ = -current_linear_dir_base_;
+      linear_norm_planner_.overwriteState(-velocity, -acceleration, false);
+      prev_measured_linear_norm_ = -prev_measured_linear_norm_;
+    }
+  }
+  continuous_linear_reversal_ = enabled;
+}
+
 void CartesianTwistLimiter::initialize(const Twist& initial_twist)
 {
   restoreNominalConstraints();
@@ -201,7 +220,12 @@ void CartesianTwistLimiter::setTargetTwist(const Twist& target_twist, CartesianF
 
 void CartesianTwistLimiter::stop()
 {
+  // target_twist_input_.setZero();
   target_twist_input_.setZero();
+  target_twist_base_.setZero();
+
+  linear_norm_planner_.setTargetVelocity(0.0);
+  angular_norm_planner_.setTargetVelocity(0.0);
 }
 
 void CartesianTwistLimiter::emergencyStop(double emergency_acceleration,
@@ -244,7 +268,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;
@@ -268,7 +293,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 走
   //   - 否则从测量状态重规划到当前目标
@@ -284,7 +309,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_;
   }
@@ -459,7 +484,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,118 @@
+#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);
+}
+
+TEST(CartesianTwistReversal, SwitchingToStreamingPreservesNegativePhysicalVelocity) {
+    for (const int reverse_ticks : {150, 250, 500}) {
+        SCOPED_TRACE(reverse_ticks);
+        CartesianTwistLimiter reference;
+        reference.setLinearConstraints(1, 3, 10);
+        reference.initialize(y(.08));
+        reference.setTargetTwist(y(-.08), CartesianFrame::Base);
+        for (int i = 0; i < reverse_ticks; ++i) reference.update(dt, Eigen::Matrix3d::Identity());
+        ASSERT_LT(reference.getTwistBase().y(), 0);
+        auto streaming = reference;
+        streaming.setContinuousLinearReversal(false);
+        EXPECT_NEAR((streaming.getTwistBase() - reference.getTwistBase()).norm(), 0, 1e-12);
+        for (int i = 0; i < 500; ++i) {
+            const auto expected = reference.update(dt, Eigen::Matrix3d::Identity());
+            const auto actual = streaming.update(dt, Eigen::Matrix3d::Identity());
+            EXPECT_NEAR((actual - expected).norm(), 0, 1e-9);
+            EXPECT_LE(streaming.getJerkBase().norm(), 10 + 1e-6);
+        }
+    }
+}
+}
+}

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h

@@ -109,37 +109,18 @@ namespace cmvr {
         static void sanitizeVsq(toppra::Vector &v);
 
 
-        // centripetal 弦长（alpha=0.5），生成严格递增 S
+        // Joint-space chord length keeps the parameterization independent of
+        // how densely the same geometric path is sampled.
         static std::vector<toppra::value_type>
-        makeS_centripetal(const std::vector<Eigen::VectorXd> &q) {
+        makeSChordLength(const std::vector<Eigen::VectorXd> &q) {
             const size_t M = q.size();
             std::vector<toppra::value_type> S(M, 0.0);
-            auto chord = [](const Eigen::VectorXd &a, const Eigen::VectorXd &b) {
-                double d = (a - b).norm();
-                return std::pow(std::max(d, 1e-16), 0.5);
-            };
             for (size_t i = 1; i < M; ++i) {
-                S[i] = S[i - 1] + chord(q[i], q[i - 1]);
-                if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12;
+                S[i] = S[i - 1] + (q[i] - q[i - 1]).norm();
             }
             return S;
         }
 
-        // 等距参数（简单稳妥）
-        static inline std::vector<toppra::value_type> makeS_equal(size_t M) {
-            std::vector<toppra::value_type> S(M);
-            for (size_t i = 0; i < M; ++i) S[i] = static_cast<toppra::value_type>(i);
-            return S;
-        }
-
-        // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds
-        static inline void normalize_and_floor_S(std::vector<toppra::value_type> &S, double ds_min = 0.2) {
-            for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0];
-            double L = S.back();
-            if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L;
-            for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min;
-        }
-
         // Catmull–Rom（centripetal）估计结点几何速度 v（端点=0）
         static std::vector<Eigen::VectorXd>
         estimateVelsCatmull(const std::vector<Eigen::VectorXd> &q,
@@ -159,14 +140,16 @@ namespace cmvr {
         // 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]）
         static void clampNodeVels(std::vector<Eigen::VectorXd> &v,
                                   const std::vector<Eigen::VectorXd> &q,
+                                  const std::vector<toppra::value_type> &S,
                                   double k = 1.0) {
             const size_t M = q.size();
             if (M <= 2) return;
             for (size_t i = 1; i + 1 < M; ++i) {
-                double d0 = (q[i] - q[i - 1]).norm();
-                double d1 = (q[i + 1] - q[i]).norm();
-                double d = std::max(std::min(d0, d1), 1e-12);
-                double vmax = k * d;
+                const double ds0 = std::max<double>(S[i] - S[i - 1], 1e-12);
+                const double ds1 = std::max<double>(S[i + 1] - S[i], 1e-12);
+                const double slope0 = (q[i] - q[i - 1]).norm() / ds0;
+                const double slope1 = (q[i + 1] - q[i]).norm() / ds1;
+                const double vmax = k * std::min(slope0, slope1);
                 double n = v[i].norm();
                 if (n > vmax) v[i] *= (vmax / n);
             }

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp

@@ -5,10 +5,117 @@
 #include <toppra/toppra.hpp>
 #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
 
+#include <algorithm>
+#include <cmath>
 #include <fstream>
 #include <iomanip>
 
 namespace cmvr {
+    namespace {
+
+    class TimeScaledTrajectory final : public ITrajectory {
+    public:
+        TimeScaledTrajectory(TrajPtr source, const double scale)
+            : source_(std::move(source)), scale_(scale), source_interval_(source_->timeInterval())
+        {
+        }
+
+        toppra::Bound timeInterval() const override
+        {
+            toppra::Bound interval;
+            interval << source_interval_[0],
+                source_interval_[0] +
+                    (source_interval_[1] - source_interval_[0]) * scale_;
+            return interval;
+        }
+
+        Eigen::VectorXd q(const double t) const override
+        {
+            return source_->q(sourceTime_(t));
+        }
+
+        Eigen::VectorXd qd(const double t) const override
+        {
+            return source_->qd(sourceTime_(t)) / scale_;
+        }
+
+        Eigen::VectorXd qdd(const double t) const override
+        {
+            return source_->qdd(sourceTime_(t)) / (scale_ * scale_);
+        }
+
+    private:
+        double sourceTime_(const double output_time) const
+        {
+            return std::clamp(
+                source_interval_[0] +
+                    (output_time - source_interval_[0]) / scale_,
+                source_interval_[0],
+                source_interval_[1]);
+        }
+
+        TrajPtr source_;
+        double scale_{1.0};
+        toppra::Bound source_interval_;
+    };
+
+    bool enforceSampledLimits(const TrajPtr& source,
+                              const std::vector<double>& velocity_limits,
+                              const std::vector<double>& acceleration_limits,
+                              const std::size_t waypoint_count,
+                              TrajPtr& output)
+    {
+        if (!source || velocity_limits.empty() ||
+            velocity_limits.size() != acceleration_limits.size()) {
+            return false;
+        }
+        const auto interval = source->timeInterval();
+        const double duration = interval[1] - interval[0];
+        if (!std::isfinite(duration) || duration <= 0.0) {
+            return false;
+        }
+
+        const std::size_t time_samples = static_cast<std::size_t>(
+            std::ceil(duration / 0.001)) + 1;
+        const std::size_t path_samples = waypoint_count * 20;
+        const std::size_t sample_count = std::clamp<std::size_t>(
+            std::max({std::size_t{1000}, time_samples, path_samples}),
+            std::size_t{1000},
+            std::size_t{200000});
+
+        double required_scale = 1.0;
+        for (std::size_t sample = 0; sample < sample_count; ++sample) {
+            const double ratio = static_cast<double>(sample) /
+                                 static_cast<double>(sample_count - 1);
+            const double time = interval[0] + duration * ratio;
+            const Eigen::VectorXd velocity = source->qd(time);
+            const Eigen::VectorXd acceleration = source->qdd(time);
+            if (!velocity.allFinite() || !acceleration.allFinite() ||
+                velocity.size() != static_cast<Eigen::Index>(velocity_limits.size()) ||
+                acceleration.size() !=
+                    static_cast<Eigen::Index>(acceleration_limits.size())) {
+                return false;
+            }
+            for (Eigen::Index joint = 0; joint < velocity.size(); ++joint) {
+                const std::size_t index = static_cast<std::size_t>(joint);
+                required_scale = std::max(
+                    required_scale,
+                    std::abs(velocity[joint]) / velocity_limits[index]);
+                required_scale = std::max(
+                    required_scale,
+                    std::sqrt(std::abs(acceleration[joint]) /
+                              acceleration_limits[index]));
+            }
+        }
+
+        constexpr double kNumericalMargin = 1.001;
+        output = std::make_shared<TimeScaledTrajectory>(
+            source, required_scale * kNumericalMargin);
+        return true;
+    }
+
+    } // namespace
+
     // ===== ConstAccelTraj =====
     ConstAccelTraj::ConstAccelTraj(std::shared_ptr<toppra::parametrizer::ConstAccel> p)
         : impl_(std::move(p)) {
@@ -54,22 +161,39 @@ namespace cmvr {
     bool ToppraJointTrajectoryPlanner::plan(const std::vector<std::vector<double>>& waypoints,
                              TrajPtr& traj_out) {
         traj_out.reset();
-        const size_t M = waypoints.size();
-        if (M < 2) return false;
+        if (waypoints.size() < 2) return false;
         const size_t DoF = waypoints.front().size();
-        for (const auto& w : waypoints) if (w.size()!=DoF) return false;
+        if (DoF == 0) return false;
+        for (const auto& w : waypoints) {
+            if (w.size() != DoF) return false;
+            for (const double value : w) {
+                if (!std::isfinite(value)) return false;
+            }
+        }
         if (!ensureLimitsSized(DoF)) return false;
+        for (size_t joint = 0; joint < DoF; ++joint) {
+            if (!std::isfinite(v_max_[joint]) || v_max_[joint] <= 0.0 ||
+                !std::isfinite(a_max_[joint]) || a_max_[joint] <= 0.0) {
+                return false;
+            }
+        }
 
-        // 组装
-        std::vector<Eigen::VectorXd> q; q.reserve(M);
-        for (const auto& w : waypoints)
-            q.emplace_back(Eigen::Map<const Eigen::VectorXd>(w.data(), DoF));
+        std::vector<Eigen::VectorXd> q;
+        q.reserve(waypoints.size());
+        constexpr double kDuplicateDistance = 1e-10;
+        for (const auto& waypoint : waypoints) {
+            Eigen::VectorXd value = Eigen::Map<const Eigen::VectorXd>(
+                waypoint.data(), static_cast<Eigen::Index>(DoF));
+            if (q.empty() || (value - q.back()).norm() > kDuplicateDistance) {
+                q.push_back(std::move(value));
+            }
+        }
+        if (q.size() < 2) return false;
+        const size_t M = q.size();
 
-        // 生成 S
-//        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
-//                                                   : makeS_centripetal(q);
-        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
-                                                   : makeS_equal(M);
+        const std::vector<toppra::value_type> S = M == 2
+            ? std::vector<toppra::value_type>{0.0, 1.0}
+            : makeSChordLength(q);
 
         // 几何路径
         auto path = buildPathUnified(q, S);
@@ -86,8 +210,23 @@ namespace cmvr {
 
         // TOPPRA
         toppra::algorithm::TOPPRA algo{constraints, path};
-        auto solve_once = [&](int N)->bool{
-            algo.setN(N);
+        auto solve_once = [&](const int requested_intervals)->bool{
+            const int segment_count = static_cast<int>(M - 1);
+            const int subdivisions = std::max(
+                1, (requested_intervals + segment_count - 1) / segment_count);
+            toppra::Vector grid(segment_count * subdivisions + 1);
+            Eigen::Index index = 0;
+            for (int segment = 0; segment < segment_count; ++segment) {
+                const double start = S[static_cast<size_t>(segment)];
+                const double length = S[static_cast<size_t>(segment + 1)] - start;
+                for (int subdivision = 0; subdivision < subdivisions; ++subdivision) {
+                    grid[index++] = start + length *
+                        static_cast<double>(subdivision) /
+                        static_cast<double>(subdivisions);
+                }
+            }
+            grid[index] = S.back();
+            algo.setGridpoints(grid);
             algo.solver(std::make_shared<toppra::solver::Seidel>());
             return algo.computePathParametrization(0.0, 0.0) == toppra::ReturnCode::OK;
         };
@@ -99,20 +238,21 @@ namespace cmvr {
         toppra::Vector grid = data.gridpoints;
         toppra::Vector vsq  = data.parametrization;
 
+        TrajPtr candidate;
         auto ca = std::make_shared<toppra::parametrizer::ConstAccel>(path, grid, vsq);
         if (ca->validate()) {
-            traj_out = std::make_shared<ConstAccelTraj>(std::move(ca));
-            return true;
-        }
+            candidate = std::make_shared<ConstAccelTraj>(std::move(ca));
+        } else {
             sanitizeVsq(vsq);
             try {
-            traj_out = std::make_shared<SplineTraj>(path, grid, vsq);
-            (void) traj_out->timeInterval();
-            return true;
+                candidate = std::make_shared<SplineTraj>(path, grid, vsq);
+                (void) candidate->timeInterval();
             } catch (...) {
                 return false;
             }
         }
+        return enforceSampledLimits(candidate, v_max_, a_max_, M, traj_out);
+    }
 
 
     bool ToppraJointTrajectoryPlanner::plan(const std::vector<double>& start_joints,
@@ -225,7 +365,7 @@ namespace cmvr {
             double ds = std::max<double>(S[k+1]-S[k], 1e-12);
             toppra::Matrix seg(2, DoF);
             Eigen::RowVectorXd A1 = ((q[k+1]-q[k])/ds).transpose();
-            Eigen::RowVectorXd A0 = (q[k] - A1.transpose()*S[k]).transpose();
+            Eigen::RowVectorXd A0 = q[k].transpose();
             seg.row(0)=A1; seg.row(1)=A0;
             segs.emplace_back(std::move(seg));
         }
@@ -237,7 +377,7 @@ namespace cmvr {
     ToppraJointTrajectoryPlanner::buildCubicHermiteMulti(const std::vector<Eigen::VectorXd>& q,
                                           const std::vector<toppra::value_type>& S) {
         auto v = estimateVelsCatmull(q, S);
-        clampNodeVels(v, q, /*k=*/1.0);
+        clampNodeVels(v, q, S, /*k=*/1.0);
         toppra::Vectors pos(q.begin(), q.end());
         toppra::Vectors vel(v.begin(), v.end());
         auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, S);
@@ -282,7 +422,7 @@ namespace cmvr {
                                        const std::vector<toppra::value_type>& S) {
         const size_t M = q.size(), DoF = q[0].size();
         auto v = estimateVelsCatmull(q, S);
-        clampNodeVels(v, q, /*k=*/1.0);
+        clampNodeVels(v, q, S, /*k=*/1.0);
         auto a = estimateAccelsSecondDiff(q, S);
 
         toppra::Matrices segs; segs.reserve(M-1);
@@ -301,11 +441,11 @@ namespace cmvr {
             Eigen::VectorXd C5 = (  6.0*dq - (3.0*A1 + 0.5*(a0*ds*ds)) - (3.0*(v1*ds) - 0.5*(a1*ds*ds)) );
 
             toppra::Matrix seg(6, DoF);
-            seg.row(0)=C5.transpose();
-            seg.row(1)=C4.transpose();
-            seg.row(2)=C3.transpose();
-            seg.row(3)=A2.transpose();
-            seg.row(4)=A1.transpose();
+            seg.row(0)=(C5 / std::pow(ds, 5)).transpose();
+            seg.row(1)=(C4 / std::pow(ds, 4)).transpose();
+            seg.row(2)=(C3 / std::pow(ds, 3)).transpose();
+            seg.row(3)=(a0 / 2.0).transpose();
+            seg.row(4)=v0.transpose();
             seg.row(5)=A0.transpose();
             segs.emplace_back(std::move(seg));
         }

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp

@@ -0,0 +1,228 @@
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstddef>
+#include <iostream>
+#include <limits>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
+
+namespace cmvr {
+namespace {
+
+constexpr std::size_t kDof = 7;
+constexpr double kVelocityLimit = 0.15;
+constexpr double kAccelerationLimit = 0.3;
+constexpr double kSamplePeriodS = 0.002;
+
+std::vector<std::vector<double>> makeSmoothWaypoints(const std::size_t count)
+{
+    constexpr double kPi = 3.14159265358979323846;
+    std::vector<std::vector<double>> waypoints;
+    waypoints.reserve(count);
+    for (std::size_t i = 0; i < count; ++i) {
+        const double s = static_cast<double>(i) /
+                         static_cast<double>(count - 1);
+        std::vector<double> q(kDof, 0.0);
+        q[0] = 0.40 * s + 0.03 * std::sin(2.0 * kPi * s);
+        q[1] = -0.25 * s + 0.04 * std::sin(kPi * s);
+        q[2] = 0.20 * s * s;
+        q[3] = 0.30 * std::sin(0.5 * kPi * s);
+        q[4] = -0.12 * s + 0.02 * std::sin(3.0 * kPi * s);
+        q[5] = 0.15 * s;
+        q[6] = -0.08 * std::sin(kPi * s);
+        waypoints.push_back(std::move(q));
+    }
+    return waypoints;
+}
+
+double maxAbs(const Eigen::VectorXd& value)
+{
+    double result = 0.0;
+    for (Eigen::Index i = 0; i < value.size(); ++i) {
+        result = std::max(result, std::abs(value[i]));
+    }
+    return result;
+}
+
+double positionError(const Eigen::VectorXd& actual,
+                     const std::vector<double>& expected)
+{
+    if (actual.size() != static_cast<Eigen::Index>(expected.size())) {
+        return std::numeric_limits<double>::infinity();
+    }
+    double squared_error = 0.0;
+    for (Eigen::Index i = 0; i < actual.size(); ++i) {
+        const double error = actual[i] - expected[static_cast<std::size_t>(i)];
+        squared_error += error * error;
+    }
+    return std::sqrt(squared_error);
+}
+
+struct PlanMetrics {
+    bool success{false};
+    double planning_ms{0.0};
+    double duration_s{0.0};
+    double max_velocity{0.0};
+    double max_acceleration{0.0};
+    double max_waypoint_error{0.0};
+    double start_error{0.0};
+    double end_error{0.0};
+    std::size_t sample_count{0};
+};
+
+PlanMetrics planAndMeasure(const std::vector<std::vector<double>>& waypoints,
+                           const PathType path_type = PathType::Linear)
+{
+    PlanMetrics metrics;
+    ToppraJointTrajectoryPlanner planner(path_type);
+    planner.setSymmetricLimits(
+        std::vector<double>(kDof, kVelocityLimit),
+        std::vector<double>(kDof, kAccelerationLimit));
+    planner.setGridSizes(150, 300);
+
+    TrajPtr trajectory;
+    const auto start = std::chrono::steady_clock::now();
+    metrics.success = planner.plan(waypoints, trajectory);
+    metrics.planning_ms = std::chrono::duration<double, std::milli>(
+        std::chrono::steady_clock::now() - start).count();
+    if (!metrics.success || !trajectory) {
+        return metrics;
+    }
+
+    const auto interval = trajectory->timeInterval();
+    metrics.duration_s = interval[1] - interval[0];
+    const auto samples = planner.sampleTrajectory(trajectory, kSamplePeriodS);
+    metrics.sample_count = samples.size();
+    if (samples.empty()) {
+        metrics.success = false;
+        return metrics;
+    }
+    metrics.start_error = positionError(samples.front().q, waypoints.front());
+    metrics.end_error = positionError(samples.back().q, waypoints.back());
+
+    for (const auto& sample : samples) {
+        if (!std::isfinite(sample.t) || !sample.q.allFinite() ||
+            !sample.qd.allFinite() || !sample.qdd.allFinite()) {
+            metrics.success = false;
+            return metrics;
+        }
+        metrics.max_velocity = std::max(metrics.max_velocity, maxAbs(sample.qd));
+        metrics.max_acceleration = std::max(
+            metrics.max_acceleration, maxAbs(sample.qdd));
+    }
+
+    std::size_t sample_index = 0;
+    for (const auto& waypoint : waypoints) {
+        while (sample_index + 1 < samples.size() &&
+               positionError(samples[sample_index + 1].q, waypoint) <=
+                   positionError(samples[sample_index].q, waypoint)) {
+            ++sample_index;
+        }
+        metrics.max_waypoint_error = std::max(
+            metrics.max_waypoint_error,
+            positionError(samples[sample_index].q, waypoint));
+    }
+    return metrics;
+}
+
+const char* pathTypeName(const PathType path_type)
+{
+    switch (path_type) {
+        case PathType::Linear: return "Linear";
+        case PathType::CubicHermite: return "CubicHermite";
+        case PathType::Quintic: return "Quintic";
+        case PathType::Natural: return "Natural";
+    }
+    return "Unknown";
+}
+
+void printMetrics(const std::size_t waypoint_count, const PlanMetrics& metrics)
+{
+    std::cout << "[ToppraMultiWaypointTest] waypoints=" << waypoint_count
+              << ", success=" << metrics.success
+              << ", planning_ms=" << metrics.planning_ms
+              << ", duration_s=" << metrics.duration_s
+              << ", samples=" << metrics.sample_count
+              << ", max_qd=" << metrics.max_velocity
+              << ", max_qdd=" << metrics.max_acceleration
+              << ", max_waypoint_error=" << metrics.max_waypoint_error
+              << ", start_error=" << metrics.start_error
+              << ", end_error=" << metrics.end_error
+              << std::endl;
+}
+
+TEST(ToppraMultiWaypointTest, SmoothSevenDofPathScalesToThousandsOfWaypoints)
+{
+    double reference_duration_s = 0.0;
+    for (const std::size_t count : {10U, 100U, 300U, 1000U, 3000U}) {
+        const auto metrics = planAndMeasure(makeSmoothWaypoints(count));
+        printMetrics(count, metrics);
+        ASSERT_TRUE(metrics.success) << "waypoint_count=" << count;
+        EXPECT_GT(metrics.duration_s, 0.0) << "waypoint_count=" << count;
+        EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6)
+            << "waypoint_count=" << count;
+        EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5)
+            << "waypoint_count=" << count;
+        EXPECT_LT(metrics.max_waypoint_error, 0.002)
+            << "waypoint_count=" << count;
+        if (reference_duration_s == 0.0) {
+            reference_duration_s = metrics.duration_s;
+        } else {
+            EXPECT_NEAR(metrics.duration_s, reference_duration_s,
+                        reference_duration_s * 0.10)
+                << "waypoint_count=" << count;
+        }
+    }
+}
+
+TEST(ToppraMultiWaypointTest, RepeatedWaypointsRemainPlannable)
+{
+    const auto smooth = makeSmoothWaypoints(300);
+    std::vector<std::vector<double>> repeated;
+    repeated.reserve(smooth.size() * 2);
+    for (const auto& waypoint : smooth) {
+        repeated.push_back(waypoint);
+        repeated.push_back(waypoint);
+    }
+
+    const auto metrics = planAndMeasure(repeated);
+    printMetrics(repeated.size(), metrics);
+    EXPECT_TRUE(metrics.success);
+    if (metrics.success) {
+        EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6);
+        EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5);
+        EXPECT_LT(metrics.max_waypoint_error, 0.002);
+    }
+}
+
+TEST(ToppraMultiWaypointTest, CompareInterpolationModesAtThreeHundredWaypoints)
+{
+    const auto waypoints = makeSmoothWaypoints(300);
+    for (const auto path_type : {
+             PathType::CubicHermite,
+             PathType::Quintic,
+             PathType::Natural}) {
+        const auto metrics = planAndMeasure(waypoints, path_type);
+        std::cout << "[ToppraMultiWaypointTest] path_type="
+                  << pathTypeName(path_type) << std::endl;
+        printMetrics(waypoints.size(), metrics);
+        EXPECT_TRUE(metrics.success) << pathTypeName(path_type);
+        if (metrics.success) {
+            EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6)
+                << pathTypeName(path_type);
+            EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5)
+                << pathTypeName(path_type);
+            EXPECT_LT(metrics.max_waypoint_error, 0.002)
+                << pathTypeName(path_type);
+            EXPECT_LT(metrics.start_error, 1e-9) << pathTypeName(path_type);
+            EXPECT_LT(metrics.end_error, 1e-9) << pathTypeName(path_type);
+        }
+    }
+}
+
+} // namespace
+} // namespace cmvr

cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp

@@ -118,9 +118,25 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity,
                                                    double acceleration)
 {
   const double measured_velocity = clamp(velocity, -max_velocity_, max_velocity_);
-  const double measured_acceleration =
+  double measured_acceleration =
       clamp(acceleration, -max_acceleration_, max_acceleration_);
 
+  // When the target is zero this planner is also used for Cartesian speed
+  // magnitudes. A magnitude is non-negative, while the finite-difference
+  // derivative of the measured magnitude is signed. Feeding a large negative
+  // measured acceleration into a signed 1-D velocity planner can generate a
+  // profile that crosses through zero and becomes negative before returning to
+  // zero. The twist limiter then multiplies that negative "norm" by the
+  // current direction, which reverses and amplifies the Cartesian command.
+  //
+  // For feedback resynchronization during a stop, synchronize the measured
+  // speed only and restart the stop profile with zero scalar acceleration.
+  // This avoids noise-sensitive stop replans and preserves a non-overshooting
+  // deceleration profile for speed-magnitude users.
+  if (std::abs(state_.target_velocity) <= VELOCITY_THRESHOLD) {
+    measured_acceleration = 0.0;
+  }
+
   if (!state_.has_active_profile &&
       std::abs(measured_velocity - state_.target_velocity) <= VELOCITY_THRESHOLD) {
     state_.velocity = state_.target_velocity;

cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp

@@ -0,0 +1,77 @@
+#include <algorithm>
+
+#include <gtest/gtest.h>
+
+#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h"
+
+namespace cmvr {
+namespace {
+
+void finishActiveProfile(SCurveVelocityPlanner1D& planner, double dt)
+{
+  for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
+    planner.update(dt);
+  }
+  ASSERT_FALSE(planner.hasActiveProfile());
+}
+
+TEST(SCurveVelocityPlannerStopTest, FeedbackResyncDoesNotReverseSpeedMagnitude)
+{
+  SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0);
+  constexpr double kDt = 0.001;
+  constexpr double kInitialSpeed = 0.04;
+
+  planner.initialize(kInitialSpeed, 0.0);
+  planner.setTargetVelocity(0.0);
+  finishActiveProfile(planner, kDt);
+
+  // Reproduce the speedL stop feedback case: the command profile has already
+  // reached zero, but the measured TCP still has residual speed. A 1 kHz
+  // finite difference may report a large negative scalar acceleration.
+  planner.synchronizeAndReplan(kInitialSpeed, -3.0);
+
+  double max_speed = planner.getVelocity();
+  double min_speed = planner.getVelocity();
+  for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
+    const double speed = planner.update(kDt);
+    max_speed = std::max(max_speed, speed);
+    min_speed = std::min(min_speed, speed);
+  }
+
+  EXPECT_GE(min_speed, -1e-9);
+  EXPECT_LE(max_speed, kInitialSpeed + 1e-9);
+  EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9);
+}
+
+TEST(SCurveVelocityPlannerStopTest, LargerMeasuredDecelerationDoesNotIncreaseStopSpeed)
+{
+  constexpr double kDt = 0.001;
+  constexpr double kInitialSpeed = 0.04;
+  const double measured_accelerations[] = {-0.5, -1.0, -2.0, -3.0};
+
+  for (const double measured_acceleration : measured_accelerations) {
+    SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0);
+    planner.initialize(kInitialSpeed, 0.0);
+    planner.setTargetVelocity(0.0);
+    finishActiveProfile(planner, kDt);
+
+    planner.synchronizeAndReplan(kInitialSpeed, measured_acceleration);
+
+    double max_speed = planner.getVelocity();
+    double min_speed = planner.getVelocity();
+    for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
+      const double speed = planner.update(kDt);
+      max_speed = std::max(max_speed, speed);
+      min_speed = std::min(min_speed, speed);
+    }
+
+    EXPECT_GE(min_speed, -1e-9) << "measured_acceleration=" << measured_acceleration;
+    EXPECT_LE(max_speed, kInitialSpeed + 1e-9)
+        << "measured_acceleration=" << measured_acceleration;
+    EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9)
+        << "measured_acceleration=" << measured_acceleration;
+  }
+}
+
+} // namespace
+} // namespace cmvr

cmvr-es/algorithms/perception/CMakeLists.txt

@@ -4,6 +4,7 @@ find_package(OpenCV REQUIRED)
 add_library(perception SHARED
         apriltag/src/tag_relative_target_3d.cpp
         apriltag/src/apriltag_perception.cpp
+        apriltag/src/tag_relative_tcp_pose.cpp
 )
 
 target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h

@@ -84,6 +84,10 @@ public:
 
         // Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
         Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
+
+        // Semantic alias used by visualization and downstream consumers:
+        // `T_C_Tag` maps points in this tag frame into camera frame C.
+        const Eigen::Matrix4d& T_C_Tag() const { return T_c_t; }
     };
 
     struct FrameCache {

cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h

@@ -0,0 +1,289 @@
+#pragma once
+
+#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
+#define CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include "algorithms/perception/apriltag/include/apriltag_perception.h"
+
+namespace cmvr::perception {
+
+/**
+ * @brief 根据同一台相机同时观测到的 Screen Tag 和 Hand Tag，
+ *        计算 TCP 相对于 Screen Tag 的位姿。
+ *
+ * 坐标系：
+ *
+ *   C : 固定外部相机坐标系
+ *   G : Screen Tag 坐标系，同时作为屏幕参考坐标系
+ *   H : Hand Tag 坐标系
+ *   P : TCP / 触控点坐标系
+ *
+ * 已知：
+ *
+ *   T_C_G : Screen Tag -> Camera
+ *   T_C_H : Hand Tag   -> Camera
+ *   T_H_P : TCP        -> Hand Tag
+ *
+ * 其中 T_H_P 是外部传入的一次标定结果。
+ *
+ * 计算：
+ *
+ *   T_G_H = inverse(T_C_G) * T_C_H
+ *
+ *   T_G_P = T_G_H * T_H_P
+ *
+ * 即：
+ *
+ *   T_G_P = inverse(T_C_G) * T_C_H * T_H_P
+ *
+ * 最终：
+ *
+ *   p_G_P = T_G_P.block<3, 1>(0, 3)
+ *
+ * 得到 TCP 原点在 Screen Tag 坐标系下的位置。
+ *
+ * 注意：
+ * - 本类不主动抓相机图像；
+ * - 本类不主动调用 AprilTagPerception::update()；
+ * - 上层应保证当前 perception 缓存来自同一帧；
+ * - Screen Tag 和 Hand Tag 必须同时在当前帧可见。
+ */
+class TagRelativeTcpPose {
+public:
+    enum class Status {
+        OK = 0,
+
+        NO_PERCEPTION,
+
+        INVALID_SCREEN_TAG_ID,
+
+        INVALID_HAND_TAG_ID,
+
+        SAME_TAG_ID,
+
+        SCREEN_TAG_NOT_FOUND,
+
+        HAND_TAG_NOT_FOUND,
+
+        INVALID_T_C_G,
+
+        INVALID_T_C_H,
+
+        INVALID_T_H_P,
+
+        INVALID_T_G_H,
+
+        INVALID_T_G_P,
+
+        INVALID_TCP_POSITION
+    };
+
+public:
+    explicit TagRelativeTcpPose(
+        const std::shared_ptr<AprilTagPerception>& perception = nullptr);
+
+    /**
+     * @brief 设置 AprilTag 感知前端。
+     *
+     * 本类只读取感知缓存，不主动 update。
+     */
+    void setPerception(
+        const std::shared_ptr<AprilTagPerception>& perception);
+
+    const std::shared_ptr<AprilTagPerception>& perception() const {
+        return perception_;
+    }
+
+    /**
+     * @brief 设置 Screen Tag ID。
+     */
+    void setScreenTagId(int id);
+
+    /**
+     * @brief 设置 Hand Tag ID。
+     */
+    void setHandTagId(int id);
+
+    int screenTagId() const {
+        return screen_tag_id_;
+    }
+
+    int handTagId() const {
+        return hand_tag_id_;
+    }
+
+    /**
+     * @brief 使用当前 AprilTagPerception 缓存计算 TCP 相对 Screen Tag 的位姿。
+     *
+     * 核心公式：
+     *
+     *   T_G_P =
+     *       inverse(T_C_G)
+     *       * T_C_H
+     *       * T_H_P
+     *
+     * @param T_H_P
+     *        TCP(P) 相对于 Hand Tag(H) 的固定齐次变换。
+     *
+     *        坐标变换语义：
+     *
+     *          p_H = T_H_P * p_P
+     *
+     *        即：
+     *
+     *          ^H T_P
+     *
+     * @return 成功返回 true。
+     */
+    bool update(
+        const Eigen::Matrix4d& T_H_P);
+
+    /**
+     * @brief 当前结果是否有效。
+     */
+    bool valid() const {
+        return valid_;
+    }
+
+    /**
+     * @brief 最近一次 update() 的状态。
+     */
+    Status lastStatus() const {
+        return last_status_;
+    }
+
+    static const char* statusToString(Status status);
+
+    /**
+     * @brief 当前 Screen Tag 在 Camera 中的位姿。
+     *
+     * ^C T_G
+     */
+    const Eigen::Matrix4d& T_C_G() const {
+        return T_C_G_;
+    }
+
+    /**
+     * @brief 当前 Hand Tag 在 Camera 中的位姿。
+     *
+     * ^C T_H
+     */
+    const Eigen::Matrix4d& T_C_H() const {
+        return T_C_H_;
+    }
+
+    /**
+     * @brief Hand Tag 相对于 Screen Tag 的位姿。
+     *
+     * ^G T_H
+     */
+    const Eigen::Matrix4d& T_G_H() const {
+        return T_G_H_;
+    }
+
+    /**
+     * @brief TCP 相对于 Screen Tag 的完整 6DoF 位姿。
+     *
+     * ^G T_P
+     */
+    const Eigen::Matrix4d& T_G_P() const {
+        return T_G_P_;
+    }
+
+    /**
+     * @brief TCP 原点在 Screen Tag 坐标系中的位置。
+     *
+     * P_P^G =
+     *
+     * [ x_P ]
+     * [ y_P ]
+     * [ z_P ]
+     */
+    const Eigen::Vector3d& tcpPositionInScreenTag() const {
+        return p_G_P_;
+    }
+
+    /**
+     * @brief TCP 相对于 Screen Tag 的旋转矩阵。
+     *
+     * R_G_P
+     */
+    Eigen::Matrix3d tcpRotationInScreenTag() const {
+        return T_G_P_.block<3, 3>(0, 0);
+    }
+
+    /**
+     * @brief 清空当前结果。
+     */
+    void clear();
+
+private:
+    /**
+     * @brief 检查 4x4 矩阵元素是否全部有限。
+     */
+    static bool isFiniteTransform(
+        const Eigen::Matrix4d& T);
+
+    /**
+     * @brief 基础检查齐次矩阵最后一行。
+     */
+    static bool hasValidHomogeneousBottomRow(
+        const Eigen::Matrix4d& T,
+        double tolerance = 1e-6);
+
+    /**
+     * @brief 判断一个矩阵是否可以作为基本齐次变换使用。
+     *
+     * 当前只检查：
+     * - 所有元素 finite
+     * - 最后一行约等于 [0 0 0 1]
+     *
+     * 暂时不强制检查 rotation orthonormal，
+     * 避免视觉估计中的微小数值误差导致误判。
+     */
+    static bool isValidTransform(
+        const Eigen::Matrix4d& T);
+
+private:
+    std::shared_ptr<AprilTagPerception> perception_{nullptr};
+
+    int screen_tag_id_{-1};
+    int hand_tag_id_{-1};
+
+    // 当前外部相机观测
+    Eigen::Matrix4d T_C_G_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    Eigen::Matrix4d T_C_H_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    // 相对变换
+    Eigen::Matrix4d T_G_H_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    Eigen::Matrix4d T_G_P_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    // TCP 原点在 Screen Tag 坐标系的位置
+    Eigen::Vector3d p_G_P_{
+        Eigen::Vector3d::Zero()
+    };
+
+    bool valid_{false};
+
+    Status last_status_{
+        Status::NO_PERCEPTION
+    };
+};
+
+} // namespace cmvr::perception
+
+#endif // CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H

cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp

@@ -0,0 +1,315 @@
+#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
+
+#include <cmath>
+
+namespace cmvr::perception {
+
+TagRelativeTcpPose::TagRelativeTcpPose(
+    const std::shared_ptr<AprilTagPerception>& perception)
+    : perception_(perception)
+{
+}
+
+void TagRelativeTcpPose::setPerception(
+    const std::shared_ptr<AprilTagPerception>& perception)
+{
+    perception_ = perception;
+    clear();
+
+    if (!perception_) {
+        last_status_ = Status::NO_PERCEPTION;
+    }
+}
+
+void TagRelativeTcpPose::setScreenTagId(
+    const int id)
+{
+    screen_tag_id_ = id;
+    valid_ = false;
+}
+
+void TagRelativeTcpPose::setHandTagId(
+    const int id)
+{
+    hand_tag_id_ = id;
+    valid_ = false;
+}
+
+bool TagRelativeTcpPose::update(
+    const Eigen::Matrix4d& T_H_P)
+{
+    valid_ = false;
+
+    /*
+     * 1. 检查 perception
+     */
+    if (!perception_) {
+        last_status_ = Status::NO_PERCEPTION;
+        return false;
+    }
+
+    /*
+     * 2. 检查 Tag ID
+     */
+    if (screen_tag_id_ < 0) {
+        last_status_ = Status::INVALID_SCREEN_TAG_ID;
+        return false;
+    }
+
+    if (hand_tag_id_ < 0) {
+        last_status_ = Status::INVALID_HAND_TAG_ID;
+        return false;
+    }
+
+    if (screen_tag_id_ == hand_tag_id_) {
+        last_status_ = Status::SAME_TAG_ID;
+        return false;
+    }
+
+    /*
+     * 3. 检查外部传入的：
+     *
+     *      ^H T_P
+     *
+     * Hand Tag -> TCP 固定标定矩阵。
+     */
+    if (!isValidTransform(T_H_P)) {
+        last_status_ = Status::INVALID_T_H_P;
+        return false;
+    }
+
+    /*
+     * 4. 从 AprilTagPerception 当前缓存取 Screen Tag。
+     *
+     * AprilTagPerception 已经通过 ViSP / PnP 得到：
+     *
+     *      ^C T_G
+     */
+    const auto* screen_tag =
+        perception_->findTag(screen_tag_id_);
+
+    if (!screen_tag) {
+        last_status_ =
+            Status::SCREEN_TAG_NOT_FOUND;
+        return false;
+    }
+
+    /*
+     * 5. 取 Hand Tag：
+     *
+     *      ^C T_H
+     */
+    const auto* hand_tag =
+        perception_->findTag(hand_tag_id_);
+
+    if (!hand_tag) {
+        last_status_ =
+            Status::HAND_TAG_NOT_FOUND;
+        return false;
+    }
+
+    /*
+     * 6. 保存当前帧两个原始视觉变换。
+     *
+     * AprilTagPerception::Tag::T_c_t
+     *
+     * 定义是：
+     *
+     *      Tag -> Camera
+     *
+     * 因此：
+     *
+     * screen tag:
+     *
+     *      ^C T_G
+     *
+     * hand tag:
+     *
+     *      ^C T_H
+     */
+    T_C_G_ = screen_tag->T_c_t;
+    T_C_H_ = hand_tag->T_c_t;
+
+    if (!isValidTransform(T_C_G_)) {
+        last_status_ = Status::INVALID_T_C_G;
+        return false;
+    }
+
+    if (!isValidTransform(T_C_H_)) {
+        last_status_ = Status::INVALID_T_C_H;
+        return false;
+    }
+
+    /*
+     * 7. 求 Hand Tag 相对于 Screen Tag 的位姿。
+     *
+     * 已知：
+     *
+     *      ^C T_G
+     *      ^C T_H
+     *
+     * 因此：
+     *
+     *      ^G T_H
+     *
+     *    = (^C T_G)^-1 * ^C T_H
+     */
+    T_G_H_ =
+        T_C_G_.inverse() *
+        T_C_H_;
+
+    if (!isValidTransform(T_G_H_)) {
+        last_status_ = Status::INVALID_T_G_H;
+        return false;
+    }
+
+    /*
+     * 8. 求 TCP 相对于 Screen Tag 的位姿。
+     *
+     * 已知：
+     *
+     *      ^G T_H
+     *      ^H T_P
+     *
+     * 因此：
+     *
+     *      ^G T_P
+     *
+     *    = ^G T_H * ^H T_P
+     *
+     *    = (^C T_G)^-1
+     *      * ^C T_H
+     *      * ^H T_P
+     */
+    T_G_P_ =
+        T_G_H_ *
+        T_H_P;
+
+    if (!isValidTransform(T_G_P_)) {
+        last_status_ = Status::INVALID_T_G_P;
+        return false;
+    }
+
+    /*
+     * 9. 提取 TCP 原点在 Screen Tag
+     *    坐标系 G 下的位置。
+     *
+     *      P_P^G =
+     *
+     *      [ x_P ]
+     *      [ y_P ]
+     *      [ z_P ]
+     *
+     */
+    p_G_P_ =
+        T_G_P_.block<3, 1>(0, 3);
+
+    if (!p_G_P_.allFinite()) {
+        last_status_ =
+            Status::INVALID_TCP_POSITION;
+        return false;
+    }
+
+    /*
+     * 10. 当前帧结果有效。
+     */
+    valid_ = true;
+    last_status_ = Status::OK;
+
+    return true;
+}
+
+void TagRelativeTcpPose::clear()
+{
+    T_C_G_.setIdentity();
+    T_C_H_.setIdentity();
+
+    T_G_H_.setIdentity();
+    T_G_P_.setIdentity();
+
+    p_G_P_.setZero();
+
+    valid_ = false;
+}
+
+const char* TagRelativeTcpPose::statusToString(
+    const Status status)
+{
+    switch (status) {
+
+        case Status::OK:
+            return "ok";
+
+        case Status::NO_PERCEPTION:
+            return "no_perception";
+
+        case Status::INVALID_SCREEN_TAG_ID:
+            return "invalid_screen_tag_id";
+
+        case Status::INVALID_HAND_TAG_ID:
+            return "invalid_hand_tag_id";
+
+        case Status::SAME_TAG_ID:
+            return "same_tag_id";
+
+        case Status::SCREEN_TAG_NOT_FOUND:
+            return "screen_tag_not_found";
+
+        case Status::HAND_TAG_NOT_FOUND:
+            return "hand_tag_not_found";
+
+        case Status::INVALID_T_C_G:
+            return "invalid_T_C_G";
+
+        case Status::INVALID_T_C_H:
+            return "invalid_T_C_H";
+
+        case Status::INVALID_T_H_P:
+            return "invalid_T_H_P";
+
+        case Status::INVALID_T_G_H:
+            return "invalid_T_G_H";
+
+        case Status::INVALID_T_G_P:
+            return "invalid_T_G_P";
+
+        case Status::INVALID_TCP_POSITION:
+            return "invalid_tcp_position";
+
+        default:
+            return "unknown";
+    }
+}
+
+bool TagRelativeTcpPose::isFiniteTransform(
+    const Eigen::Matrix4d& T)
+{
+    return T.allFinite();
+}
+
+bool TagRelativeTcpPose::hasValidHomogeneousBottomRow(
+    const Eigen::Matrix4d& T,
+    const double tolerance)
+{
+    return
+        std::abs(T(3, 0)) <= tolerance &&
+        std::abs(T(3, 1)) <= tolerance &&
+        std::abs(T(3, 2)) <= tolerance &&
+        std::abs(T(3, 3) - 1.0) <= tolerance;
+}
+
+bool TagRelativeTcpPose::isValidTransform(
+    const Eigen::Matrix4d& T)
+{
+    if (!isFiniteTransform(T)) {
+        return false;
+    }
+
+    if (!hasValidHomogeneousBottomRow(T)) {
+        return false;
+    }
+
+    return true;
+}
+
+} // namespace cmvr::perception

cmvr-es/common/CMakeLists.txt

@@ -29,6 +29,12 @@ target_link_libraries(common PUBLIC
 add_library(cmvr_es::common ALIAS common)
 install(TARGETS common LIBRARY DESTINATION lib)
 
+add_executable(support_functions_test
+        math/support_functions_test.cpp
+)
+target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es)
+target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog)
+
 #add_executable(image_display_test
 #        utils/visualization/image_display_test.cpp
 #)

cmvr-es/common/math/proto_geometry.h

@@ -27,6 +27,26 @@ inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src)
     return toEigenVec3(src, Eigen::Vector3d::Zero());
 }
 
+inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src,
+                                    Eigen::Vector3d defaults)
+{
+    if (src.has_rx()) {
+        defaults.x() = src.rx();
+    }
+    if (src.has_ry()) {
+        defaults.y() = src.ry();
+    }
+    if (src.has_rz()) {
+        defaults.z() = src.rz();
+    }
+    return defaults;
+}
+
+inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src)
+{
+    return toEigenEuler(src, Eigen::Vector3d::Zero());
+}
+
 inline Eigen::Matrix<double, 6, 1> toEigenVec6(
     const cmvr::common::Vec6& src,
     Eigen::Matrix<double, 6, 1> defaults)
@@ -102,6 +122,10 @@ inline bool hasVec3(const cmvr::common::Vec3& value) {
     return value.has_x() && value.has_y() && value.has_z();
 }
 
+inline bool hasEuler(const cmvr::common::Euler& value) {
+    return value.has_rx() && value.has_ry() && value.has_rz();
+}
+
 inline bool hasVec6(const cmvr::common::Vec6& value) {
     return value.has_x() && value.has_y() && value.has_z() &&
            value.has_rx() && value.has_ry() && value.has_rz();

cmvr-es/common/math/support_functions.h

@@ -3,6 +3,7 @@
 //
 
 #pragma once
+#include <cstdint>
 #include <cmath>
 #include <vector>
 #include <algorithm>
@@ -14,6 +15,25 @@ class SupportFunctions {
 private:
     static constexpr double EPS = 1e-9;
 public:
+    static constexpr std::int64_t absoluteDifference(const std::int32_t lhs,
+                                                     const std::int32_t rhs) noexcept {
+        return lhs >= rhs
+                   ? static_cast<std::int64_t>(lhs) - static_cast<std::int64_t>(rhs)
+                   : static_cast<std::int64_t>(rhs) - static_cast<std::int64_t>(lhs);
+    }
+
+    static constexpr std::int64_t cyclicAbsoluteDifference(
+        const std::int32_t lhs,
+        const std::int32_t rhs,
+        const std::int64_t period) noexcept {
+        const auto linear_distance = absoluteDifference(lhs, rhs);
+        if (period <= 0) {
+            return linear_distance;
+        }
+        const auto wrapped_distance = linear_distance % period;
+        return std::min(wrapped_distance, period - wrapped_distance);
+    }
+
     static  std::vector<double> eigen_to_vector(const Eigen::VectorXd &v) {
         return std::vector<double>(v.data(), v.data() + v.size());
     }

cmvr-es/common/math/support_functions_test.cpp

@@ -0,0 +1,34 @@
+#include <cstdint>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+#include "common/math/support_functions.h"
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceTreatsFullTurnsAsEquivalent)
+{
+    constexpr std::int64_t period = 65536LL * 101LL;
+
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(5254257, -1364879, period), 0);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(-10883488, -17502624, period), 0);
+}
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceUsesShortestWrappedDistance)
+{
+    constexpr std::int64_t period = 100;
+
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(3, 97, period), 6);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(97, 3, period), 6);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(10, 40, period), 30);
+}
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceHandlesInt32Range)
+{
+    constexpr std::int64_t period = 65536LL * 101LL;
+
+    const auto distance = SupportFunctions::cyclicAbsoluteDifference(
+        std::numeric_limits<std::int32_t>::min(),
+        std::numeric_limits<std::int32_t>::max(), period);
+    EXPECT_GE(distance, 0);
+    EXPECT_LE(distance, period / 2);
+}

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

@@ -3,6 +3,7 @@
 
 #include <cstdint>
 #include <functional>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -65,6 +66,27 @@ 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;
+    // Unset follows the arm config. False preserves the original streaming
+    // direction-following policy used by PBVS; true enables fixed-axis reversal.
+    std::optional<bool> continuous_linear_reversal;
+    // Capture the first applied command's measured TCP position and target
+    // direction in Base. Useful for distance-based motion without caller FK.
+    bool capture_reference{false};
+};
+
+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};
@@ -113,6 +135,14 @@ struct JointGroupState {
     }
 };
 
+struct JointTrajectoryPoint {
+    double time_s{0.0};
+    std::vector<double> position;
+    std::vector<double> velocity;
+};
+
+using JointTrajectory = std::vector<JointTrajectoryPoint>;
+
 struct JointPositionCommand {
     std::vector<double> position;
 

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

@@ -3,8 +3,8 @@ arm {
     id: "right_arm"
 
     motor {
-      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can"
+      motor_system_id: "right_arm_can_motors"
+      motor_group_ids: "right_arm_can_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"
@@ -51,7 +51,166 @@ arm {
             gain: 0.2
             margin_ratio: 0.15
             max_push: 0.25
-            weight: 0.05
+          }
+        }
+      }
+    }
+
+    motion {
+      move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
+        toppra_joint_motion_planner {
+          path_type: TOPPRA_PATH_TYPE_QUINTIC
+          sample_period_s: 0.001
+          grid_size: 150
+          high_grid_size: 300
+        }
+      }
+
+      move_l {
+        pinocchio_cartesian_motion_planner {
+          sample_period_s: 0.001
+          position_gain: 4.0
+          rotation_gain: 4.0
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_continuity_check {
+            enable: true
+            max_joint_delta_rad: 0.05
+            max_joint_velocity_rad_s: 10.0
+            max_joint_acceleration_rad_s2: 5000.0
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 45.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 45.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+      }
+
+      speed_l {
+        pinocchio_cartesian_motion_planner {
+          linear_velocity_max: 0.55
+          linear_acceleration_max: 5.0
+          linear_jerk_max: 10.0
+          angular_velocity_max: 1.0
+          angular_acceleration_max: 5.0
+          angular_jerk_max: 12.0
+          linear_target_replan_threshold: 1e-4
+          angular_target_replan_threshold: 1e-4
+          linear_reverse_cos_threshold: -0.8660254037844386
+          linear_reverse_switch_speed_threshold: 1e-3
+          enforce_joint_acceleration_limits: true
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 30.0
+            max_joint_acceleration_rad_s2: 10000.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 5.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 5.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+
+        speed_l_controller {
+          cartesian_velocity_controller {
+            control_period_s: 0.001
+            stop_twist_norm: 1e-9
+            stop_command_velocity_norm: 1e-3
+            stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 10
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
+          }
+        }
+      }
+    }
+  }
+
+  robot_arms {
+        id: "left_arm"
+
+        motor {
+          motor_system_id: "left_arm_can_motors"
+          motor_group_ids: "left_arm_can_motors"
+          dof: 7
+          joint_names: "L_SHOULDER_P"
+          joint_names: "L_SHOULDER_R"
+          joint_names: "L_SHOULDER_Y"
+          joint_names: "L_ELBOW_R"
+          joint_names: "L_WRIST_P"
+          joint_names: "L_WRIST_Y"
+          joint_names: "L_WRIST_R"
+          upd_freq: 1000
+          buffer_size: 50
+          default_vel: 1.0
+          default_acc: 2.0
+        }
+
+        kinematics {
+          pinocchio_dls_ik_solver {
+            urdf_path: "model/xiaoyan_description/dual_arm.urdf"
+            base_frame_name: "PELVIS_S"
+            flange_frame_name: "L_WRIST_R_S"
+            tcp_frame_name: "L_FINGER_TIP_FIXED"
+            max_iters: 100
+            pos_eps: 1e-6
+            rot_eps: 1e-6
+            damping: 1e-6
+            joint_limit_policy {
+              limits {
+                enable: true
+                source: JOINT_LIMIT_SOURCE_CUSTOM
+                joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+                joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+                joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+                joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+                joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+                joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+                joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+              }
+              soft_limit {
+                enable: true
+                margin_ratio: 0.01
+                min_margin_rad: 0.01
+              }
+              avoidance {
+                enable: false
+                gain: 0.2
+                margin_ratio: 0.15
+                max_push: 0.25
               }
             }
           }

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

@@ -0,0 +1,160 @@
+arm {
+  robot_arms {
+    id: "mujoco_right_arm"
+
+    motor {
+      motor_system_id: "right_arm_mujoco_motors"
+      motor_group_ids: "right_arm_mujoco_motors"
+      dof: 7
+      joint_names: "right_arm_J1"
+      joint_names: "right_arm_J2"
+      joint_names: "right_arm_J3"
+      joint_names: "right_arm_J4"
+      joint_names: "right_arm_J5"
+      joint_names: "right_arm_J6"
+      joint_names: "right_arm_J7"
+      upd_freq: 1000
+      buffer_size: 50
+      default_vel: 0.6
+      default_acc: 2.0
+    }
+
+    kinematics {
+      pinocchio_dls_ik_solver {
+        urdf_path: "model/gen2/robot.urdf"
+        base_frame_name: "body_link"
+        flange_frame_name: "arm_link_7_2"
+        max_iters: 200
+        pos_eps: 1e-6
+        rot_eps: 1e-6
+        damping: 1e-5
+        joint_limit_policy {
+          limits {
+            enable: true
+            source: JOINT_LIMIT_SOURCE_CUSTOM
+            joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.15
+            max_push: 0.25
+          }
+        }
+      }
+    }
+
+    motion {
+      move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
+        toppra_joint_motion_planner {
+          path_type: TOPPRA_PATH_TYPE_QUINTIC
+          sample_period_s: 0.001
+          grid_size: 150
+          high_grid_size: 300
+        }
+      }
+
+      move_l {
+        pinocchio_cartesian_motion_planner {
+          sample_period_s: 0.001
+          position_gain: 4.0
+          rotation_gain: 4.0
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.005
+          }
+          joint_continuity_check {
+            enable: true
+            max_joint_delta_rad: 0.05
+            max_joint_velocity_rad_s: 4.0
+            max_joint_acceleration_rad_s2: 100.0
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 10.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 10.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+      }
+
+      speed_l {
+        pinocchio_cartesian_motion_planner {
+          linear_velocity_max: 0.5
+          linear_acceleration_max: 2.0
+          linear_jerk_max: 10.0
+          angular_velocity_max: 1.0
+          angular_acceleration_max: 5.0
+          angular_jerk_max: 12.0
+          linear_target_replan_threshold: 1e-4
+          angular_target_replan_threshold: 1e-4
+          linear_reverse_cos_threshold: -0.8660254037844386
+          linear_reverse_switch_speed_threshold: 1e-3
+          enforce_joint_acceleration_limits: true
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.005
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 4.0
+            max_joint_acceleration_rad_s2: 100.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 10.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 10.0
+            min_desired_linear_speed: 0.01
+            min_desired_angular_speed: 1e-4
+          }
+        }
+
+        speed_l_controller {
+          cartesian_velocity_controller {
+            control_period_s: 0.001
+            stop_twist_norm: 1e-9
+            stop_command_velocity_norm: 1e-3
+            stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 2.0
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
+          }
+        }
+      }
+    }
+  }
+}

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

@@ -3,8 +3,8 @@ arm {
     id: "mujoco_right_arm"
 
     motor {
-      motor_system_id: "mujoco_motors"
-      motor_group_ids: "mujoco_right_arm"
+      motor_system_id: "right_arm_mujoco_motors"
+      motor_group_ids: "right_arm_mujoco_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"
@@ -51,7 +51,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.15
             max_push: 0.25
-            weight: 2.0
           }
         }
       }
@@ -59,6 +58,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -144,6 +151,8 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 10
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }

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

@@ -3,8 +3,8 @@ arm {
     id: "mujoco_right_arm"
 
     motor {
-      motor_system_id: "mujoco_motors"
-      motor_group_ids: "mujoco_right_arm"
+      motor_system_id: "right_arm_mujoco_motors"
+      motor_group_ids: "right_arm_mujoco_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"
@@ -52,7 +52,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.01
             max_push: 0.02
-            weight: 0.05
           }
         }
       }
@@ -60,6 +59,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.002
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -145,6 +152,8 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 5
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }

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

@@ -3,8 +3,8 @@ arm {
     id: "right_arm"
 
     motor {
-      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can"
+      motor_system_id: "right_arm_can_motors"
+      motor_group_ids: "right_arm_can_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"
@@ -52,7 +52,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.01
             max_push: 0.02
-            weight: 0.05
           }
         }
       }
@@ -60,6 +59,14 @@ arm {
 
     motion {
       move_j {
+        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
+        settle_timeout_s: 2.0
+        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
+        settle_position_tolerance_rad: 0.01
+        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
+        settle_velocity_tolerance_rad_s: 0.02
+        # 位置和速度连续满足条件的采样次数。
+        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -102,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: 60.0
           angular_velocity_max: 1.0
           angular_acceleration_max: 5.0
           angular_jerk_max: 12.0
@@ -130,9 +137,9 @@ arm {
           cartesian_velocity_feasibility_check {
             enable: true
             min_linear_speed_ratio: 0.2
-            max_linear_direction_deviation_deg: 5.0
+            max_linear_direction_deviation_deg: 70
             min_angular_speed_ratio: 0.2
-            max_angular_direction_deviation_deg: 5.0
+            max_angular_direction_deviation_deg: 70
             min_desired_linear_speed: 1e-4
             min_desired_angular_speed: 1e-4
           }
@@ -144,7 +151,9 @@ arm {
             stop_twist_norm: 1e-9
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
-            stop_acceleration: 0.5
+            stop_acceleration: 5
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
           }
         }
       }

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

@@ -35,8 +35,8 @@ camera {
         stream_mode: STREAM_MODE_RGB
       }
       encoder {
-        width: 640
-        height: 360
+        width: 480
+        height: 320
         fps: 30
         codec: "H264"
         enable_stream_timestamp: true
@@ -48,21 +48,21 @@ camera {
   }
 
   cameras {
-    id: "cam3"
+    id: "left_hand_cam"
     realsense {
       serialNumber: "243122075614"
       camera_mode: CAMERA_MODE_VIDEO
       capture {
-        width: 640
-        height: 480
+        width: 1280
+        height: 720
         fps: 30
-        stream_mode: STREAM_MODE_RGBD
+        stream_mode: STREAM_MODE_RGB
       }
       encoder {
-        width: 640
-        height: 480
+        width: 480
+        height: 320
         fps: 30
-        codec: "H265"
+        codec: "H264"
         enable_stream_timestamp: true
         buffer_size: 30
       }
@@ -83,8 +83,8 @@ camera {
         stream_mode: STREAM_MODE_RGBD
       }
       encoder {
-        width: 1280
-        height: 720
+        width: 480
+        height: 320
         fps: 30
         codec: "H264"
         enable_stream_timestamp: true
@@ -92,7 +92,7 @@ camera {
       }
       consume_new_frame_only: false
       viewer_pip {
-        enable: true
+        enable: false
         left: -10
         bottom: 10
         width: 320
@@ -101,6 +101,36 @@ camera {
     }
   }
 
+  cameras {
+    id: "mujoco_external_touch_cam"
+    mujoco {
+      world_id: "mujoco_world"
+      camera_name: "external_touch_cam"
+      render {
+        width: 1280
+        height: 720
+        fps: 30
+        stream_mode: STREAM_MODE_RGBD
+      }
+      encoder {
+        width: 480
+        height: 320
+        fps: 30
+        codec: "H264"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
+      consume_new_frame_only: false
+      viewer_pip {
+        enable: false
+        left: 10
+        bottom: 10
+        width: 320
+        height: 180
+      }
+    }
+  }
+
   cameras {
     id: "left_eye_cam"
     uvc {

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

@@ -2,7 +2,7 @@ dexhand {
   dexhands {
     id: "hand1"
     rh56dftp {
-      ip: "192.168.1.213"
+      ip: "192.168.1.223"
       port: 6000
       poll_interval_ms: 10
     }
@@ -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
@@ -38,4 +40,9 @@ dexhand {
       auto_calibrate: false
     }
   }
+
+  dexhands {
+    id: "mujoco_zero_touch_dexhand"
+    zero_sim_touch {}
+  }
 }

cmvr-es/config/devices/motor/ethercat_motors.pb.txt

@@ -0,0 +1,72 @@
+motor {
+  id: "ethercat_motors"
+
+  motor_groups {
+    id: "right_arm_ethercat_motors"
+    bus_type: MOTOR_BUS_ETHERCAT
+    vendor: MOTOR_VENDOR_EYOU
+    protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
+
+    ethercat {
+      master_index: 0
+      cycle_us: 1000
+      slave_op_timeout_ms: 12000
+      slave_state_poll_period_ms: 10
+
+      cia402 {
+        state_transition_timeout_ms: 1200
+        velocity_stop_timeout_ms: 2000
+        status_poll_period_ms: 10
+        stopped_velocity_tolerance_rad_s: 0.001
+      }
+
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 1000
+      }
+
+      dc {
+        enable: true
+        reference_motor_id: 1
+        sync0_cycle_us: 1000
+        sync0_shift_us: 0
+        sync_reference_clock_period: 1
+        assign_activate: 768
+        sync_monitor_period_ms: 1000
+      }
+
+      slaves { motor_id: 1 alias: 0 position: 0 }
+      slaves { motor_id: 2 alias: 0 position: 1 }
+      slaves { motor_id: 3 alias: 0 position: 2 }
+      slaves { motor_id: 4 alias: 0 position: 3 }
+      slaves { motor_id: 5 alias: 0 position: 4 }
+      slaves { motor_id: 6 alias: 0 position: 5 }
+      slaves { motor_id: 7 alias: 0 position: 6 }
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+    }
+  }
+}

cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt

@@ -0,0 +1,63 @@
+motor {
+  id: "ethercat_motors"
+
+  motor_groups {
+    id: "right_arm_ethercat_motors"
+    bus_type: MOTOR_BUS_ETHERCAT
+    vendor: MOTOR_VENDOR_EYOU
+    protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
+
+    ethercat {
+      master_index: 0
+      cycle_us: 1000
+      slave_op_timeout_ms: 15000
+      slave_state_poll_period_ms: 10
+
+      cia402 {
+        state_transition_timeout_ms: 1200
+        velocity_stop_timeout_ms: 2000
+        status_poll_period_ms: 10
+        stopped_velocity_tolerance_rad_s: 0.001
+      }
+
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 1000
+      }
+
+      dc {
+        enable: false
+        reference_motor_id: 1
+        sync0_cycle_us: 1000
+        sync0_shift_us: 0
+        sync_reference_clock_period: 1
+        assign_activate: 768
+        sync_monitor_period_ms: 1000
+      }
+
+      slaves { motor_id: 1 alias: 0 position: 0 }
+      slaves { motor_id: 2 alias: 0 position: 1 }
+      slaves { motor_id: 3 alias: 0 position: 2 }
+      slaves { motor_id: 4 alias: 0 position: 3 }
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+    }
+  }
+}

cmvr-es/config/devices/motor/mujoco_motors.pb.txt

@@ -2,11 +2,10 @@ motor {
   id: "mujoco_motors"
 
   motor_groups {
-    id: "mujoco_right_arm"
+    id: "right_arm_mujoco_motors"
     bus_type: MOTOR_BUS_MUJOCO
     vendor: MOTOR_VENDOR_MUJOCO
     protocol: MOTOR_PROTOCOL_MUJOCO
-    tool_frame: "R_FINGER_TIP"
     mujoco {
       world_id: "mujoco_world"
     }

cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt

@@ -0,0 +1,35 @@
+motor {
+  id: "mujoco_motors"
+
+  motor_groups {
+    id: "right_arm_mujoco_motors"
+    bus_type: MOTOR_BUS_MUJOCO
+    vendor: MOTOR_VENDOR_MUJOCO
+    protocol: MOTOR_PROTOCOL_MUJOCO
+    mujoco {
+      world_id: "mujoco_world"
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "right_arm_J1" }
+      motors { id: 2 joint_name: "right_arm_J2" }
+      motors { id: 3 joint_name: "right_arm_J3" }
+      motors { id: 4 joint_name: "right_arm_J4" }
+      motors { id: 5 joint_name: "right_arm_J5" }
+      motors { id: 6 joint_name: "right_arm_J6" }
+      motors { id: 7 joint_name: "right_arm_J7" }
+    }
+  }
+}

cmvr-es/config/devices/motor/ti5_motors.pb.txt

@@ -2,11 +2,10 @@ motor {
   id: "ti5_motors"
 
   motor_groups {
-    id: "left_arm_can"
+    id: "left_arm_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
-    tool_frame: "L_FINGER_TIP"
     can {
       channel_id: 0
     }
@@ -16,22 +15,21 @@ motor {
       urdf_path: "model/xiaoyan_description/dual_arm.urdf"
     }
     motors {
-      motors { id: 23 joint_name: "L_SHOULDER_P" }
-      motors { id: 24 joint_name: "L_SHOULDER_R" }
-      motors { id: 25 joint_name: "L_SHOULDER_Y" }
-      motors { id: 26 joint_name: "L_ELBOW_R" }
-      motors { id: 27 joint_name: "L_WRIST_P" }
-      motors { id: 28 joint_name: "L_WRIST_Y" }
-      motors { id: 29 joint_name: "L_WRIST_R" }
+      motors { id: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 24 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 25 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 26 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 27 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 28 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 29 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 
   motor_groups {
-    id: "right_arm_can"
+    id: "right_arm_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
-    tool_frame: "R_FINGER_TIP"
     can {
       channel_id: 1
     }
@@ -47,18 +45,18 @@ motor {
       joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
     }
     motors {
-      motors { id: 16 joint_name: "R_SHOULDER_P" }
-      motors { id: 17 joint_name: "R_SHOULDER_R" }
-      motors { id: 18 joint_name: "R_SHOULDER_Y" }
-      motors { id: 19 joint_name: "R_ELBOW_R" }
-      motors { id: 20 joint_name: "R_WRIST_P" }
-      motors { id: 21 joint_name: "R_WRIST_Y" }
-      motors { id: 22 joint_name: "R_WRIST_R" }
+      motors { id: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 17 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 18 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 19 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 20 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 21 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 22 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 
   motor_groups {
-    id: "head_can"
+    id: "head_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -73,14 +71,14 @@ motor {
       joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
     motors {
-      motors { id: 32 joint_name: "HEAD_Y" }
-      motors { id: 30 joint_name: "HEAD_P" }
-      motors { id: 31 joint_name: "HEAD_R" }
+      motors { id: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 31 joint_name: "HEAD_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 
   motor_groups {
-    id: "waist_can"
+    id: "waist_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -94,8 +92,8 @@ motor {
       joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
     motors {
-      motors { id: 4 joint_name: "WAIST_Y" }
-      motors { id: 15 joint_name: "WAIST_P" }
+      motors { id: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 }

cmvr-es/config/devices/mujoco/right_arm_eye_to_hand_world.pb.txt

@@ -0,0 +1,7 @@
+worlds {
+  id: "mujoco_world"
+  model_path: "model/xiaoyan_description/right_arm_eye_to_hand.xml"
+  timestep_s: 0.001
+  realtime_factor: 1.0
+  require_actuator: true
+}

cmvr-es/config/logger/logger.pb.txt

@@ -30,7 +30,7 @@ logger {
   max_file_size_mb: 100
   flush_interval_seconds: 1
   format {
-    show_time: false
+    show_time: true
     show_level: true
     show_thread_id: false
     show_source_location: true

cmvr-es/config/manager/device_manager.pb.txt

@@ -2,16 +2,15 @@ device_manager {
   name: "cmvr_es"
   version: "0.1"
   description: "cmvr edge system version 0.1"
-
   devices {
     id: "mujoco_world"
     type: DEVICE_TYPE_MUJOCO_WORLD
-    config_file: "devices/mujoco/mujoco_world.pb.txt"
+    config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"
     enable: false
   }
 
   devices {
-    id: "mujoco_motors"
+    id: "right_arm_mujoco_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/mujoco_motors.pb.txt"
     enable: false
@@ -39,52 +38,95 @@ device_manager {
   }
 
   devices {
-    id: "right_hand_cam"
+    id: "mujoco_external_touch_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
     enable: false
   }
 
   devices {
-    id: "cam5"
+    id: "right_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: false
+    enable: true
   }
 
+    devices {
+      id: "left_hand_cam"
+      type: DEVICE_TYPE_CAMERA
+      config_file: "devices/camera/camera.pb.txt"
+      enable: true
+    }
+
+
   devices {
     id: "hand2"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "paxini_tip_1"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "mujoco_zero_touch_dexhand"
+    type: DEVICE_TYPE_DEXHAND
+    config_file: "devices/dexhand/dexhand.pb.txt"
     enable: false
   }
 
   devices {
-    id: "ti5_motors"
+    id: "left_arm_can_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
     enable: false
   }
 
+  devices {
+    id: "right_arm_can_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "head_can_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "waist_can_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "right_arm_ethercat_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ethercat_motors.pb.txt"
+    enable: false
+  }
+
   devices {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/arm.pb.txt"
-    enable: false
+    config_file: "devices/arm/arm_qp.pb.txt"
+    enable: true
   }
 
   devices {
     id: "aubo_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/aubo_arm.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -107,12 +149,4 @@ device_manager {
     config_file: "devices/agv/agv.pb.txt"
     enable: false
   }
-
-  devices {
-    # 仙工控制器实例；与 agv.pb.txt 中的设备 ID 保持一致。
-    id: "src1100"
-    type: DEVICE_TYPE_AGV
-    config_file: "devices/agv/agv.pb.txt"
-    enable: true
-  }
 }

cmvr-es/config/manager/task_manager.pb.txt

@@ -5,7 +5,7 @@ task_manager {
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.001
     config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
-    enable: false
+    enable: true
   }
   tasks {
     id: "grpc_server"
@@ -14,4 +14,12 @@ task_manager {
     config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt"
     enable: true
   }
+  tasks {
+    id: "right_arm_self_collision"
+    type: TASK_TYPE_SELF_COLLISION
+    run_mode: TASK_RUN_MODE_PERIODIC_STEP
+    control_period_s: 0.002
+    config_file: "tasks/self_collision_task/self_collision_task.pb.txt"
+    enable: false
+  }
 }

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

@@ -0,0 +1,34 @@
+self_collision_task {
+  id: "right_arm_self_collision"
+  arm_id: "mujoco_right_arm"
+
+  checker {
+    urdf_path: "model/xiaoyan_description/dual_arm_collision.urdf"
+
+    # Simplified compact-wrist bodies overlap in the normal assembled pose.
+    ignored_pairs {
+      first: "R_WRIST_P_S"
+      second: "R_WRIST_R_S"
+    }
+  }
+
+  sampling {
+    max_geometry_displacement_m: 0.002
+    max_check_period_s: 0.01
+  }
+
+  safety {
+    warning_distance_m: 0.02
+    stop_distance_m: 0.005
+  }
+
+
+  recovery {
+    clear_distance_m: 0.025
+    stable_period_s: 0.1
+    max_joint_velocity_rad_s: 0.15
+    max_joint_acceleration_rad_s2: 0.3
+    history_duration_s: 10.0
+    max_distance_regression_m: 0.001
+  }
+}

cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt

@@ -0,0 +1,37 @@
+self_collision_task {
+  id: "gen2_right_arm_self_collision"
+  arm_id: "mujoco_right_arm"
+
+  checker {
+    urdf_path: "model/gen2/collision/robot_collision.urdf"
+
+    # These second-neighbor mounting bodies overlap in normal assembled poses.
+    ignored_pairs {
+      first: "arm_link_5_2"
+      second: "arm_link_7_2"
+    }
+    ignored_pairs {
+      first: "body_link"
+      second: "arm_link_2_2"
+    }
+  }
+
+  sampling {
+    max_geometry_displacement_m: 0.002
+    max_check_period_s: 0.01
+  }
+
+  safety {
+    warning_distance_m: 0.02
+    stop_distance_m: 0.005
+  }
+
+  recovery {
+    clear_distance_m: 0.05
+    stable_period_s: 0.1
+    max_joint_velocity_rad_s: 0.3
+    max_joint_acceleration_rad_s2: 5.0
+    history_duration_s: 10.0
+    max_distance_regression_m: 0.001
+  }
+}

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

@@ -1,90 +1,109 @@
 touch_screen_task {
   id: "touch_screen"
+  # 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
+  debug_draw_coordinate_frames: true
+  # G/H 坐标轴长度，单位为米。
+  debug_coordinate_axis_length_m: 0.02
 
   devices {
     arm_id: "right_arm"
     dexhand_id: "paxini_tip_1"
+    # 手部相机和外部相机的 DeviceManager ID。
     camera_id: "right_hand_cam"
+    external_camera_id: "left_hand_cam"
   }
 
   initialization {
-    before_start: true
+    before_start: false
     after_finish: true
-    joint_positions { joint_name: "R_SHOULDER_P" rad: -0.3678 }
-    joint_positions { joint_name: "R_SHOULDER_R" rad: 1.1127 }
-    joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.6084 }
-    joint_positions { joint_name: "R_ELBOW_R" rad: 1.61 }
-    joint_positions { joint_name: "R_WRIST_P" rad: -2.5718 }
-    joint_positions { joint_name: "R_WRIST_Y" rad: 0.1276 }
-    joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 }
+    joint_positions { joint_name: "R_SHOULDER_P" rad: -0.338732 }
+    joint_positions { joint_name: "R_SHOULDER_R" rad: 1.265943 }
+    joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.572396 }
+    joint_positions { joint_name: "R_ELBOW_R" rad: 1.5464 }
+    joint_positions { joint_name: "R_WRIST_P" rad: -2.8179 }
+    joint_positions { joint_name: "R_WRIST_Y" rad: 0.1105 }
+    joint_positions { joint_name: "R_WRIST_R" rad: 0.1347 }
     velocity: 1.0
     acceleration: 2.0
+    # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
+    skip_position_tolerance_rad: 0.01
+    # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
+    skip_velocity_tolerance_rad_s: 0.1
   }
 
   perception {
-    apriltag {
-      tag_size_m: 0.012
+    # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
+    tags {
+      screen {
+        id: 14
+        size_m: 0.016
+      }
+      hand {
+        id: 16
+        size_m: 0.016
+      }
+    }
+    # 手部相机：用于点击目标点和手部目标跟踪。
+    hand_camera {
       depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
       target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
     }
   }
 
   alignment {
-    ibvs {
-      camera_link: "R_CAM"
-      lambda: 0.4
-      mu: 0.1
-      qdot_max: 1.0
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
+    calibration {
+      #  TCP P 相对于 Hand Tag H 的目标姿态
+      hand_tag_to_tcp {
+        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.035
+        m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.09
+        m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.18
+        m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
+      }
+    }
+    pbvs {
+      position_gain { x: 1.0 y: 2.0 z: 1.0 }
+      rotation_gain { x: 1.0 y: 1.5 z: 1.0 }
+      vmax6 { x: 0.04 y: 0.04 z: 0.04 rx: 0.050 ry: 0.050 rz: 0.050 }
+      amax6 { x: 0.80 y: 0.80 z: 0.80 rx: 2.0 ry: 2.0 rz: 2.0 }
       twist_filter_alpha: 1.0
-      r_camera_to_visp {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: 1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: 1.0
-      }
-      r_camera_to_urdf {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: 1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: 1.0
-      }
-      control_joint_names: "R_SHOULDER_P"
-      control_joint_names: "R_SHOULDER_R"
-      control_joint_names: "R_SHOULDER_Y"
-      control_joint_names: "R_ELBOW_R"
-      control_joint_names: "R_WRIST_P"
-      control_joint_names: "R_WRIST_Y"
-      control_joint_names: "R_WRIST_R"
     }
     target {
-      position_in_camera { x: -0.001 y: 0.08 z: 0.15 }
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
+      # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
+      # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
+      # 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
+      # PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
+      hand_orientation_G { rx: 0.0 ry: 0.0 rz: -1.57 }
+      # 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
+      position_offset_G { x: 0.0 y: 0.0 z: 0.00 }
       mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
     }
     error_threshold {
       x: 0.005
       y: 0.005
-      z: 0.01
-      rx: 0.1026646259971647
-      ry: 0.1026646259971647
+      z: 0.005
+      rx: 0.0126646259971647
+      ry: 0.0126646259971647
       rz: 0.1026646259971647
     }
     stable_frames: 2
+    # 对齐及到位暂停各自的超时时间（秒）；暂停从对齐成功时重新计时。
     timeout_s: 20.0
+    # 到位后暂停；暂停超时结束任务，after_finish 为 true 时尝试回初始位置。
     pause_when_reached: false
   }
 
   touch {
     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
-      max_distance_m: 0.035
+      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
   }
@@ -92,6 +111,8 @@ touch_screen_task {
   retract {
     twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
     acceleration: 8.0
-    duration_s: 0.45
+    linear_jerk: 40.0  # m/s^3，约束接触后的减速和连续换向。
+    # TCP 后退目标距离，单位为米。
+    distance_m: 0.01
   }
 }

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

@@ -1,10 +1,16 @@
 touch_screen_task {
   id: "touch_screen"
+  # 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
+  debug_draw_coordinate_frames: true
+  # G/H 坐标轴长度，单位为米。
+  debug_coordinate_axis_length_m: 0.02
 
   devices {
-    arm_id: "right_arm_mujoco"
+    arm_id: "mujoco_right_arm"
     dexhand_id: "mujoco_zero_touch_dexhand"
-    camera_id: "hand_cam"
+    # 手部相机和外部相机的 DeviceManager ID。
+    camera_id: "mujoco_hand_cam"
+    external_camera_id: "mujoco_external_touch_cam"
   }
 
   initialization {
@@ -17,60 +23,81 @@ touch_screen_task {
     joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 }
     joint_positions { joint_name: "R_WRIST_Y" rad: 0.1150 }
     joint_positions { joint_name: "R_WRIST_R" rad: -0.08 }
-    velocity: 2.8
-    acceleration: 20.0
+    velocity: 2.0
+    acceleration: 3.0
+    # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
+    skip_position_tolerance_rad: 0.001
+    # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
+    skip_velocity_tolerance_rad_s: 0.01
   }
 
   perception {
-    apriltag {
-      tag_size_m: 0.12
+    # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
+    tags {
+      screen {
+        id: 1
+        size_m: 0.03
+      }
+      hand {
+        id: 0
+        size_m: 0.03
+      }
+    }
+    # 手部相机：用于点击目标点和手部目标跟踪。
+    hand_camera {
+
       depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
       target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
     }
   }
 
   alignment {
-    ibvs {
-      camera_link: "R_CAM"
-      lambda: 0.4
-      mu: 0.1
-      qdot_max: 0.8
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
+    calibration {
+      # TCP P 相对于屏幕 Hand Tag H 的目标姿态
+      hand_tag_to_tcp {
+        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
+        m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
+        m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
+        m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
+      }
+    }
+    pbvs {
+      position_gain { x: 2.0 y: 2.0 z: 1.5 }
+      rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
+      vmax6 { x: 0.10 y: 0.10 z: 0.05 rx: 0.50 ry: 0.50 rz: 0.50 }
+      amax6 { x: 0.50 y: 0.50 z: 0.30 rx: 2.0 ry: 2.0 rz: 2.0 }
       twist_filter_alpha: 1.0
-      r_camera_to_visp {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: -1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: -1.0
-      }
-      r_camera_to_urdf {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: -1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: -1.0
-      }
-      control_joint_names: "R_SHOULDER_P"
-      control_joint_names: "R_SHOULDER_R"
-      control_joint_names: "R_SHOULDER_Y"
-      control_joint_names: "R_ELBOW_R"
-      control_joint_names: "R_WRIST_P"
-      control_joint_names: "R_WRIST_Y"
-      control_joint_names: "R_WRIST_R"
     }
     target {
-      position_in_camera { x: 0.0 y: 0.0 z: 0.30 }
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
-      mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
+      # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
+      # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
+      # 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
+      # PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
+      hand_orientation_G {
+        rx: 0.0
+        ry: 0.0
+        rz: 3.141592653589793
+      }
+      # 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
+      position_offset_G {
+        x: 0.0
+        y: 0.0
+        z: 0.05
+      }
+      mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
     }
     error_threshold {
       x: 0.005
       y: 0.005
-      z: 0.010
+      z: 0.005
       rx: 0.08726646259971647
       ry: 0.08726646259971647
       rz: 0.08726646259971647
     }
     stable_frames: 5
+    # 对齐及到位暂停各自的超时时间（秒）；暂停从对齐成功时重新计时。
     timeout_s: 20.0
+    # 到位后暂停；暂停超时结束任务，after_finish 为 true 时尝试回初始位置。
     pause_when_reached: false
   }
 
@@ -78,20 +105,23 @@ 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
-      max_distance_m: 0.12
+      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
   }
 
   retract {
-    twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 8.0
-    duration_s: 5.0
+    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/aubo_arm/include/aubo_arm.h

@@ -52,6 +52,14 @@ public:
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
     Result protectiveStop() override { return emergencyStop(); }
+    Result recoverProtectiveStop(
+        const JointTrajectory&,
+        const MotionOptions&) override
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "protective recovery is not implemented for AuboArm");
+    }
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
     bool isProtectiveStopped() const override { return false; }

cmvr-es/devices/arm/huayan_arm/huayan_arm.h

@@ -48,6 +48,14 @@ public:
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
     Result protectiveStop() override { return emergencyStop(); }
+    Result recoverProtectiveStop(
+        const JointTrajectory&,
+        const MotionOptions&) override
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "protective recovery is not implemented for HuayanRobot");
+    }
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
     bool isProtectiveStopped() const override;

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
 )
@@ -34,3 +39,21 @@ target_link_libraries(motor_robot_arm_mujoco_test
         gtest_main
         pthread
 )
+
+add_executable(motor_robot_arm_gen2_mujoco_test
+        src/motor_robot_arm_gen2_mujoco_test.cpp
+)
+
+target_link_libraries(motor_robot_arm_gen2_mujoco_test
+        PRIVATE
+        cmvr_es::device::motor_robot_arm
+        cmvr_es::device::motor_manager
+        cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device_manager
+        cmvr_es::mujoco_viewer
+        cmvr_es::proto
+        cmvr_es::task
+        gtest
+        gtest_main
+        pthread
+)

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

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

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

@@ -41,11 +41,14 @@ public:
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override { return emergencyStop(); }
+    Result protectiveStop() override;
+    Result recoverProtectiveStop(
+        const JointTrajectory& path,
+        const MotionOptions& options) override;
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
-    bool isProtectiveStopped() const override { return false; }
-    bool isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isProtectiveStopped() const override { return protective_stopped_.load(); }
+    bool isEmergencyStopped() const override { return emergency_stopped_.load(); }
     bool isFault() const override { return false; }
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
@@ -59,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;
@@ -73,10 +79,10 @@ public:
     bool isConnected() const override { return motor_manager_ != nullptr; }
     Result powerOn() override { return torqueOn(); }
     Result powerOff() override { return torqueOff(); }
-    Result brakeRelease() override { return torqueOn(); }
+    Result brakeRelease() override;
     Result shutdown() override;
     Result clearFault() override { return Result::success(); }
-    Result unlockProtectiveStop() override { return Result::success(); }
+    Result unlockProtectiveStop() override;
     Result loadProgram(const std::string& program_name) override;
     Result playProgram() override;
     Result pauseProgram() override;
@@ -94,11 +100,17 @@ public:
 
 private:
     bool containsJoint_(const std::string& joint_name) const;
+    bool safetyStopRequested_() const;
+    std::optional<Result> safetyStopResult_(const std::string& command,
+                                            bool interrupted = false) const;
+    Result quickStopMotors_();
     bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const;
     bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const;
     std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const;
     bool readArmState_(std::vector<double>& q_now, std::vector<double>& qd_now) const;
     std::vector<double> readJointPosition_() const;
+    Result stopCartesianMotionAndWait_();
+    Result waitForJointTarget_(const std::vector<double>& target) const;
 
     bool configureAlgorithms_();
     bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory);
@@ -123,10 +135,20 @@ private:
     std::shared_ptr<CartesianMotionPlanner> cartesian_planner_{nullptr};
     std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{nullptr};
 
+    // MoveJ post-trajectory settling criteria.  These defaults preserve the
+    // historical behavior when the optional arm configuration fields are absent.
+    double move_j_settle_timeout_s_{2.0};
+    double move_j_position_tolerance_rad_{2e-3};
+    double move_j_velocity_tolerance_rad_s_{2e-2};
+    int move_j_stable_sample_count_{3};
+
     mutable std::mutex mutex_;
     std::atomic<bool> busy_{false};
     double speed_scaling_{1.0};
-    bool emergency_stopped_{false};
+    std::atomic<bool> protective_stopped_{false};
+    std::atomic<bool> emergency_stopped_{false};
+    std::atomic<bool> protective_recovery_active_{false};
+    std::atomic<bool> protective_recovery_cancel_requested_{false};
     ServoOptions servo_options_;
 };
 

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

@@ -1,7 +1,10 @@
 #include "arm/motor_robot_arm/include/motor_robot_arm.h"
 
+#include <algorithm>
 #include <chrono>
+#include <cmath>
 #include <Eigen/Dense>
+#include <iomanip>
 #include <stdexcept>
 #include <thread>
 #include <utility>
@@ -28,6 +31,11 @@ struct BusyGuard {
     ~BusyGuard() { busy.store(false); }
 };
 
+struct AtomicFlagGuard {
+    std::atomic<bool>& flag;
+    ~AtomicFlagGuard() { flag.store(false); }
+};
+
 } // namespace
 
 MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
@@ -132,12 +140,15 @@ bool MotorRobotArm::stop()
 
 ArmState MotorRobotArm::getRobotState() const
 {
+    const bool protective_stopped = protective_stopped_.load();
+    const bool emergency_stopped = emergency_stopped_.load();
     ArmState state;
     state.connected = motor_manager_ != nullptr;
     state.powered_on = true;
-    state.brake_released = !emergency_stopped_;
+    state.brake_released = !emergency_stopped;
     state.moving = busy();
-    state.emergency_stopped = emergency_stopped_;
+    state.protective_stopped = protective_stopped;
+    state.emergency_stopped = emergency_stopped;
     state.speed_scaling = speed_scaling_;
     state.robot_mode = RobotMode::Idle;
     state.safety_mode = getSafetyMode();
@@ -186,7 +197,13 @@ CartesianPose MotorRobotArm::getTcpPose(const FrameType frame) const
 
 SafetyMode MotorRobotArm::getSafetyMode() const
 {
-    return emergency_stopped_ ? SafetyMode::EmergencyStop : SafetyMode::Normal;
+    if (emergency_stopped_.load()) {
+        return SafetyMode::EmergencyStop;
+    }
+    if (protective_stopped_.load()) {
+        return SafetyMode::ProtectiveStop;
+    }
+    return SafetyMode::Normal;
 }
 
 Result MotorRobotArm::torqueOn()
@@ -196,9 +213,12 @@ Result MotorRobotArm::torqueOn()
         if (!motor) {
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
-        motor->brake();
+        if (!motor->torqueOn()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to torque on motor for joint: " + joint_name);
         }
-    emergency_stopped_ = false;
+    }
+    emergency_stopped_.store(false);
     return Result::success();
 }
 
@@ -209,7 +229,26 @@ Result MotorRobotArm::torqueOff()
         if (!motor) {
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
-        motor->torqueOff();
+        if (!motor->torqueOff() || !motor->brakeRelease()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to torque off motor for joint: " + joint_name);
+        }
+    }
+    return Result::success();
+}
+
+Result MotorRobotArm::brakeRelease()
+{
+    for (const auto& joint_name : joint_names_) {
+        auto motor = getMotor_(joint_name);
+        if (!motor) {
+            return Result::failure(ArmErrorCode::RobotNotReady,
+                                   "motor not found for joint: " + joint_name);
+        }
+        if (!motor->brakeRelease()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to release brake for joint: " + joint_name);
+        }
     }
     return Result::success();
 }
@@ -233,17 +272,230 @@ Result MotorRobotArm::emergencyStop()
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
+    protective_recovery_cancel_requested_.store(true);
+    protective_stopped_.store(false);
+    emergency_stopped_.store(true);
+    return quickStopMotors_();
+}
+
+Result MotorRobotArm::protectiveStop()
+{
+    if (emergency_stopped_.load()) {
+        return Result::success();
+    }
+    if (cartesian_velocity_controller_) {
+        cartesian_velocity_controller_->shutdown();
+    }
+    protective_recovery_cancel_requested_.store(true);
+    protective_stopped_.store(true);
+    return quickStopMotors_();
+}
+
+Result MotorRobotArm::recoverProtectiveStop(
+    const JointTrajectory& path,
+    const MotionOptions& options)
+{
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            "protective recovery rejected: arm is in emergency stop");
+    }
+    if (!protective_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery rejected: arm is not protective stopped");
+    }
+    if (path.size() < 2 ||
+        !std::isfinite(options.velocity) || options.velocity <= 0.0 ||
+        !std::isfinite(options.acceleration) || options.acceleration <= 0.0) {
+        return Result::failure(
+            ArmErrorCode::InvalidArgument,
+            "protective recovery path or options are invalid");
+    }
+
+    for (std::size_t i = 0; i < path.size(); ++i) {
+        const auto& sample = path[i];
+        if (!std::isfinite(sample.time_s) ||
+            sample.position.size() != joint_names_.size() ||
+            sample.velocity.size() != joint_names_.size() ||
+            (i > 0 && sample.time_s <= path[i - 1].time_s)) {
+            return Result::failure(
+                ArmErrorCode::InvalidArgument,
+                "protective recovery sample shape or time is invalid");
+        }
+        for (std::size_t joint = 0; joint < sample.position.size(); ++joint) {
+            if (!std::isfinite(sample.position[joint]) ||
+                !std::isfinite(sample.velocity[joint])) {
+                return Result::failure(
+                    ArmErrorCode::InvalidArgument,
+                    "protective recovery sample contains a non-finite value");
+            }
+        }
+    }
+
+    if (busy_.exchange(true)) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery rejected: arm is busy");
+    }
+    BusyGuard busy_guard{busy_};
+
+    bool expected = false;
+    if (!protective_recovery_active_.compare_exchange_strong(expected, true)) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery is already active");
+    }
+    AtomicFlagGuard recovery_guard{protective_recovery_active_};
+    protective_recovery_cancel_requested_.store(false);
+
+    if (!joint_planner_) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery planner is not initialized");
+    }
+
+    JointTrajectory recovery_trajectory;
+    const auto planning_start = std::chrono::steady_clock::now();
+    if (!joint_planner_->planReplay(
+            readJointPosition_(), path, options, recovery_trajectory)) {
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            "failed to plan protective recovery replay trajectory");
+    }
+    const double planning_ms = std::chrono::duration<double, std::milli>(
+        std::chrono::steady_clock::now() - planning_start).count();
+    CMVR_LOG(INFO) << "[MotorRobotArm] protective recovery planned"
+                   << ", input_samples=" << path.size()
+                   << ", command_samples=" << recovery_trajectory.size()
+                   << ", planning_ms=" << planning_ms
+                   << ", trajectory_duration_s="
+                   << recovery_trajectory.back().time_s;
+
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            "protective recovery interrupted by emergency stop during planning");
+    }
+    if (protective_recovery_cancel_requested_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInProtectiveStop,
+            "protective recovery aborted by collision monitor during planning");
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(joint_names_.size());
+    for (const auto& joint_name : joint_names_) {
+        auto motor = getMotor_(joint_name);
+        if (!motor) {
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "motor not found for joint: " + joint_name);
+        }
+        if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION &&
+            !motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "failed to set recovery position mode for joint: " + joint_name);
+        }
+        motors.push_back(std::move(motor));
+    }
+
+    const auto trajectory_start = std::chrono::steady_clock::now();
+    for (std::size_t i = 0; i < recovery_trajectory.size(); ++i) {
+        if (emergency_stopped_.load()) {
+            return Result::failure(
+                ArmErrorCode::RobotInEmergencyStop,
+                "protective recovery interrupted by emergency stop");
+        }
+        if (protective_recovery_cancel_requested_.load()) {
+            return Result::failure(
+                ArmErrorCode::RobotInProtectiveStop,
+                "protective recovery aborted by collision monitor");
+        }
+
+        const auto& sample = recovery_trajectory[i];
+        if (!motor_manager_->commandCyclicPositionsAtomic(
+                motors, sample.position, sample.velocity)) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "failed to submit protective recovery sample");
+        }
+
+        if (i + 1 < recovery_trajectory.size()) {
+            std::this_thread::sleep_until(
+                trajectory_start +
+                std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+                    std::chrono::duration<double>(
+                        recovery_trajectory[i + 1].time_s)));
+        }
+    }
+
+    const std::vector<double> zero_velocity(joint_names_.size(), 0.0);
+    if (!motor_manager_->commandCyclicPositionsAtomic(
+            motors, path.front().position, zero_velocity)) {
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            "failed to hold final protective recovery position");
+    }
+    return Result::success();
+}
+
+Result MotorRobotArm::unlockProtectiveStop()
+{
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            "cannot unlock protective stop while arm is emergency stopped");
+    }
+    if (protective_recovery_active_.load()) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "cannot unlock protective stop while recovery is active");
+    }
+    protective_stopped_.store(false);
+    return Result::success();
+}
+
+Result MotorRobotArm::quickStopMotors_()
+{
     for (const auto& joint_name : joint_names_) {
         auto motor = getMotor_(joint_name);
         if (!motor) {
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
-        motor->brake();
+        if (!motor->quickStop()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to quick stop motor for joint: " + joint_name);
+        }
     }
-    emergency_stopped_ = true;
     return Result::success();
 }
 
+bool MotorRobotArm::safetyStopRequested_() const
+{
+    return emergency_stopped_.load() || protective_stopped_.load();
+}
+
+std::optional<Result> MotorRobotArm::safetyStopResult_(
+    const std::string& command,
+    const bool interrupted) const
+{
+    const char* action = interrupted ? " interrupted by " : " rejected: arm is in ";
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            command + action + "emergency stop");
+    }
+    if (protective_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInProtectiveStop,
+            command + action + "protective stop");
+    }
+    return std::nullopt;
+}
+
 Result MotorRobotArm::setSpeedScaling(const double scaling)
 {
     if (scaling < 0.0 || scaling > 1.0) {
@@ -255,6 +507,9 @@ Result MotorRobotArm::setSpeedScaling(const double scaling)
 
 Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
 {
+    if (const auto stopped = safetyStopResult_("moveJ")) {
+        return *stopped;
+    }
     std::string error;
     if (!validatePositionCommand_(target, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
@@ -262,13 +517,23 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
     if (!joint_planner_) {
         return Result::failure(ArmErrorCode::RobotNotReady, "joint planner is not initialized");
     }
+
+    // speedL runs in a worker thread and sends speedJ commands.  A moveJ
+    // trajectory writes cyclic-position commands directly, so allowing both
+    // paths to run concurrently can overwrite the motor mode/target and cause
+    // a short surge at the transition.  Finish the Cartesian worker before
+    // reading the planning start state.
+    const auto cartesian_stop = stopCartesianMotionAndWait_();
+    if (!cartesian_stop.ok()) {
+        return cartesian_stop;
+    }
     if (busy_.exchange(true)) {
         return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_);
     }
     BusyGuard busy_guard{busy_};
     std::lock_guard<std::mutex> lock(mutex_);
 
-    std::vector<JointTrajectorySample> samples;
+    JointTrajectory samples;
     if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) {
         return Result::failure(ArmErrorCode::CommandFailed, "[MotorRobotArm] moveJ planner failed: " + id_);
     }
@@ -284,29 +549,66 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
         if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-            motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+            if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "failed to set cyclic position mode for joint: " +
+                                           joint_name);
+            }
         }
         motors.push_back(std::move(motor));
     }
 
     const auto t0 = std::chrono::steady_clock::now();
     constexpr double fallback_dt = 0.001;
+    std::vector<double> command_velocity(motors.size(), 0.0);
     for (std::size_t k = 1; k < samples.size(); ++k) {
+        if (const auto stopped = safetyStopResult_("moveJ", true)) {
+            return *stopped;
+        }
         const auto& sample = samples[k];
         if (sample.position.size() != motors.size()) {
             return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
         }
-        for (std::size_t i = 0; i < motors.size(); ++i) {
-            const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0;
-            motors[i]->setTarget(sample.position[i], qd);
+        std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
+        // A MoveJ is a rest-to-rest command.  Do not let a non-zero numerical
+        // endpoint velocity from an alternate planner keep the drive moving
+        // while the position-mode trajectory is being handed back to the arm.
+        if (k + 1 < samples.size()) {
+            std::copy_n(sample.velocity.begin(),
+                        std::min(sample.velocity.size(), command_velocity.size()),
+                        command_velocity.begin());
+        }
+        if (!motor_manager_->commandCyclicPositionsAtomic(
+                motors, sample.position, command_velocity)) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to submit atomic cyclic position command");
         }
         if (k + 1 < samples.size()) {
-            const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t
+            const double next_t = samples[k + 1].time_s > 0.0
+                                      ? samples[k + 1].time_s
                                                          : static_cast<double>(k + 1) * fallback_dt;
             std::this_thread::sleep_until(t0 + std::chrono::duration_cast<std::chrono::steady_clock::duration>(
                                                    std::chrono::duration<double>(next_t)));
         }
     }
+
+    // Repeat the final position with zero velocity before checking feedback.
+    // This seeds the position-mode target after a velocity-to-position switch
+    // and prevents a stale final velocity command from producing a short
+    // motion spike at the destination.
+    if (!motor_manager_->commandCyclicPositionsAtomic(
+            motors, target.position, std::vector<double>(motors.size(), 0.0))) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "failed to hold final moveJ target");
+    }
+    // Allow one servo cycle to consume the explicit hold command before
+    // evaluating feedback-based settling criteria.
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+    const auto settle_result = waitForJointTarget_(target.position);
+    if (!settle_result.ok()) {
+        return settle_result;
+    }
     return Result::success();
 }
 
@@ -315,6 +617,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
                              const double duration)
 {
     (void)acceleration;
+    if (const auto stopped = safetyStopResult_("speedJ")) {
+        return *stopped;
+    }
     std::string error;
     if (!validateVelocityCommand_(velocity, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
@@ -329,9 +634,17 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
                                        "motor not found for joint: " + joint_names_[i]);
             }
             if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
-                motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
+                if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) {
+                    return Result::failure(ArmErrorCode::CommandFailed,
+                                           "failed to set cyclic velocity mode for joint: " +
+                                               joint_names_[i]);
+                }
+            }
+            if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "failed to command cyclic velocity for joint: " +
+                                           joint_names_[i]);
             }
-            motor->setTarget(velocity.velocity[i] * speed_scaling_);
         }
     }
 
@@ -344,6 +657,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
 
 Result MotorRobotArm::stopJ(const double acceleration)
 {
+    if (safetyStopRequested_()) {
+        return Result::success();
+    }
     JointVelocityCommand zero;
     zero.velocity.assign(joint_names_.size(), 0.0);
     return speedJ(zero, acceleration, 0.0);
@@ -353,8 +669,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                             const MotionOptions& options,
                             const FrameType frame)
 {
-    if (cartesian_velocity_controller_) {
-        cartesian_velocity_controller_->shutdown();
+    if (const auto stopped = safetyStopResult_("moveL")) {
+        return *stopped;
+    }
+    const auto cartesian_stop = stopCartesianMotionAndWait_();
+    if (!cartesian_stop.ok()) {
+        return cartesian_stop;
     }
     if (options.asynchronous) {
         return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported");
@@ -394,8 +714,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                           << ", executable_path_m=" << trajectory.executable_path_length;
     }
 
-    return executeMoveLTrajectory_(trajectory) ? Result::success()
-                                               : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
+    if (executeMoveLTrajectory_(trajectory)) {
+        return Result::success();
+    }
+    if (const auto stopped = safetyStopResult_("moveL", true)) {
+        return *stopped;
+    }
+    return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
 }
 
 Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@@ -403,6 +728,9 @@ Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
                              const double duration,
                              const FrameType frame)
 {
+    if (const auto stopped = safetyStopResult_("speedL")) {
+        return *stopped;
+    }
     if (busy_.load()) {
         return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
     }
@@ -420,9 +748,27 @@ 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()
 {
-    stopL(0.0);
+    const auto cartesian_stop = stopCartesianMotionAndWait_();
+    if (!cartesian_stop.ok()) {
+        return cartesian_stop;
+    }
     return stopJ(0.0);
 }
 
@@ -442,12 +788,17 @@ Result MotorRobotArm::startServoMode(const ServoOptions& options)
 
 Result MotorRobotArm::servoJ(const JointPositionCommand& target)
 {
+    if (const auto stopped = safetyStopResult_("servoJ")) {
+        return *stopped;
+    }
     std::string error;
     if (!validatePositionCommand_(target, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
     }
 
     std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(joint_names_.size());
     for (std::size_t i = 0; i < joint_names_.size(); ++i) {
         auto motor = getMotor_(joint_names_[i]);
         if (!motor) {
@@ -455,9 +806,18 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target)
                                    "motor not found for joint: " + joint_names_[i]);
         }
         if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-            motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+            if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "failed to set cyclic position mode for joint: " +
+                                           joint_names_[i]);
             }
-        motor->setTarget(target.position[i], 0.0);
+        }
+        motors.push_back(std::move(motor));
+    }
+    const std::vector<double> velocities(motors.size(), 0.0);
+    if (!motor_manager_->commandCyclicPositionsAtomic(motors, target.position, velocities)) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "failed to submit atomic cyclic position command");
     }
     return Result::success();
 }
@@ -656,8 +1016,181 @@ std::vector<double> MotorRobotArm::readJointPosition_() const
     return q_start;
 }
 
+Result MotorRobotArm::stopCartesianMotionAndWait_()
+{
+    if (!cartesian_velocity_controller_) {
+        return Result::success();
+    }
+
+    if (cartesian_velocity_controller_->busy()) {
+        const auto stop_result = cartesian_velocity_controller_->stop();
+        if (!stop_result.ok()) {
+            return stop_result;
+        }
+
+        const auto deadline = std::chrono::steady_clock::now() +
+                              std::chrono::duration<double>(
+                                  cartesian_velocity_controller_->stopTimeoutS());
+        while (cartesian_velocity_controller_->busy() &&
+               std::chrono::steady_clock::now() < deadline) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        if (cartesian_velocity_controller_->busy()) {
+            // Do not start a position trajectory while the worker can still
+            // issue velocity commands.  Shutdown joins it and sends one final
+            // zero-velocity command before reporting the timeout.
+            cartesian_velocity_controller_->shutdown();
+            return Result::failure(
+                ArmErrorCode::Timeout,
+                "timed out waiting for Cartesian velocity motion to stop");
+        }
+    }
+
+    // The worker may be idle but still joinable.  Joining it here removes any
+    // last command/worker race before the next motion mode is selected.
+    cartesian_velocity_controller_->shutdown();
+    return Result::success();
+}
+
+Result MotorRobotArm::waitForJointTarget_(const std::vector<double>& target) const
+{
+    if (target.size() != joint_names_.size()) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "moveJ target size mismatch while settling");
+    }
+
+    struct JointFeedback {
+        double position{0.0};
+        double velocity{0.0};
+    };
+    std::vector<JointFeedback> last_feedback(joint_names_.size());
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::duration<double>(move_j_settle_timeout_s_);
+    std::size_t sample_count = 0;
+    int stable_samples = 0;
+    int max_stable_samples = 0;
+    while (std::chrono::steady_clock::now() < deadline) {
+        if (const auto stopped = safetyStopResult_("moveJ", true)) {
+            return *stopped;
+        }
+
+        bool settled = true;
+        for (std::size_t i = 0; i < joint_names_.size(); ++i) {
+            const auto motor = getMotor_(joint_names_[i]);
+            if (!motor) {
+                return Result::failure(
+                    ArmErrorCode::RobotNotReady,
+                    "motor not found while waiting for moveJ target: " + joint_names_[i]);
+            }
+            const double position = motor->getQ();
+            const double velocity = motor->getQd();
+            last_feedback[i] = {position, velocity};
+            if (!std::isfinite(position) || !std::isfinite(velocity) ||
+                std::abs(position - target[i]) > move_j_position_tolerance_rad_ ||
+                std::abs(velocity) > move_j_velocity_tolerance_rad_s_) {
+                settled = false;
+            }
+        }
+
+        ++sample_count;
+        stable_samples = settled ? stable_samples + 1 : 0;
+        max_stable_samples = std::max(max_stable_samples, stable_samples);
+        if (stable_samples >= move_j_stable_sample_count_) {
+            return Result::success();
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+    CMVR_LOG(ERROR) << "[MotorRobotArm] moveJ target did not settle before timeout: " << id_
+                   << ", timeout_s=" << move_j_settle_timeout_s_
+                   << ", sample_count=" << sample_count
+                   << ", stable_samples=" << stable_samples
+                   << ", required_stable_samples=" << move_j_stable_sample_count_
+                   << ", max_stable_samples=" << max_stable_samples
+                   << ", reason=" << (sample_count == 0 ? "no_feedback_samples"
+                                       : stable_samples > 0 ? "insufficient_stable_samples"
+                                                            : "joint_feedback_not_settled");
+    // Report the feedback from the final check, rather than reading newer
+    // values that may no longer explain the timeout. Include every joint so
+    // valid axes and non-finite feedback are distinguishable in the same sample.
+    for (std::size_t i = 0; sample_count > 0 && i < joint_names_.size(); ++i) {
+        const auto& feedback = last_feedback[i];
+        const double position_error = std::abs(feedback.position - target[i]);
+        const bool position_ok = std::isfinite(feedback.position) &&
+                                 position_error <= move_j_position_tolerance_rad_;
+        const bool velocity_ok = std::isfinite(feedback.velocity) &&
+                                 std::abs(feedback.velocity) <= move_j_velocity_tolerance_rad_s_;
+        CMVR_LOG(ERROR) << std::setprecision(12)
+                       << "[MotorRobotArm][MOVEJ_SETTLE_CHECK] arm=" << id_
+                       << ", joint=" << joint_names_[i]
+                       << ", target_rad=" << target[i]
+                       << ", actual_rad=" << feedback.position
+                       << ", abs_position_error_rad=" << position_error
+                       << ", position_tolerance_rad=" << move_j_position_tolerance_rad_
+                       << ", position_ok=" << (position_ok ? "true" : "false")
+                       << ", velocity_rad_s=" << feedback.velocity
+                       << ", velocity_tolerance_rad_s=" << move_j_velocity_tolerance_rad_s_
+                       << ", velocity_ok=" << (velocity_ok ? "true" : "false");
+    }
+    return Result::failure(ArmErrorCode::Timeout,
+                           "timed out waiting for moveJ target to settle");
+}
+
 bool MotorRobotArm::configureAlgorithms_()
 {
+    // Optional settling fields are read once during initialization so every
+    // MoveJ command uses one consistent set of safety criteria.
+    move_j_settle_timeout_s_ = 2.0;
+    move_j_position_tolerance_rad_ = 2e-3;
+    move_j_velocity_tolerance_rad_s_ = 2e-2;
+    move_j_stable_sample_count_ = 3;
+    const auto& move_j_config = cfg_.motion().move_j();
+    if (move_j_config.has_settle_timeout_s()) {
+        const double value = move_j_config.settle_timeout_s();
+        if (!std::isfinite(value) || value <= 0.0) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_timeout_s: " << value;
+            return false;
+        }
+        move_j_settle_timeout_s_ = value;
+    }
+    if (move_j_config.has_settle_position_tolerance_rad()) {
+        const double value = move_j_config.settle_position_tolerance_rad();
+        if (!std::isfinite(value) || value < 0.0) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_position_tolerance_rad: "
+                            << value;
+            return false;
+        }
+        move_j_position_tolerance_rad_ = value;
+    }
+    if (move_j_config.has_settle_velocity_tolerance_rad_s()) {
+        const double value = move_j_config.settle_velocity_tolerance_rad_s();
+        if (!std::isfinite(value) || value < 0.0) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_velocity_tolerance_rad_s: "
+                            << value;
+            return false;
+        }
+        move_j_velocity_tolerance_rad_s_ = value;
+    }
+    if (move_j_config.has_settle_stable_sample_count()) {
+        const int value = move_j_config.settle_stable_sample_count();
+        if (value < 1) {
+            CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_stable_sample_count: "
+                            << value;
+            return false;
+        }
+        move_j_stable_sample_count_ = value;
+    }
+
+    const auto& cartesian_controller_config =
+        cfg_.motion().speed_l().speed_l_controller().cartesian_velocity_controller();
+    if (cartesian_controller_config.has_stop_timeout_s() &&
+        (!std::isfinite(cartesian_controller_config.stop_timeout_s()) ||
+         cartesian_controller_config.stop_timeout_s() <= 0.0)) {
+        CMVR_LOG(ERROR) << "[MotorRobotArm] invalid Cartesian stop_timeout_s: "
+                        << cartesian_controller_config.stop_timeout_s();
+        return false;
+    }
+
     joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j());
     if (!joint_planner_) {
         return false;
@@ -725,7 +1258,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
                 return false;
             }
             if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-                motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+                if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                    return false;
+                }
             }
             motors.push_back(std::move(motor));
         }
@@ -733,14 +1268,17 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
 
     auto next_deadline = std::chrono::steady_clock::now();
     for (std::size_t i = 1; i < trajectory.position.size(); ++i) {
+        if (safetyStopRequested_()) {
+            return false;
+        }
         const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]);
         const auto& position = trajectory.position[i];
         const auto& velocity = trajectory.velocity[i];
         if (position.size() != motors.size() || velocity.size() != motors.size()) {
             return false;
         }
-        for (std::size_t j = 0; j < motors.size(); ++j) {
-            motors[j]->setTarget(position[j], velocity[j]);
+        if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
+            return false;
         }
         next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
             std::chrono::duration<double>(dt_segment));
@@ -765,6 +1303,10 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController
     result.stop_acceleration =
         config.stop_acceleration() > 0.0 ? config.stop_acceleration()
                                          : result.stop_acceleration;
+    if (config.has_stop_timeout_s() && std::isfinite(config.stop_timeout_s()) &&
+        config.stop_timeout_s() > 0.0) {
+        result.stop_timeout_s = config.stop_timeout_s();
+    }
     return result;
 }
 

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

@@ -0,0 +1,871 @@
+#include "arm/motor_robot_arm/include/motor_robot_arm.h"
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <filesystem>
+#include <functional>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Geometry>
+#include <gtest/gtest.h>
+
+#include "common/io/proto_file_io.h"
+#include "common/math/transform_math.h"
+#include "manager/device_manager/include/device_manager.h"
+#include "devices/motor/manager/include/motor_manager.h"
+#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+#include "task/self_collision_task/include/self_collision_task.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr std::size_t kDof = 7;
+constexpr std::array<const char*, kDof> kJointNames = {
+    "right_arm_J1", "right_arm_J2", "right_arm_J3", "right_arm_J4",
+    "right_arm_J5", "right_arm_J6", "right_arm_J7"
+};
+
+const std::vector<double> kSetupPose{
+    0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0
+};
+
+const std::vector<double> kTorsoCollisionPose{
+    1.57607137794121,
+    2.06613762981425,
+    -1.76915077905899,
+    0.959251437141443,
+    -0.725973209527894,
+    1.79390262120717,
+    0.2223354372144,
+};
+
+std::filesystem::path findProjectRoot()
+{
+    const std::filesystem::path marker = "model/gen2/gen2_fixed.xml";
+    const auto search = [&](std::filesystem::path current) {
+        while (!current.empty()) {
+            if (std::filesystem::exists(current / marker)) {
+                return current;
+            }
+            const auto parent = current.parent_path();
+            if (parent == current) {
+                break;
+            }
+            current = parent;
+        }
+        return std::filesystem::path{};
+    };
+
+    auto root = search(std::filesystem::current_path());
+    if (!root.empty()) {
+        return root;
+    }
+    return search(std::filesystem::path(__FILE__).parent_path());
+}
+
+double maxPositionError(const std::vector<double>& actual,
+                        const std::vector<double>& expected)
+{
+    if (actual.size() != expected.size()) {
+        return std::numeric_limits<double>::infinity();
+    }
+    double error = 0.0;
+    for (std::size_t i = 0; i < actual.size(); ++i) {
+        error = std::max(error, std::abs(actual[i] - expected[i]));
+    }
+    return error;
+}
+
+double translationError(const CartesianPose& lhs, const CartesianPose& rhs)
+{
+    return std::sqrt(std::pow(lhs.x - rhs.x, 2.0) +
+                     std::pow(lhs.y - rhs.y, 2.0) +
+                     std::pow(lhs.z - rhs.z, 2.0));
+}
+
+double rotationError(const CartesianPose& lhs, const CartesianPose& rhs)
+{
+    const Eigen::Matrix3d lhs_rotation =
+        common::math::poseToMatrix(lhs).block<3, 3>(0, 0);
+    const Eigen::Matrix3d rhs_rotation =
+        common::math::poseToMatrix(rhs).block<3, 3>(0, 0);
+    return std::abs(Eigen::AngleAxisd(lhs_rotation.transpose() * rhs_rotation).angle());
+}
+
+Eigen::Vector3d baseRotationDelta(const CartesianPose& start, const CartesianPose& end)
+{
+    const Eigen::Matrix3d start_rotation =
+        common::math::poseToMatrix(start).block<3, 3>(0, 0);
+    const Eigen::Matrix3d end_rotation =
+        common::math::poseToMatrix(end).block<3, 3>(0, 0);
+    const Eigen::AngleAxisd delta(end_rotation * start_rotation.transpose());
+    return delta.axis() * delta.angle();
+}
+
+template <class Predicate>
+void waitFor(Predicate predicate, const std::chrono::milliseconds timeout)
+{
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (!predicate() && std::chrono::steady_clock::now() < deadline) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+struct ScenarioOutcome {
+    Result move_j{Result::failure(ArmErrorCode::UnknownError, "not run")};
+    Result move_l{Result::failure(ArmErrorCode::UnknownError, "not run")};
+    double move_j_error{std::numeric_limits<double>::infinity()};
+    double move_l_error{std::numeric_limits<double>::infinity()};
+    double move_l_rotation_error{std::numeric_limits<double>::infinity()};
+    std::string worker_error;
+};
+
+class MotorRobotArmGen2MujocoTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        DeviceManager::destroyInstance();
+        project_root_ = findProjectRoot();
+        ASSERT_FALSE(project_root_.empty());
+
+        config::MujocoWorldRootConfig world_root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(),
+            &world_root_config));
+        ASSERT_GT(world_root_config.worlds_size(), 0);
+
+        auto world_config = world_root_config.worlds(0);
+        world_config.set_model_path(
+            (project_root_ / "model/gen2/gen2_fixed.xml").string());
+        world_device_ = std::make_shared<simulate::MujocoWorldDevice>(world_config);
+        ASSERT_TRUE(world_device_->init());
+        ASSERT_TRUE(world_device_->start());
+
+        config::MotorRootConfig motor_root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ /
+             "cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt").string(),
+            &motor_root_config));
+
+        motor_system_ = std::make_shared<MotorManager>(
+            "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
+        ASSERT_TRUE(motor_system_->init());
+        world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
+        ASSERT_TRUE(world_);
+        ASSERT_TRUE(world_->isLoaded());
+
+        config::ArmRootConfig root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ /
+             "cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt").string(),
+            &root_config));
+        ASSERT_GT(root_config.arm().robot_arms_size(), 0);
+
+        auto arm_config = root_config.arm().robot_arms(0);
+        arm_config.mutable_kinematics()
+            ->mutable_pinocchio_dls_ik_solver()
+            ->set_urdf_path((project_root_ / "model/gen2/robot.urdf").string());
+
+        arm_ = std::make_shared<MotorRobotArm>(arm_config);
+        ASSERT_TRUE(arm_->init());
+        const Result torque_result = arm_->torqueOn();
+        ASSERT_TRUE(torque_result.ok()) << torque_result.message;
+
+        config::DeviceManagerConfig device_manager_config;
+        device_manager_config.set_name("gen2_collision_mujoco_test");
+        DeviceManager::getInstance(device_manager_config).registerDevice(arm_);
+    }
+
+    void TearDown() override
+    {
+        if (arm_) {
+            arm_->stop();
+        }
+        if (motor_system_) {
+            motor_system_->stop();
+        }
+        if (world_device_) {
+            world_device_->stop();
+        }
+        DeviceManager::destroyInstance();
+    }
+
+    std::filesystem::path project_root_;
+    std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
+    std::shared_ptr<MotorManager> motor_system_;
+    std::shared_ptr<simulate::MujocoWorld> world_;
+    std::shared_ptr<MotorRobotArm> arm_;
+};
+
+TEST_F(MotorRobotArmGen2MujocoTest, HoldsInitialPosition)
+{
+    const auto start = arm_->getJointState().position;
+    ASSERT_EQ(start.size(), kDof);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    const auto end = arm_->getJointState().position;
+    const double drift = maxPositionError(end, start);
+
+    std::cout << "[MotorRobotArmGen2MujocoTest] hold max drift: "
+              << drift << std::endl;
+    EXPECT_LT(drift, 0.02);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, ProtectiveAndEmergencyStopAreDistinct)
+{
+    ASSERT_TRUE(arm_->protectiveStop().ok());
+    EXPECT_TRUE(arm_->isProtectiveStopped());
+    EXPECT_FALSE(arm_->isEmergencyStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::ProtectiveStop);
+    const auto protective_state = arm_->getRobotState();
+    EXPECT_TRUE(protective_state.protective_stopped);
+    EXPECT_FALSE(protective_state.emergency_stopped);
+
+    MotionOptions options;
+    options.velocity = 0.6;
+    options.acceleration = 2.0;
+    const Result protected_move = arm_->moveJ(
+        JointPositionCommand{kSetupPose}, options);
+    EXPECT_EQ(protected_move.code, ArmErrorCode::RobotInProtectiveStop);
+
+    ASSERT_TRUE(arm_->unlockProtectiveStop().ok());
+    EXPECT_FALSE(arm_->isProtectiveStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
+
+    ASSERT_TRUE(arm_->emergencyStop().ok());
+    EXPECT_FALSE(arm_->isProtectiveStopped());
+    EXPECT_TRUE(arm_->isEmergencyStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::EmergencyStop);
+    const auto emergency_state = arm_->getRobotState();
+    EXPECT_FALSE(emergency_state.protective_stopped);
+    EXPECT_TRUE(emergency_state.emergency_stopped);
+
+    const Result rejected_unlock = arm_->unlockProtectiveStop();
+    EXPECT_EQ(rejected_unlock.code, ArmErrorCode::RobotInEmergencyStop);
+    ASSERT_TRUE(arm_->torqueOn().ok());
+    EXPECT_FALSE(arm_->isEmergencyStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, MoveJ)
+{
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+    ScenarioOutcome outcome;
+
+    std::thread scenario([&] {
+        try {
+            if (!world_ || !world_->isRunning()) {
+                throw std::runtime_error("MuJoCo world is not running");
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            MotionOptions options;
+            options.velocity = 0.6;
+            options.acceleration = 2.0;
+
+            outcome.move_j = arm_->moveJ(JointPositionCommand{kSetupPose}, options);
+            waitFor([&] {
+                return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
+            }, std::chrono::seconds(3));
+            outcome.move_j_error = maxPositionError(
+                arm_->getJointState().position, kSetupPose);
+        } catch (const std::exception& error) {
+            outcome.worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    std::cout << "[MotorRobotArmGen2MujocoTest] moveJ max error: "
+              << outcome.move_j_error << std::endl;
+    EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
+    EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
+    EXPECT_LT(outcome.move_j_error, 0.08);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, MoveL)
+{
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+    ScenarioOutcome outcome;
+
+    std::thread scenario([&] {
+        try {
+            if (!world_ || !world_->isRunning()) {
+                throw std::runtime_error("MuJoCo world is not running");
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            MotionOptions joint_options;
+            joint_options.velocity = 1.6;
+            joint_options.acceleration = 12.0;
+            outcome.move_j = arm_->moveJ(
+                JointPositionCommand{kSetupPose}, joint_options);
+            waitFor([&] {
+                return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
+            }, std::chrono::seconds(3));
+            outcome.move_j_error = maxPositionError(
+                arm_->getJointState().position, kSetupPose);
+            if (!outcome.move_j.ok()) {
+                throw std::runtime_error(outcome.move_j.message);
+            }
+
+            MotionOptions cartesian_options;
+            cartesian_options.velocity = 0.08;
+            cartesian_options.acceleration = 0.4;
+            cartesian_options.jerk = 1.0;
+
+            MotionOptions rotation_options;
+            rotation_options.velocity = 0.15;
+            rotation_options.acceleration = 0.5;
+            rotation_options.jerk = 2.0;
+
+            const auto return_to_setup = [&](const char* step_name) {
+                outcome.move_j = arm_->moveJ(
+                    JointPositionCommand{kSetupPose}, joint_options);
+                if (!outcome.move_j.ok()) {
+                    throw std::runtime_error(
+                        std::string("moveJ before moveL ") + step_name +
+                        ": " + outcome.move_j.message);
+                }
+                waitFor([&] {
+                    return maxPositionError(
+                               arm_->getJointState().position, kSetupPose) < 0.04;
+                }, std::chrono::seconds(3));
+                std::this_thread::sleep_for(std::chrono::milliseconds(300));
+            };
+
+            struct CartesianStep {
+                const char* name;
+                double dx;
+                double dy;
+                double dz;
+            };
+            const std::array<CartesianStep, 3> translation_steps{{
+                {"+X", 0.15, 0.0, 0.0},
+                {"+Y", 0.0, 0.15, 0.0},
+                {"+Z", 0.0, 0.0, 0.15},
+            }};
+
+            struct RotationStep {
+                const char* name;
+                double drx;
+                double dry;
+                double drz;
+            };
+            constexpr double kRotationStep =
+                20.0 * 3.14159265358979323846 / 180.0;
+            const std::array<RotationStep, 3> rotation_steps{{
+                {"+RX", kRotationStep, 0.0, 0.0},
+                {"+RY", 0.0, kRotationStep, 0.0},
+                {"+RZ", 0.0, 0.0, kRotationStep},
+            }};
+
+            outcome.move_l_error = 0.0;
+            for (std::size_t i = 0; i < translation_steps.size(); ++i) {
+                const auto& step = translation_steps[i];
+                if (i > 0) {
+                    return_to_setup(step.name);
+                }
+                CartesianPose target = arm_->getTcpPose();
+                target.x += step.dx;
+                target.y += step.dy;
+                target.z += step.dz;
+
+                outcome.move_l = arm_->moveL(
+                    target, cartesian_options, FrameType::Base);
+                if (!outcome.move_l.ok()) {
+                    throw std::runtime_error(
+                        std::string("moveL ") + step.name + ": " +
+                        outcome.move_l.message);
+                }
+                waitFor([&] {
+                    return translationError(arm_->getTcpPose(), target) < 0.005;
+                }, std::chrono::seconds(5));
+                const double error = translationError(arm_->getTcpPose(), target);
+                outcome.move_l_error = std::max(outcome.move_l_error, error);
+                std::cout << "[MotorRobotArmGen2MujocoTest] moveL "
+                          << step.name << " translation error: " << error
+                          << std::endl;
+                std::this_thread::sleep_for(std::chrono::milliseconds(500));
+            }
+
+            outcome.move_l_rotation_error = 0.0;
+            for (const auto& step : rotation_steps) {
+                return_to_setup(step.name);
+                CartesianPose target = arm_->getTcpPose();
+                target.rx += step.drx;
+                target.ry += step.dry;
+                target.rz += step.drz;
+
+                outcome.move_l = arm_->moveL(
+                    target, rotation_options, FrameType::Base);
+                if (!outcome.move_l.ok()) {
+                    throw std::runtime_error(
+                        std::string("moveL ") + step.name + ": " +
+                        outcome.move_l.message);
+                }
+                waitFor([&] {
+                    return rotationError(arm_->getTcpPose(), target) < 0.01;
+                }, std::chrono::seconds(7));
+                const double error = rotationError(arm_->getTcpPose(), target);
+                outcome.move_l_rotation_error = std::max(
+                    outcome.move_l_rotation_error, error);
+                std::cout << "[MotorRobotArmGen2MujocoTest] moveL "
+                          << step.name << " rotation error: " << error
+                          << " rad" << std::endl;
+                std::this_thread::sleep_for(std::chrono::milliseconds(500));
+            }
+        } catch (const std::exception& error) {
+            outcome.worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(3));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    std::cout << "[MotorRobotArmGen2MujocoTest] moveJ setup max error: "
+              << outcome.move_j_error
+              << ", moveL translation error: " << outcome.move_l_error
+              << ", moveL rotation error: "
+              << outcome.move_l_rotation_error << " rad" << std::endl;
+    EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
+    EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
+    EXPECT_LT(outcome.move_j_error, 0.08);
+    EXPECT_TRUE(outcome.move_l.ok()) << outcome.move_l.message;
+    EXPECT_LT(outcome.move_l_error, 0.01);
+    EXPECT_LT(outcome.move_l_rotation_error, 0.02);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, SelfCollisionProtectiveStopMoveJMoveLSpeedL)
+{
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+
+    struct CollisionCaseOutcome {
+        std::string name;
+        Result setup_move{Result::failure(ArmErrorCode::UnknownError, "not run")};
+        Result collision_move{Result::failure(ArmErrorCode::UnknownError, "not run")};
+        Result recovery{Result::failure(ArmErrorCode::UnknownError, "not run")};
+        task::SelfCollisionTaskStatus initial_status;
+        task::SelfCollisionTaskStatus stop_status;
+        task::SelfCollisionTaskStatus recovered_status;
+        CartesianPose start_tcp;
+        CartesianPose final_tcp;
+        std::vector<double> final_position;
+        bool task_initialized{false};
+        bool task_started{false};
+        bool stop_seen{false};
+        bool recovery_succeeded{false};
+        bool monitor_ok{true};
+        bool protective_stopped{false};
+        bool emergency_stopped{false};
+        SafetyMode safety_mode{SafetyMode::Unknown};
+    };
+
+    CollisionCaseOutcome move_j_outcome;
+    CollisionCaseOutcome move_l_outcome;
+    CollisionCaseOutcome speed_l_outcome;
+    CartesianPose move_l_target;
+    CartesianPose collision_tcp_target;
+    std::string worker_error;
+
+    std::thread scenario([&] {
+        try {
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            config::SelfCollisionTaskRootConfig root_config;
+            const auto config_path = project_root_ /
+                "cmvr-es/config/tasks/self_collision_task/"
+                "self_collision_task_gen2.pb.txt";
+            if (!ProtoMessageIo::getProtoFromAsciiFile(
+                    config_path.string(), &root_config)) {
+                throw std::runtime_error(
+                    "failed to load self-collision config: " + config_path.string());
+            }
+            auto collision_config = root_config.self_collision_task();
+            collision_config.mutable_checker()->set_urdf_path(
+                (project_root_ /
+                 "model/gen2/collision/robot_collision.urdf").string());
+
+            Eigen::Matrix4d collision_tcp_transform = Eigen::Matrix4d::Identity();
+            const auto solver = arm_->kinematicsSolver();
+            if (!solver ||
+                !solver->fk(kTorsoCollisionPose, collision_tcp_transform, true)) {
+                throw std::runtime_error("failed to calculate collision TCP target");
+            }
+            collision_tcp_target =
+                common::math::matrixToPose(collision_tcp_transform);
+
+            const auto run_collision_case = [&](
+                const std::string& name,
+                const std::function<Result()>& start_motion,
+                const std::chrono::milliseconds stop_timeout) {
+                CollisionCaseOutcome outcome;
+                outcome.name = name;
+
+                const Result torque_result = arm_->torqueOn();
+                if (!torque_result.ok()) {
+                    throw std::runtime_error(
+                        name + " torqueOn: " + torque_result.message);
+                }
+
+                MotionOptions setup_options;
+                setup_options.velocity = 0.6;
+                setup_options.acceleration = 2.0;
+                outcome.setup_move = arm_->moveJ(
+                    JointPositionCommand{kSetupPose}, setup_options);
+                if (!outcome.setup_move.ok()) {
+                    throw std::runtime_error(
+                        name + " setup moveJ: " + outcome.setup_move.message);
+                }
+
+                task::SelfCollisionTask collision_task(collision_config);
+                outcome.task_initialized = collision_task.init();
+                if (!outcome.task_initialized) {
+                    throw std::runtime_error(
+                        name + " init: " + collision_task.detailStatusString());
+                }
+                outcome.task_started = collision_task.start();
+                if (!outcome.task_started || !collision_task.step(0.002)) {
+                    throw std::runtime_error(
+                        name + " start: " + collision_task.detailStatusString());
+                }
+                outcome.initial_status = collision_task.latestStatus();
+                if (outcome.initial_status.level != task::CollisionSafetyLevel::SAFE) {
+                    throw std::runtime_error(
+                        name + " setup pose is not SAFE: " +
+                        collision_task.detailStatusString());
+                }
+                outcome.start_tcp = arm_->getTcpPose();
+
+                std::atomic_bool monitor_running{true};
+                std::atomic_bool monitor_ok{true};
+                std::atomic_bool stop_seen{false};
+                std::thread monitor([&] {
+                    while (monitor_running.load()) {
+                        if (!collision_task.step(0.002)) {
+                            monitor_ok = false;
+                            break;
+                        }
+                        const auto status = collision_task.latestStatus();
+                        if (status.stop_latched && !stop_seen.load()) {
+                            outcome.stop_status = status;
+                            stop_seen.store(true);
+                        }
+                        std::this_thread::sleep_for(std::chrono::milliseconds(2));
+                    }
+                });
+
+                outcome.collision_move = start_motion();
+                waitFor([&] {
+                    return stop_seen.load() || !monitor_ok.load();
+                }, stop_timeout);
+                if (!stop_seen.load()) {
+                    arm_->stopMotion();
+                    monitor_running = false;
+                    monitor.join();
+                    collision_task.stop();
+                    throw std::runtime_error(name + " did not trigger protective stop");
+                }
+
+                outcome.recovery = collision_task.requestRecovery(
+                    outcome.stop_status.event_id);
+                outcome.recovered_status = collision_task.latestStatus();
+                outcome.recovery_succeeded = outcome.recovery.ok();
+
+                monitor_running = false;
+                monitor.join();
+                collision_task.stop();
+                outcome.stop_seen = stop_seen.load();
+                outcome.monitor_ok = monitor_ok.load();
+                outcome.final_position = arm_->getJointState().position;
+                outcome.final_tcp = arm_->getTcpPose();
+                outcome.protective_stopped = arm_->isProtectiveStopped();
+                outcome.emergency_stopped = arm_->isEmergencyStopped();
+                outcome.safety_mode = arm_->getSafetyMode();
+
+                std::cout << "[MotorRobotArmGen2MujocoTest] collision "
+                          << name
+                          << " stop_seen=" << outcome.stop_seen
+                          << ", distance_m="
+                          << outcome.stop_status.result.minimum_distance_m
+                          << ", pair=" << outcome.stop_status.result.first
+                          << "/" << outcome.stop_status.result.second
+                          << ", event_id=" << outcome.stop_status.event_id
+                          << ", recovery=" << outcome.recovery.message
+                          << ", recovered_distance_m="
+                          << outcome.recovered_status.result.minimum_distance_m
+                          << std::endl;
+                return outcome;
+            };
+
+            move_j_outcome = run_collision_case(
+                "MoveJ",
+                [&] {
+                    MotionOptions options;
+                    options.velocity = 0.45;
+                    options.acceleration = 1.0;
+                    return arm_->moveJ(
+                        JointPositionCommand{kTorsoCollisionPose}, options);
+                },
+                std::chrono::seconds(3));
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+
+            move_l_outcome = run_collision_case(
+                "MoveL",
+                [&] {
+                    move_l_target = collision_tcp_target;
+                    MotionOptions options;
+                    options.velocity = 0.12;
+                    options.acceleration = 0.5;
+                    options.jerk = 2.0;
+                    return arm_->moveL(
+                        move_l_target, options, FrameType::Base);
+                },
+                std::chrono::seconds(3));
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+
+            speed_l_outcome = run_collision_case(
+                "SpeedL",
+                [&] {
+                    const CartesianPose start = arm_->getTcpPose();
+                    Eigen::Vector3d linear_direction{
+                        collision_tcp_target.x - start.x,
+                        collision_tcp_target.y - start.y,
+                        collision_tcp_target.z - start.z,
+                    };
+                    Eigen::Vector3d angular_direction =
+                        baseRotationDelta(start, collision_tcp_target);
+                    const double command_duration_s = std::max(
+                        linear_direction.norm() / 0.05,
+                        angular_direction.norm() / 0.20);
+                    if (command_duration_s <= 0.0) {
+                        return Result::failure(
+                            ArmErrorCode::InvalidArgument,
+                            "SpeedL collision target has zero displacement");
+                    }
+                    linear_direction /= command_duration_s;
+                    angular_direction /= command_duration_s;
+                    std::cout
+                        << "[MotorRobotArmGen2MujocoTest] collision SpeedL target_time="
+                        << command_duration_s << " s" << std::endl;
+                    return arm_->speedL(
+                        CartesianVelocity{
+                            linear_direction.x(),
+                            linear_direction.y(),
+                            linear_direction.z(),
+                            angular_direction.x(),
+                            angular_direction.y(),
+                            angular_direction.z(),
+                        },
+                        0.5,
+                        0.0,
+                        FrameType::Base);
+                },
+                std::chrono::seconds(15));
+        } catch (const std::exception& error) {
+            worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(3));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    EXPECT_TRUE(worker_error.empty()) << worker_error;
+    const auto expect_protective_stop = [&](const CollisionCaseOutcome& outcome) {
+        EXPECT_TRUE(outcome.setup_move.ok())
+            << outcome.name << ": " << outcome.setup_move.message;
+        EXPECT_TRUE(outcome.task_initialized) << outcome.name;
+        EXPECT_TRUE(outcome.task_started) << outcome.name;
+        EXPECT_EQ(outcome.initial_status.level, task::CollisionSafetyLevel::SAFE)
+            << outcome.name;
+        EXPECT_TRUE(outcome.monitor_ok) << outcome.name;
+        EXPECT_TRUE(outcome.stop_seen) << outcome.name;
+        EXPECT_EQ(outcome.stop_status.level, task::CollisionSafetyLevel::STOP)
+            << outcome.name;
+        EXPECT_TRUE(outcome.stop_status.stop_latched) << outcome.name;
+        EXPECT_NE(outcome.stop_status.event_id, 0U) << outcome.name;
+        EXPECT_EQ(outcome.stop_status.recovery_state,
+                  task::ProtectiveRecoveryState::AVAILABLE)
+            << outcome.name;
+        EXPECT_GE(outcome.stop_status.recovery_sample_count, 2U)
+            << outcome.name;
+        EXPECT_LE(outcome.stop_status.result.minimum_distance_m, 0.005)
+            << outcome.name;
+        EXPECT_TRUE(outcome.stop_status.result.first == "body_link" ||
+                    outcome.stop_status.result.second == "body_link")
+            << outcome.name;
+        EXPECT_TRUE(outcome.recovery_succeeded)
+            << outcome.name << ": " << outcome.recovery.message;
+        EXPECT_FALSE(outcome.recovered_status.stop_latched) << outcome.name;
+        EXPECT_EQ(outcome.recovered_status.recovery_state,
+                  task::ProtectiveRecoveryState::SUCCEEDED)
+            << outcome.name;
+        EXPECT_GE(outcome.recovered_status.result.minimum_distance_m, 0.025)
+            << outcome.name;
+        EXPECT_FALSE(outcome.protective_stopped) << outcome.name;
+        EXPECT_FALSE(outcome.emergency_stopped) << outcome.name;
+        EXPECT_EQ(outcome.safety_mode, SafetyMode::Normal)
+            << outcome.name;
+    };
+    expect_protective_stop(move_j_outcome);
+    expect_protective_stop(move_l_outcome);
+    expect_protective_stop(speed_l_outcome);
+
+    EXPECT_EQ(move_j_outcome.collision_move.code,
+              ArmErrorCode::RobotInProtectiveStop);
+    EXPECT_GT(maxPositionError(move_j_outcome.final_position, kTorsoCollisionPose), 0.02);
+    EXPECT_EQ(move_l_outcome.collision_move.code,
+              ArmErrorCode::RobotInProtectiveStop);
+    EXPECT_GT(translationError(move_l_outcome.final_tcp, move_l_target), 0.01);
+    EXPECT_TRUE(speed_l_outcome.collision_move.ok())
+        << speed_l_outcome.collision_move.message;
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, SpeedL)
+{
+    constexpr auto kCommandDuration = std::chrono::seconds(2);
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+    ScenarioOutcome outcome;
+    std::array<double, 6> measured_deltas{};
+
+    std::thread scenario([&] {
+        try {
+            if (!world_ || !world_->isRunning()) {
+                throw std::runtime_error("MuJoCo world is not running");
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            MotionOptions joint_options;
+            joint_options.velocity = 0.6;
+            joint_options.acceleration = 2.0;
+            outcome.move_j = arm_->moveJ(
+                JointPositionCommand{kSetupPose}, joint_options);
+            waitFor([&] {
+                return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
+            }, std::chrono::seconds(3));
+            outcome.move_j_error = maxPositionError(
+                arm_->getJointState().position, kSetupPose);
+            if (!outcome.move_j.ok()) {
+                throw std::runtime_error(outcome.move_j.message);
+            }
+
+            struct SpeedStep {
+                const char* name;
+                CartesianVelocity command;
+                bool angular;
+                std::size_t axis;
+            };
+            const std::array<SpeedStep, 6> steps{{
+                {"+X", CartesianVelocity{0.05, 0.0, 0.0, 0.0, 0.0, 0.0}, false, 0},
+                {"+Y", CartesianVelocity{0.0, 0.05, 0.0, 0.0, 0.0, 0.0}, false, 1},
+                {"+Z", CartesianVelocity{0.0, 0.0, 0.05, 0.0, 0.0, 0.0}, false, 2},
+                {"+RX", CartesianVelocity{0.0, 0.0, 0.0, 0.30, 0.0, 0.0}, true, 0},
+                {"+RY", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.30, 0.0}, true, 1},
+                {"+RZ", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.0, 0.30}, true, 2},
+            }};
+
+            for (std::size_t i = 0; i < steps.size(); ++i) {
+                const auto& step = steps[i];
+                if (i > 0) {
+                    outcome.move_j = arm_->moveJ(
+                        JointPositionCommand{kSetupPose}, joint_options);
+                    if (!outcome.move_j.ok()) {
+                        throw std::runtime_error(
+                            std::string("moveJ before speedL ") + step.name +
+                            ": " + outcome.move_j.message);
+                    }
+                    waitFor([&] {
+                        return maxPositionError(
+                                   arm_->getJointState().position, kSetupPose) < 0.04;
+                    }, std::chrono::seconds(3));
+                    std::this_thread::sleep_for(std::chrono::milliseconds(300));
+                }
+
+                const CartesianPose start = arm_->getTcpPose();
+                const Result speed_result = arm_->speedL(
+                    step.command, 0.5, 0.0, FrameType::Base);
+                if (!speed_result.ok()) {
+                    throw std::runtime_error(
+                        std::string("speedL ") + step.name + ": " +
+                        speed_result.message);
+                }
+
+                std::this_thread::sleep_for(kCommandDuration);
+                const Result stop_result = arm_->stopL(0.5);
+                if (!stop_result.ok()) {
+                    throw std::runtime_error(
+                        std::string("stopL ") + step.name + ": " +
+                        stop_result.message);
+                }
+                waitFor([&] { return !arm_->busy(); }, std::chrono::seconds(3));
+
+                const CartesianPose end = arm_->getTcpPose();
+                if (step.angular) {
+                    measured_deltas[i] = baseRotationDelta(start, end)[step.axis];
+                    std::cout << "[MotorRobotArmGen2MujocoTest] speedL "
+                              << step.name << " rotation delta: "
+                              << measured_deltas[i] << " rad" << std::endl;
+                } else {
+                    const Eigen::Vector3d translation_delta{
+                        end.x - start.x, end.y - start.y, end.z - start.z};
+                    measured_deltas[i] = translation_delta[step.axis];
+                    std::cout << "[MotorRobotArmGen2MujocoTest] speedL "
+                              << step.name << " translation delta: "
+                              << measured_deltas[i] << " m" << std::endl;
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(500));
+            }
+        } catch (const std::exception& error) {
+            outcome.worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(3));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
+    EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
+    EXPECT_LT(outcome.move_j_error, 0.08);
+    for (std::size_t i = 0; i < 3; ++i) {
+        EXPECT_GT(measured_deltas[i], 0.02);
+    }
+    for (std::size_t i = 3; i < measured_deltas.size(); ++i) {
+        EXPECT_GT(measured_deltas[i], 0.10);
+    }
+}
+
+} // namespace
+} // namespace cmvr::device

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

@@ -10,7 +10,6 @@
 #include <memory>
 #include <string>
 #include <thread>
-#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -147,17 +146,10 @@ protected:
             (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
             &motor_root_config));
 
-        std::unordered_set<std::string> right_arm_joints;
-        for (const auto* joint_name : kJointNames) {
-            right_arm_joints.insert(joint_name);
-        }
-        MotorManager::clearActiveJoints();
-        MotorManager::setActiveJoints(
-            "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}});
-
-        motor_system_ = std::make_shared<MotorManager>("mujoco_motors", motor_root_config.motor());
+        motor_system_ = std::make_shared<MotorManager>(
+            "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
         ASSERT_NO_THROW(motor_system_->init());
-        world_ = MotorManager::mujocoWorldFor("mujoco_motors");
+        world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
         ASSERT_TRUE(world_);
         ASSERT_TRUE(world_->isLoaded());
 
@@ -187,7 +179,6 @@ protected:
         if (world_device_) {
             world_device_->stop();
         }
-        MotorManager::clearActiveJoints();
     }
 
     std::filesystem::path project_root_;

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

@@ -0,0 +1,352 @@
+#include "arm/motor_robot_arm/include/motor_robot_arm.h"
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <filesystem>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "algorithms/kinematics/ik_solver/ik_solver_factory.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.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"
+
+namespace cmvr::device {
+namespace {
+
+constexpr std::size_t kDof = 7;
+constexpr std::array<const char*, kDof> kJointNames = {
+    "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R",
+    "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
+constexpr std::array<double, kDof> kInitialJointPosition = {
+    -0.2423, 1.2929, 1.61, 1.58, -2.8792, 0.1150, -0.08};
+constexpr std::array<double, 7> kAccelerationSweep = {
+    0.1, 0.3, 0.5, 1.0, 2.0, 3.0, 5.0};
+constexpr double kCommandVelocity = 0.04;
+constexpr double kCommandDurationS = 2.0;
+constexpr double kSamplePeriodS = 0.001;
+
+std::filesystem::path findProjectRoot()
+{
+    const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml";
+    auto search = [&](std::filesystem::path current) {
+        while (!current.empty()) {
+            if (std::filesystem::exists(current / marker)) {
+                return current;
+            }
+            const auto parent = current.parent_path();
+            if (parent == current) {
+                break;
+            }
+            current = parent;
+        }
+        return std::filesystem::path{};
+    };
+
+    auto root = search(std::filesystem::current_path());
+    if (!root.empty()) {
+        return root;
+    }
+    return search(std::filesystem::path(__FILE__).parent_path());
+}
+
+void setKinematicsUrdfPath(config::RobotArmConfig& arm_config,
+                           const std::string& urdf_path)
+{
+    switch (arm_config.kinematics().algorithm_case()) {
+    case config::ArmKinematicsConfig::kPinocchioDlsIkSolver:
+        arm_config.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path(urdf_path);
+        break;
+    case config::ArmKinematicsConfig::kPinocchioQpIkSolver:
+        arm_config.mutable_kinematics()->mutable_pinocchio_qp_ik_solver()->set_urdf_path(urdf_path);
+        break;
+    default:
+        break;
+    }
+}
+
+struct Sample {
+    double time_s{0.0};
+    std::vector<double> q;
+    std::vector<double> qd;
+    CartesianVelocity commanded_twist;
+};
+
+struct Metrics {
+    double max_qd_before_stop{0.0};
+    double max_qd_after_stop{0.0};
+    double max_qdd{0.0};
+    double max_tcp_linear_speed_before_stop{0.0};
+    double max_tcp_linear_speed_after_stop{0.0};
+    double max_tcp_linear_acceleration{0.0};
+};
+
+double vectorNorm(const std::vector<double>& value)
+{
+    double sum = 0.0;
+    for (const double item : value) {
+        sum += item * item;
+    }
+    return std::sqrt(sum);
+}
+
+double linearSpeed(const Eigen::Matrix<double, 6, 1>& twist)
+{
+    return twist.head<3>().norm();
+}
+
+std::string numberForFile(const double value)
+{
+    std::ostringstream stream;
+    stream << std::fixed << std::setprecision(3) << value;
+    auto result = stream.str();
+    std::replace(result.begin(), result.end(), '.', '_');
+    return result;
+}
+
+void writeCsv(const std::filesystem::path& path,
+              const std::vector<Sample>& samples,
+              const double stop_time_s,
+              const cmvr::PinocchioIKBase& solver,
+              Metrics& metrics)
+{
+    std::ofstream output(path);
+    ASSERT_TRUE(output.is_open()) << "failed to open CSV: " << path;
+
+    output << "time_s,phase";
+    for (const auto* name : kJointNames) {
+        output << "," << name << "_q";
+    }
+    for (const auto* name : kJointNames) {
+        output << "," << name << "_qd";
+    }
+    output << ",command_vx,command_vy,command_vz,command_wx,command_wy,command_wz"
+           << ",tcp_vx,tcp_vy,tcp_vz,tcp_wx,tcp_wy,tcp_wz,tcp_linear_speed,tcp_linear_acceleration";
+    output << '\n';
+
+    Eigen::Matrix<double, 6, 1> previous_tcp_twist = Eigen::Matrix<double, 6, 1>::Zero();
+    bool have_previous_tcp = false;
+    for (const auto& sample : samples) {
+        Eigen::Matrix<double, 6, 1> tcp_twist = Eigen::Matrix<double, 6, 1>::Zero();
+        ASSERT_TRUE(solver.computeTwistBaseAtQ(sample.q, sample.qd, true, tcp_twist));
+        const bool after_stop = stop_time_s >= 0.0 && sample.time_s >= stop_time_s;
+        const double qd_norm = vectorNorm(sample.qd);
+        const double tcp_speed = linearSpeed(tcp_twist);
+        double tcp_acceleration = 0.0;
+        if (have_previous_tcp) {
+            tcp_acceleration = (tcp_twist.head<3>() - previous_tcp_twist.head<3>()).norm() /
+                               std::max(kSamplePeriodS, sample.time_s -
+                                                       (samples[&sample - samples.data() - 1].time_s));
+        }
+        previous_tcp_twist = tcp_twist;
+        have_previous_tcp = true;
+
+        if (after_stop) {
+            metrics.max_qd_after_stop = std::max(metrics.max_qd_after_stop, qd_norm);
+            metrics.max_tcp_linear_speed_after_stop =
+                std::max(metrics.max_tcp_linear_speed_after_stop, tcp_speed);
+        } else {
+            metrics.max_qd_before_stop = std::max(metrics.max_qd_before_stop, qd_norm);
+            metrics.max_tcp_linear_speed_before_stop =
+                std::max(metrics.max_tcp_linear_speed_before_stop, tcp_speed);
+        }
+        metrics.max_tcp_linear_acceleration =
+            std::max(metrics.max_tcp_linear_acceleration, tcp_acceleration);
+
+        if (&sample != samples.data()) {
+            const auto& previous = samples[&sample - samples.data() - 1];
+            const double dt = std::max(kSamplePeriodS, sample.time_s - previous.time_s);
+            for (std::size_t i = 0; i < kDof; ++i) {
+                metrics.max_qdd = std::max(metrics.max_qdd,
+                                           std::abs(sample.qd[i] - previous.qd[i]) / dt);
+            }
+        }
+
+        output << std::setprecision(9) << sample.time_s << ',' << (after_stop ? "stop" : "run");
+        for (const double value : sample.q) {
+            output << ',' << value;
+        }
+        for (const double value : sample.qd) {
+            output << ',' << value;
+        }
+        output << ',' << sample.commanded_twist.vx
+               << ',' << sample.commanded_twist.vy
+               << ',' << sample.commanded_twist.vz
+               << ',' << sample.commanded_twist.wx
+               << ',' << sample.commanded_twist.wy
+               << ',' << sample.commanded_twist.wz;
+        for (Eigen::Index i = 0; i < 6; ++i) {
+            output << ',' << tcp_twist[i];
+        }
+        output << ',' << tcp_speed << ',' << tcp_acceleration << '\n';
+    }
+}
+
+class MotorRobotArmSpeedLStopTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        project_root_ = findProjectRoot();
+        ASSERT_FALSE(project_root_.empty());
+
+        config::MujocoWorldRootConfig world_root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(),
+            &world_root_config));
+        ASSERT_GT(world_root_config.worlds_size(), 0);
+        auto world_config = world_root_config.worlds(0);
+        world_config.set_model_path(
+            (project_root_ / "model/xiaoyan_description/dual_arm.xml").string());
+        world_device_ = std::make_shared<simulate::MujocoWorldDevice>(world_config);
+        ASSERT_TRUE(world_device_->init());
+        ASSERT_TRUE(world_device_->start());
+
+        config::MotorRootConfig motor_root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
+            &motor_root_config));
+        motor_system_ = std::make_shared<MotorManager>(
+            "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
+        ASSERT_TRUE(motor_system_->init());
+
+        config::ArmRootConfig root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt").string(),
+            &root_config));
+        ASSERT_GT(root_config.arm().robot_arms_size(), 0);
+        arm_config_ = root_config.arm().robot_arms(0);
+        setKinematicsUrdfPath(
+            arm_config_, (project_root_ / "model/xiaoyan_description/dual_arm.urdf").string());
+        arm_ = std::make_unique<MotorRobotArm>(arm_config_);
+        ASSERT_TRUE(arm_->init());
+
+        analysis_solver_ = IKSolverFactory::create(arm_config_.kinematics());
+        ASSERT_TRUE(analysis_solver_);
+        ASSERT_TRUE(analysis_solver_->init());
+        analysis_pinocchio_solver_ = std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(analysis_solver_);
+        ASSERT_TRUE(analysis_pinocchio_solver_);
+    }
+
+    void TearDown() override
+    {
+        if (arm_) {
+            arm_->stop();
+        }
+        if (motor_system_) {
+            motor_system_->stop();
+        }
+        if (world_device_) {
+            world_device_->stop();
+        }
+    }
+
+    std::filesystem::path project_root_;
+    config::RobotArmConfig arm_config_;
+    std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
+    std::shared_ptr<MotorManager> motor_system_;
+    std::unique_ptr<MotorRobotArm> arm_;
+    std::shared_ptr<cmvr::IKSolver> analysis_solver_;
+    std::shared_ptr<cmvr::PinocchioIKBase> analysis_pinocchio_solver_;
+};
+
+TEST_F(MotorRobotArmSpeedLStopTest, SweepAccelerationAndDirection)
+{
+    ASSERT_TRUE(world_device_);
+    ASSERT_TRUE(world_device_->world());
+
+    const std::vector<double> initial(kInitialJointPosition.begin(), kInitialJointPosition.end());
+    MotionOptions move_options;
+    move_options.velocity = 2.0;
+    move_options.acceleration = 3.0;
+    ASSERT_TRUE(arm_->moveJ(JointPositionCommand{initial}, move_options).ok());
+
+    for (const double acceleration : kAccelerationSweep) {
+        for (const double direction : {1.0, -1.0}) {
+            ASSERT_FALSE(arm_->busy());
+            std::vector<Sample> samples;
+            samples.reserve(5000);
+            std::atomic<double> stop_time_s{-1.0};
+            Result command_result = Result::failure(ArmErrorCode::UnknownError, "not run");
+            const auto start_time = std::chrono::steady_clock::now();
+
+            std::thread command_thread([&] {
+                CartesianVelocity velocity;
+                velocity.vx = direction * kCommandVelocity;
+                command_result = arm_->speedL(velocity,
+                                              acceleration,
+                                              kCommandDurationS,
+                                              FrameType::Base);
+                stop_time_s.store(std::chrono::duration<double>(
+                                      std::chrono::steady_clock::now() - start_time).count());
+            });
+
+            while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() < 5.0) {
+                const double time_s = std::chrono::duration<double>(
+                    std::chrono::steady_clock::now() - start_time).count();
+                const auto state = arm_->getJointState();
+                ASSERT_EQ(state.position.size(), kDof);
+                ASSERT_EQ(state.velocity.size(), kDof);
+                samples.push_back(Sample{time_s,
+                                         state.position,
+                                         state.velocity,
+                                         arm_->getSpeedLCommandTwistBase()});
+
+                const double stop = stop_time_s.load();
+                if (stop >= 0.0 && time_s > stop + 0.8 && !arm_->busy()) {
+                    break;
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            }
+            command_thread.join();
+
+            ASSERT_TRUE(command_result.ok()) << command_result.message;
+            const double stop = stop_time_s.load();
+            ASSERT_GT(stop, 1.8);
+            ASSERT_LT(stop, 2.5);
+            ASSERT_FALSE(arm_->busy());
+
+            const auto csv_path = std::filesystem::path("/tmp") /
+                                  ("speedl_stop_acc_" + numberForFile(acceleration) +
+                                   "_" + (direction > 0.0 ? "pos" : "neg") + ".csv");
+            Metrics metrics;
+            writeCsv(csv_path, samples, stop, *analysis_pinocchio_solver_, metrics);
+            std::cout << "[SpeedLStopTest] acceleration=" << acceleration
+                      << ", direction=" << direction
+                      << ", csv=" << csv_path
+                      << ", max_qd_before=" << metrics.max_qd_before_stop
+                      << ", max_qd_after=" << metrics.max_qd_after_stop
+                      << ", max_qdd=" << metrics.max_qdd
+                      << ", max_tcp_speed_before=" << metrics.max_tcp_linear_speed_before_stop
+                      << ", max_tcp_speed_after=" << metrics.max_tcp_linear_speed_after_stop
+                      << ", max_tcp_acceleration=" << metrics.max_tcp_linear_acceleration
+                      << std::endl;
+
+            // Stopping must not create a new velocity peak.  A small tolerance
+            // allows one 1 ms feedback sample of transport jitter.
+            EXPECT_LE(metrics.max_qd_after_stop,
+                      metrics.max_qd_before_stop + 0.25)
+                << "post-stop joint velocity peak for acceleration=" << acceleration
+                << ", direction=" << direction;
+            EXPECT_LE(metrics.max_tcp_linear_speed_after_stop,
+                      metrics.max_tcp_linear_speed_before_stop + 0.01)
+                << "post-stop TCP velocity peak for acceleration=" << acceleration
+                << ", direction=" << direction;
+        }
+    }
+}
+
+} // namespace
+} // namespace cmvr::device

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

@@ -79,6 +79,9 @@ public:
 
     virtual Result emergencyStop() = 0;
     virtual Result protectiveStop() = 0;
+    virtual Result recoverProtectiveStop(
+        const JointTrajectory& path,
+        const MotionOptions& options) = 0;
     virtual Result setSpeedScaling(double scaling) = 0;
     virtual double getSpeedScaling() const = 0;
     virtual bool isProtectiveStopped() const = 0;
@@ -113,6 +116,17 @@ 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 ||
+            options.continuous_linear_reversal.value_or(false)) {
+            return Result::failure(ArmErrorCode::UnsupportedCommand, "speedL options are not supported by this arm");
+        }
+        return speedL(velocity, options.acceleration, duration, frame);
+    }
+    virtual SpeedLReference getSpeedLReference() const { return {}; }
     virtual Result stopMotion() = 0;
 
     virtual Result moveP(const CartesianPose& target,

cmvr-es/devices/camera/abstract_camera.h

@@ -3,20 +3,22 @@
 #pragma once
 
 #include <opencv2/opencv.hpp>
+#include <mutex>
 #include "../abstract_device.h"
 #include <Eigen/Core>
 #include "cmvr/config/camera_config/camera_config.pb.h"
+#include "devices/camera/common/include/camera_stream_overlay.h"
 
 namespace cmvr::device {
     enum CameraMode {PHOTO_MODE, VIDEO_MODE};
 
     struct Rs2Intrinsics
     {
-        float   cx;
-        float   cy;
-        float   fx;
-        float   fy;
-        float   coeffs[5];
+        float cx{0.0F};
+        float cy{0.0F};
+        float fx{0.0F};
+        float fy{0.0F};
+        float coeffs[5]{};
 
     };
     struct StreamFrameData
@@ -64,11 +66,26 @@ namespace cmvr::device {
             return false;
         }
 
+        // Video overlay is a presentation-only snapshot.  It is deliberately
+        // kept on the camera so the encoding thread can consume it without
+        // coupling the camera to AprilTag or task code.
+        void setStreamOverlay(const CameraStreamOverlay& overlay) {
+            std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
+            stream_overlay_ = overlay;
+        }
+
+        CameraStreamOverlay streamOverlay() const {
+            std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
+            return stream_overlay_;
+        }
+
         virtual bool startStreaming() {return true;}
         virtual void stopStreaming() {}
         virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
     protected:
         CameraState state_{};
+        mutable std::mutex stream_overlay_mutex_;
+        CameraStreamOverlay stream_overlay_{};
         void clear_error_() {
             this->state_.is_error = false;
             this->state_.error_message.clear();

cmvr-es/devices/camera/common/include/camera_stream_encoder.h

@@ -7,9 +7,12 @@
 #include <opencv2/opencv.hpp>
 
 #include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
+#include "devices/camera/common/include/camera_stream_overlay.h"
 
 namespace cmvr::device {
 
+struct Rs2Intrinsics;
+
 struct FfmpegEncoderInfo {
     std::string codec_name;
     int width = 0;
@@ -27,8 +30,23 @@ struct FfmpegEncoderInfo {
 
 struct CameraStreamEncodeOptions {
     bool draw_timestamp = false;
+    CameraStreamOverlay overlay;
 };
 
+// Draw a frame whose pose is expressed as ^C T_Frame onto a BGR/BGRA image.
+// The image is modified in place and no camera/perception state is touched.
+void drawCoordinateFrame(cv::Mat& image,
+                         const Eigen::Matrix4d& T_C_Frame,
+                         const Rs2Intrinsics& intrinsics,
+                         double axis_length_m,
+                         const std::string& label);
+
+Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
+                              int source_width,
+                              int source_height,
+                              int target_width,
+                              int target_height);
+
 class CameraStreamEncoder {
 public:
     static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
@@ -37,6 +55,14 @@ public:
                      int height,
                      int fps);
 
+    static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                       const cv::Mat& frame,
+                       std::vector<uint8_t>& encoded_frame,
+                       bool& is_key,
+                       const Rs2Intrinsics& intrinsics,
+                       const CameraStreamEncodeOptions& options = {});
+
+    // Compatibility overload for callers that only need timestamp drawing.
     static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
                        const cv::Mat& frame,
                        std::vector<uint8_t>& encoded_frame,

cmvr-es/devices/camera/common/include/camera_stream_overlay.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <Eigen/Dense>
+
+namespace cmvr::device {
+
+// A pose snapshot used only by the encoded video overlay.  The transform is
+// ^C T_Frame: it maps points in the named frame into the camera frame.
+struct CoordinateFrameOverlay {
+    Eigen::Matrix4d T_C_Frame{Eigen::Matrix4d::Identity()};
+    std::string label;
+    int tag_id{-1};
+    bool valid{false};
+};
+
+struct CameraStreamOverlay {
+    bool draw_coordinate_frames{false};
+    double coordinate_axis_length_m{0.02};
+    std::vector<CoordinateFrameOverlay> coordinate_frames;
+};
+
+} // namespace cmvr::device

cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp

@@ -1,6 +1,7 @@
 #include "devices/camera/common/include/camera_stream_encoder.h"
 
 #include <chrono>
+#include <cmath>
 #include <ctime>
 #include <iomanip>
 #include <sstream>
@@ -9,6 +10,7 @@
 #include <opencv2/imgproc.hpp>
 
 #include "common/base/logging/logger.h"
+#include "devices/camera/abstract_camera.h"
 
 namespace cmvr::device {
 namespace {
@@ -46,6 +48,41 @@ void drawTimeStamp(cv::Mat& image)
     cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
 }
 
+bool projectPoint(const Eigen::Vector3d& point,
+                 const Rs2Intrinsics& intrinsics,
+                 cv::Point& pixel)
+{
+    if (!point.allFinite() || point.z() <= 1e-9 ||
+        !std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
+        !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
+        intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
+        return false;
+    }
+    const double u = static_cast<double>(intrinsics.fx) * point.x() / point.z() +
+                     static_cast<double>(intrinsics.cx);
+    const double v = static_cast<double>(intrinsics.fy) * point.y() / point.z() +
+                     static_cast<double>(intrinsics.cy);
+    if (!std::isfinite(u) || !std::isfinite(v)) {
+        return false;
+    }
+    pixel = cv::Point(cvRound(u), cvRound(v));
+    return true;
+}
+
+void drawOutlinedText(cv::Mat& image,
+                      const std::string& text,
+                      const cv::Point& origin,
+                      const cv::Scalar& color)
+{
+    constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX;
+    constexpr double font_scale = 0.55;
+    constexpr int thickness = 1;
+    cv::putText(image, text, origin, font_face, font_scale,
+                cv::Scalar(0, 0, 0), thickness + 2, cv::LINE_AA);
+    cv::putText(image, text, origin, font_face, font_scale,
+                color, thickness, cv::LINE_AA);
+}
+
 const AVCodec* findEncoder(const std::string& codec_name)
 {
     if (codec_name == "h264" || codec_name == "H264") {
@@ -75,6 +112,77 @@ AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
 
 } // namespace
 
+void drawCoordinateFrame(cv::Mat& image,
+                         const Eigen::Matrix4d& T_C_Frame,
+                         const Rs2Intrinsics& intrinsics,
+                         const double axis_length_m,
+                         const std::string& label)
+{
+    if (image.empty() || image.channels() < 3 || !T_C_Frame.allFinite() ||
+        !std::isfinite(axis_length_m) || axis_length_m <= 0.0) {
+        return;
+    }
+
+    const Eigen::Vector4d origin_h(0.0, 0.0, 0.0, 1.0);
+    const Eigen::Vector4d x_h(axis_length_m, 0.0, 0.0, 1.0);
+    const Eigen::Vector4d y_h(0.0, axis_length_m, 0.0, 1.0);
+    const Eigen::Vector4d z_h(0.0, 0.0, axis_length_m, 1.0);
+    const Eigen::Vector3d origin = (T_C_Frame * origin_h).head<3>();
+    const Eigen::Vector3d x = (T_C_Frame * x_h).head<3>();
+    const Eigen::Vector3d y = (T_C_Frame * y_h).head<3>();
+    const Eigen::Vector3d z = (T_C_Frame * z_h).head<3>();
+
+    cv::Point origin_px;
+    if (!projectPoint(origin, intrinsics, origin_px)) {
+        return;
+    }
+
+    cv::Point x_px;
+    cv::Point y_px;
+    cv::Point z_px;
+    constexpr int thickness = 2;
+    if (projectPoint(x, intrinsics, x_px)) {
+        cv::arrowedLine(image, origin_px, x_px, cv::Scalar(0, 0, 255), thickness,
+                        cv::LINE_AA, 0, 0.15);
+        drawOutlinedText(image, "X", x_px + cv::Point(4, -4), cv::Scalar(0, 0, 255));
+    }
+    if (projectPoint(y, intrinsics, y_px)) {
+        cv::arrowedLine(image, origin_px, y_px, cv::Scalar(0, 255, 0), thickness,
+                        cv::LINE_AA, 0, 0.15);
+        drawOutlinedText(image, "Y", y_px + cv::Point(4, -4), cv::Scalar(0, 255, 0));
+    }
+    if (projectPoint(z, intrinsics, z_px)) {
+        cv::arrowedLine(image, origin_px, z_px, cv::Scalar(255, 0, 0), thickness,
+                        cv::LINE_AA, 0, 0.15);
+        drawOutlinedText(image, "Z", z_px + cv::Point(4, -4), cv::Scalar(255, 0, 0));
+    }
+
+    cv::drawMarker(image, origin_px, cv::Scalar(255, 255, 255), cv::MARKER_CROSS, 9, 1,
+                   cv::LINE_AA);
+    if (!label.empty()) {
+        drawOutlinedText(image, label, origin_px + cv::Point(7, -7),
+                         cv::Scalar(255, 255, 255));
+    }
+}
+
+Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
+                              const int source_width,
+                              const int source_height,
+                              const int target_width,
+                              const int target_height)
+{
+    Rs2Intrinsics scaled = intrinsics;
+    if (source_width > 0 && source_height > 0 && target_width > 0 && target_height > 0) {
+        const float sx = static_cast<float>(target_width) / static_cast<float>(source_width);
+        const float sy = static_cast<float>(target_height) / static_cast<float>(source_height);
+        scaled.fx *= sx;
+        scaled.cx *= sx;
+        scaled.fy *= sy;
+        scaled.cy *= sy;
+    }
+    return scaled;
+}
+
 FfmpegEncoderInfo::~FfmpegEncoderInfo()
 {
     if (frame) {
@@ -189,6 +297,7 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
                                  const cv::Mat& frame,
                                  std::vector<uint8_t>& encoded_frame,
                                  bool& is_key,
+                                 const Rs2Intrinsics& intrinsics,
                                  const CameraStreamEncodeOptions& options)
 {
     encoded_frame.clear();
@@ -205,10 +314,28 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
     }
 
     cv::Mat frame_to_encode = frame;
-    if (options.draw_timestamp) {
+    if (options.draw_timestamp || options.overlay.draw_coordinate_frames) {
         frame_to_encode = frame.clone();
+        if (options.draw_timestamp) {
             drawTimeStamp(frame_to_encode);
         }
+        if (options.overlay.draw_coordinate_frames) {
+            for (const auto& coordinate_frame : options.overlay.coordinate_frames) {
+                if (!coordinate_frame.valid) {
+                    continue;
+                }
+                std::string label = coordinate_frame.label;
+                if (coordinate_frame.tag_id >= 0) {
+                    label += " #" + std::to_string(coordinate_frame.tag_id);
+                }
+                drawCoordinateFrame(frame_to_encode,
+                                    coordinate_frame.T_C_Frame,
+                                    intrinsics,
+                                    options.overlay.coordinate_axis_length_m,
+                                    label);
+            }
+        }
+    }
 
     const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
     if (src_pix_fmt == AV_PIX_FMT_NONE) {
@@ -294,4 +421,14 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
     return true;
 }
 
+bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                                 const cv::Mat& frame,
+                                 std::vector<uint8_t>& encoded_frame,
+                                 bool& is_key,
+                                 const CameraStreamEncodeOptions& options)
+{
+    Rs2Intrinsics intrinsics{};
+    return encode(encoder, frame, encoded_frame, is_key, intrinsics, options);
+}
+
 } // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h

@@ -5,10 +5,13 @@
 #pragma once
 
 #include <cstdint>
+#include <condition_variable>
+#include <chrono>
 #include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include <mujoco/mujoco.h>
@@ -57,6 +60,11 @@ private:
     bool initOffscreen_();
     void destroyOffscreen_();
     bool renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
+    void renderLoop_();
+    bool fetchCached_(cv::Mat& color,
+                      cv::Mat& depth,
+                      Rs2Intrinsics& intrinsics,
+                      bool consume_new_frame_only);
     bool ensureEncoder_(int width, int height, int fps);
     void setError_(const std::string& error);
     static void flipRgbAndDepth_(std::vector<unsigned char>& rgb,
@@ -65,6 +73,14 @@ private:
                                  int height);
     static void linearizeDepth_(const mjModel* model, std::vector<float>& depth);
 
+    struct CachedFrame {
+        cv::Mat color;
+        cv::Mat depth;
+        Rs2Intrinsics intrinsics{};
+        uint64_t frame_id{0};
+        bool valid{false};
+    };
+
 private:
     FetchRgbdFn fetch_rgbd_fn_;
     mutable std::mutex mtx_;
@@ -78,6 +94,7 @@ private:
     mjrContext context_{};
     bool scene_initialized_{false};
     bool context_initialized_{false};
+    mjData* render_data_{nullptr};
     int camera_id_{-1};
     int width_{640};
     int height_{480};
@@ -92,6 +109,13 @@ private:
     size_t stream_frame_index_{0};
     bool streaming_{false};
     std::shared_ptr<FfmpegEncoderInfo> rgb_encoder_;
+
+    mutable std::mutex cache_mtx_;
+    std::condition_variable cache_cv_;
+    CachedFrame latest_frame_;
+    std::thread render_thread_;
+    bool render_thread_running_{false};
+    bool render_stop_requested_{false};
 };
 
 } // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -19,6 +19,13 @@ constexpr int kDefaultWidth = 640;
 constexpr int kDefaultHeight = 480;
 constexpr int kMaxGeom = 100000;
 
+// GLFW keeps process-global initialization state. MuJoCo cameras render on
+// independent threads, so serialize the one-time init/window creation path.
+std::mutex& glfwInitMutex() {
+    static std::mutex mutex;
+    return mutex;
+}
+
 int positiveOrDefault(const int value, const int fallback)
 {
     return value > 0 ? value : fallback;
@@ -106,10 +113,6 @@ bool MujocoCamera::init()
         fovy_deg_ = model->cam_fovy[camera_id_];
     }
 
-    if (!initOffscreen_()) {
-        return false;
-    }
-
     state_.is_initialized = true;
     state_.is_opened = true;
     state_.fps = positiveOrDefault(config_.render().fps(), 30);
@@ -125,37 +128,132 @@ bool MujocoCamera::init()
 
 bool MujocoCamera::start()
 {
-    if (!state_.is_initialized) {
+    bool initialized = false;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        initialized = state_.is_initialized;
+    }
+    if (!initialized) {
         if (!init()) {
             return false;
         }
     }
-    std::lock_guard<std::mutex> lock(mtx_);
+
     auto world = world_.lock();
     if (world && !world->isRunning() && !world->start()) {
         setError_("[MujocoCamera] failed to start MuJoCo world: " + world->lastError());
         return false;
     }
+
+    bool use_external_frames = false;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
         state_.is_streaming = true;
         state_.is_opened = true;
+        use_external_frames = static_cast<bool>(fetch_rgbd_fn_);
+    }
+    std::thread stale_thread;
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        if (render_thread_running_) {
+            return true;
+        }
+        latest_frame_ = CachedFrame{};
+        last_frame_id_ = 0;
+        has_last_frame_id_ = false;
+        render_stop_requested_ = false;
+        render_thread_running_ = true;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        stale_thread = std::move(render_thread_);
+    }
+    if (stale_thread.joinable()) {
+        stale_thread.join();
+    }
+
+    try {
+        std::lock_guard<std::mutex> lock(mtx_);
+        render_thread_ = std::thread(&MujocoCamera::renderLoop_, this);
+    } catch (const std::exception& e) {
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            render_thread_running_ = false;
+            render_stop_requested_ = true;
+        }
+        cache_cv_.notify_all();
+        setError_("[MujocoCamera] failed to start render thread: " + std::string(e.what()));
+        return false;
+    }
+
+    // External PiP callbacks are not ready until the viewer enters its render
+    // loop, so let their polling thread warm up asynchronously.
+    if (use_external_frames) {
+        return true;
+    }
+
+    std::unique_lock<std::mutex> cache_lock(cache_mtx_);
+    const bool ready = cache_cv_.wait_for(
+        cache_lock,
+        std::chrono::seconds(5),
+        [this] { return latest_frame_.valid || !render_thread_running_ || render_stop_requested_; });
+    const bool has_frame = latest_frame_.valid;
+    cache_lock.unlock();
+    if (!ready || !has_frame) {
+        stop();
+        if (ready) {
+            setError_("[MujocoCamera] render thread stopped before producing a frame");
+        } else {
+            setError_("[MujocoCamera] timed out waiting for the first rendered frame");
+        }
+        return false;
+    }
     return true;
 }
 
 bool MujocoCamera::stop()
+{
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_stop_requested_ = true;
+    }
+    cache_cv_.notify_all();
+
+    std::thread thread_to_join;
     {
         std::lock_guard<std::mutex> lock(mtx_);
         state_.is_streaming = false;
         state_.is_opened = false;
-    destroyOffscreen_();
+        thread_to_join = std::move(render_thread_);
+    }
+    if (thread_to_join.joinable()) {
+        thread_to_join.join();
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_running_ = false;
+        latest_frame_ = CachedFrame{};
+    }
+    cache_cv_.notify_all();
     return true;
 }
 
 void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn)
 {
+    bool render_thread_active = false;
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_active = render_thread_running_;
+    }
+    if (render_thread_active) {
+        stop();
+    }
+
     std::lock_guard<std::mutex> lock(mtx_);
     fetch_rgbd_fn_ = std::move(fetch_rgbd_fn);
     if (fetch_rgbd_fn_) {
-        destroyOffscreen_();
         state_.is_initialized = true;
         state_.is_opened = true;
         state_.fps = positiveOrDefault(config_.render().fps(), 30);
@@ -203,9 +301,10 @@ void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics&
 
 bool MujocoCamera::startStreaming()
 {
-    if (!state_.is_initialized && !init()) {
+    if (!start()) {
         return false;
     }
+
     std::lock_guard<std::mutex> lock(mtx_);
     streaming_ = true;
     state_.is_streaming = true;
@@ -246,12 +345,17 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
     }
 
     frame_data.rgbImage = color.clone();
+    frame_data.intrinsics = intrinsics;
     CameraStreamEncodeOptions encode_options;
     encode_options.draw_timestamp = enable_stream_timestamp_;
+    encode_options.overlay = streamOverlay();
+    const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
+        intrinsics, color.cols, color.rows, color_to_encode.cols, color_to_encode.rows);
     if (!CameraStreamEncoder::encode(rgb_encoder_,
                                      color_to_encode,
                                      frame_data.rgbFrame,
                                      frame_data.bKey,
+                                     encode_intrinsics,
                                      encode_options)) {
         return false;
     }
@@ -261,7 +365,6 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
         const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
         frame_data.depthFrame.assign(depth_begin, depth_end);
     }
-    frame_data.intrinsics = intrinsics;
     frame_data.width = color_to_encode.cols;
     frame_data.height = color_to_encode.rows;
     frame_data.fps = fps;
@@ -272,15 +375,30 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
 }
 
 bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
+{
+    FetchRgbdFn fetch_rgbd_fn;
+    bool consume_new_frame_only = false;
+    bool render_thread_active = false;
     {
         std::lock_guard<std::mutex> lock(mtx_);
-    if (fetch_rgbd_fn_) {
+        fetch_rgbd_fn = fetch_rgbd_fn_;
+        consume_new_frame_only = consume_new_frame_only_;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_active = render_thread_running_;
+    }
+
+    // Keep compatibility with callback-only cameras that have not been
+    // started. Once start() owns a polling thread, reads are cache-only.
+    if (fetch_rgbd_fn && !render_thread_active) {
         std::vector<unsigned char> rgb_raw;
         std::vector<float> depth_raw;
         int width = 0;
         int height = 0;
         std::uint64_t frame_id = 0;
-        if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
+        if (!fetch_rgbd_fn(rgb_raw, depth_raw, width, height, frame_id)) {
             return false;
         }
         if (width <= 0 || height <= 0) {
@@ -292,11 +410,14 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
         if (!depth_raw.empty() && static_cast<int>(depth_raw.size()) != width * height) {
             return false;
         }
-        if (consume_new_frame_only_ && has_last_frame_id_ && frame_id == last_frame_id_) {
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            if (consume_new_frame_only && has_last_frame_id_ && frame_id == last_frame_id_) {
                 return false;
             }
             last_frame_id_ = frame_id;
             has_last_frame_id_ = true;
+        }
 
         cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
         cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
@@ -310,7 +431,31 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
         return true;
     }
 
-    return renderOffscreen_(color, depth, intrinsics);
+    return fetchCached_(color, depth, intrinsics, consume_new_frame_only);
+}
+
+bool MujocoCamera::fetchCached_(cv::Mat& color,
+                                cv::Mat& depth,
+                                Rs2Intrinsics& intrinsics,
+                                const bool consume_new_frame_only)
+{
+    std::lock_guard<std::mutex> lock(cache_mtx_);
+    if (!latest_frame_.valid || latest_frame_.color.empty()) {
+        return false;
+    }
+    if (consume_new_frame_only && has_last_frame_id_ &&
+        latest_frame_.frame_id == last_frame_id_) {
+        return false;
+    }
+
+    // cv::Mat copies are reference-counted; keep the cache immutable while the
+    // consumer reads the published frame and avoid a full image copy per poll.
+    color = latest_frame_.color;
+    depth = latest_frame_.depth;
+    intrinsics = latest_frame_.intrinsics;
+    last_frame_id_ = latest_frame_.frame_id;
+    has_last_frame_id_ = true;
+    return !color.empty();
 }
 
 bool MujocoCamera::initOffscreen_()
@@ -319,6 +464,7 @@ bool MujocoCamera::initOffscreen_()
         return true;
     }
 
+    std::lock_guard<std::mutex> glfw_lock(glfwInitMutex());
     if (!glfwInit()) {
         setError_("[MujocoCamera] glfwInit failed");
         return false;
@@ -345,12 +491,27 @@ bool MujocoCamera::initOffscreen_()
         return false;
     }
 
+    mjModel* model = nullptr;
+    {
         std::lock_guard<std::mutex> world_lock(world->mutex());
-    const mjModel* model = world->model();
+        model = world->model();
         if (model == nullptr) {
             setError_("[MujocoCamera] world model is null");
             return false;
         }
+
+        // MuJoCo clips rendering to the model's offscreen buffer. Make sure
+        // the buffer is large enough before creating this camera's context;
+        // otherwise a larger requested frame is only partially populated.
+        model->vis.global.offwidth = std::max(model->vis.global.offwidth, width_);
+        model->vis.global.offheight = std::max(model->vis.global.offheight, height_);
+    }
+
+    render_data_ = mj_makeData(model);
+    if (render_data_ == nullptr) {
+        setError_("[MujocoCamera] failed to allocate render data");
+        return false;
+    }
     mjv_makeScene(model, &scene_, kMaxGeom);
     scene_initialized_ = true;
     mjr_makeContext(model, &context_, mjFONTSCALE_150);
@@ -360,9 +521,116 @@ bool MujocoCamera::initOffscreen_()
         setError_("[MujocoCamera] MuJoCo offscreen buffer is not available");
         return false;
     }
+    if (context_.offWidth < width_ || context_.offHeight < height_) {
+        setError_("[MujocoCamera] offscreen buffer is smaller than requested frame: " +
+                  std::to_string(context_.offWidth) + "x" +
+                  std::to_string(context_.offHeight) + " < " +
+                  std::to_string(width_) + "x" + std::to_string(height_));
+        return false;
+    }
     return true;
 }
 
+void MujocoCamera::renderLoop_()
+{
+    FetchRgbdFn external_fetch;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        external_fetch = fetch_rgbd_fn_;
+    }
+
+    const bool use_external_frames = static_cast<bool>(external_fetch);
+    if (!use_external_frames && !initOffscreen_()) {
+        destroyOffscreen_();
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            render_thread_running_ = false;
+        }
+        cache_cv_.notify_all();
+        return;
+    }
+
+    const int fps = positiveOrDefault(config_.render().fps(), 30);
+    const auto period = std::chrono::duration<double>(1.0 / static_cast<double>(fps));
+    const auto period_ticks = std::chrono::duration_cast<std::chrono::steady_clock::duration>(period);
+    auto next_tick = std::chrono::steady_clock::now();
+
+    while (true) {
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            if (render_stop_requested_) {
+                break;
+            }
+        }
+
+        cv::Mat color;
+        cv::Mat depth;
+        Rs2Intrinsics intrinsics{};
+        uint64_t external_frame_id = 0;
+        bool got_frame = false;
+
+        if (use_external_frames) {
+            std::vector<unsigned char> rgb_raw;
+            std::vector<float> depth_raw;
+            int width = 0;
+            int height = 0;
+            if (external_fetch(rgb_raw, depth_raw, width, height, external_frame_id) &&
+                width > 0 && height > 0 &&
+                static_cast<int>(rgb_raw.size()) == width * height * 3 &&
+                (depth_raw.empty() || static_cast<int>(depth_raw.size()) == width * height)) {
+                cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
+                cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
+                if (!depth_raw.empty()) {
+                    cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
+                    depth = dep.clone();
+                }
+                fillIntrinsics(width, height, intrinsics);
+                got_frame = !color.empty();
+            }
+        } else {
+            got_frame = renderOffscreen_(color, depth, intrinsics);
+        }
+
+        if (got_frame) {
+            {
+                std::lock_guard<std::mutex> lock(cache_mtx_);
+                latest_frame_.color = std::move(color);
+                latest_frame_.depth = std::move(depth);
+                latest_frame_.intrinsics = intrinsics;
+                latest_frame_.frame_id = use_external_frames && external_frame_id != 0
+                    ? external_frame_id
+                    : latest_frame_.frame_id + 1;
+                latest_frame_.valid = true;
+            }
+            {
+                std::lock_guard<std::mutex> lock(mtx_);
+                clear_error_();
+            }
+            cache_cv_.notify_all();
+        }
+
+        next_tick += period_ticks;
+        std::unique_lock<std::mutex> lock(cache_mtx_);
+        if (cache_cv_.wait_until(lock, next_tick, [this] { return render_stop_requested_; })) {
+            break;
+        }
+
+        const auto now = std::chrono::steady_clock::now();
+        if (next_tick < now) {
+            next_tick = now + period_ticks;
+        }
+    }
+
+    if (!use_external_frames) {
+        destroyOffscreen_();
+    }
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_running_ = false;
+    }
+    cache_cv_.notify_all();
+}
+
 void MujocoCamera::destroyOffscreen_()
 {
     if (window_ != nullptr) {
@@ -376,6 +644,10 @@ void MujocoCamera::destroyOffscreen_()
         mjv_freeScene(&scene_);
         scene_initialized_ = false;
     }
+    if (render_data_ != nullptr) {
+        mj_deleteData(render_data_);
+        render_data_ = nullptr;
+    }
     if (window_ != nullptr) {
         glfwDestroyWindow(window_);
         window_ = nullptr;
@@ -388,7 +660,8 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         setError_("[MujocoCamera] camera is not initialized: " + id_);
         return false;
     }
-    if (!initOffscreen_()) {
+    if (window_ == nullptr || !context_initialized_ || !scene_initialized_) {
+        setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_);
         return false;
     }
 
@@ -402,15 +675,27 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
     std::vector<unsigned char> rgb(static_cast<std::size_t>(width_) * height_ * 3);
     std::vector<float> depth_raw(static_cast<std::size_t>(width_) * height_);
 
+    mjModel* model = nullptr;
     {
-        std::lock_guard<std::mutex> world_lock(world->mutex());
-        mjModel* model = world->model();
-        mjData* data = world->data();
-        if (model == nullptr || data == nullptr) {
+        std::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
+        if (!world_lock.owns_lock()) {
+            // Never make the simulation wait for a camera frame. The next
+            // scheduled capture will use a newer state if this one is busy.
+            return false;
+        }
+        model = world->model();
+        const mjData* data = world->data();
+        if (model == nullptr || data == nullptr || render_data_ == nullptr) {
             setError_("[MujocoCamera] world model/data is null");
             return false;
         }
 
+        // Keep the world lock limited to the state copy. GPU rendering runs on
+        // the camera thread using its private data snapshot.
+        mjv_copyData(render_data_, model, data);
+    }
+
+    {
         camera_.type = mjCAMERA_FIXED;
         camera_.fixedcamid = camera_id_;
         camera_.trackbodyid = -1;
@@ -421,7 +706,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         viewport.width = width_;
         viewport.height = height_;
 
-        mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
+        mjv_updateScene(model, render_data_, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
         mjr_render(viewport, &scene_, &context_);
         mjr_readPixels(rgb.data(), depth_raw.data(), viewport, &context_);
         flipRgbAndDepth_(rgb, depth_raw, width_, height_);
@@ -429,13 +714,12 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
     }
 
     cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
-    color = rgb_mat.clone();
+    // mjr_readPixels returns RGB, while the rest of the camera API exposes
+    // OpenCV-compatible BGR frames (as UVC and RealSense do).
+    cv::cvtColor(rgb_mat, color, cv::COLOR_RGB2BGR);
     cv::Mat depth_mat(height_, width_, CV_32FC1, depth_raw.data());
     depth = depth_mat.clone();
     fillIntrinsics(width_, height_, intrinsics);
-    ++last_frame_id_;
-    has_last_frame_id_ = true;
-    clear_error_();
     return true;
 }
 

cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp

@@ -772,10 +772,18 @@ void RealsenseCamera::streaming_worker_() {
             }
             CameraStreamEncodeOptions encode_options;
             encode_options.draw_timestamp = enable_stream_timestamp_;
+            encode_options.overlay = streamOverlay();
+            const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
+                frame_data.intrinsics,
+                frame_data.rgbImage.cols,
+                frame_data.rgbImage.rows,
+                rgb_to_encode.cols,
+                rgb_to_encode.rows);
             success = CameraStreamEncoder::encode(rgbEncoder_,
                                                   rgb_to_encode,
                                                   frame_data.rgbFrame,
                                                   frame_data.bKey,
+                                                  encode_intrinsics,
                                                   encode_options);
             // 深度图编码
             // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);

cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp

@@ -504,10 +504,18 @@ void UVCCamera::streaming_worker_() {
             }
             CameraStreamEncodeOptions encode_options;
             encode_options.draw_timestamp = enable_stream_timestamp_;
+            encode_options.overlay = streamOverlay();
+            const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
+                frame_data.intrinsics,
+                frame_data.rgbImage.cols,
+                frame_data.rgbImage.rows,
+                rgb_to_encode.cols,
+                rgb_to_encode.rows);
             success = CameraStreamEncoder::encode(rgbEncoder_,
                                                   rgb_to_encode,
                                                   frame_data.rgbFrame,
                                                   frame_data.bKey,
+                                                  encode_intrinsics,
                                                   encode_options);
             if (success) {
                 frame_data.fps = fps_;

cmvr-es/devices/canbus/can_comm/can_sender.h

@@ -247,6 +247,9 @@ namespace cmvr {
             curr_period_ = period_;
 
             Update();
+            if (send_with_once_) {
+                has_sent_ = true;
+            }
         }
 
         template<typename SensorType>

cmvr-es/devices/canbus/can_comm/can_sender_test.cc

@@ -77,6 +77,19 @@ namespace cmvr {
             sensor_data->enable = (bytes[2] != 0);
         }
 
+        TEST(CanSenderTest, OneShotMessageWaitsForExplicitUpdate) {
+            MyProtocol protocol;
+            SenderMessage<MySensorData> message(MyProtocol::ID, &protocol, true);
+
+            EXPECT_TRUE(message.has_sent());
+
+            protocol.SetSpeed(12.3);
+            protocol.SetEnable(true);
+            message.Update();
+
+            EXPECT_FALSE(message.has_sent());
+        }
+
 
         TEST(CanSenderTest, OneRunCase) {
             cmvr::config::SocketCanConfig cfg;

cmvr-es/devices/canbus/canopen/sdo_request_protocol.h

@@ -30,7 +30,7 @@ namespace cmvr {
                 return BASE_ID + sdo_frame_.node_id();
             }
 
-            void SetFrameData(msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data) {
+            void SetFrameData(msgs::CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data) {
                 std::lock_guard<std::mutex> lock(mutex_);
                 sdo_frame_.set_cs(cs);
                 sdo_frame_.set_index(index);

cmvr-es/devices/canbus/canopen/sdo_response_protocol.h

@@ -49,10 +49,10 @@ namespace cmvr {
             auto command = static_cast<msgs::CommandSpecifier>(bytes[0]);
 
             // 解析 index（字节1和字节2，低字节优先）
-            auto index = static_cast<msgs::ObIndex>(bytes[1] + (bytes[2] << 8));
+            const uint32_t index = bytes[1] + (bytes[2] << 8);
 
             // 解析 subindex（字节3）
-            auto subindex = static_cast<msgs::ObSubIndex>(bytes[3]);
+            const uint32_t subindex = bytes[3];
 
             // 根据 command 解析 data（字节4~7）
             uint32_t data = 0;

cmvr-es/devices/dexhand/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_subdirectory(rh56dftp_dexhand)
 add_subdirectory(px_6ax_gen3)
+add_subdirectory(zero_sim_touch_dexhand)
 
 add_library(dexhand INTERFACE)
 
@@ -9,6 +10,7 @@ target_link_libraries(dexhand
         INTERFACE
         cmvr_es::device::rh56dftp_dexhand
         cmvr_es::device::px_6ax_gen3
+        cmvr_es::device::zero_sim_touch_dexhand
         cmvr_es::proto
 )
 

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/dexhand_factory.h

@@ -10,6 +10,7 @@
 #include "devices/dexhand/abstract_dexhand.h"
 #include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h"
 #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
+#include "devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h"
 
 namespace cmvr::device {
 
@@ -32,6 +33,10 @@ public:
             return std::make_shared<PX6AXGen3>(
                 backendWithId_(cfg.id(), cfg.px_6ax_gen3()));
 
+        case config::DexHandDeviceConfig::kZeroSimTouch:
+            return std::make_shared<ZeroSimTouchDexHand>(
+                backendWithId_(cfg.id(), cfg.zero_sim_touch()));
+
         case config::DexHandDeviceConfig::BACKEND_NOT_SET:
         default:
         {

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)