Merge branch 'lgv_dev' into dev

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")
-            list(APPEND LIBRARY_DIRS "${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,11 +61,26 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
                                            const double duration,
                                            const FrameType frame)
 {
-    if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
+    SpeedLOptions options;
+    options.acceleration = acceleration;
+    return speedL(velocity, options, duration, frame);
+}
+
+Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
+                                           const SpeedLOptions& options,
+                                           const double duration, const FrameType frame)
+{
+    const double acceleration = options.acceleration;
+    if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 ||
+        !std::isfinite(acceleration) || acceleration <= 0.0 ||
+        !std::isfinite(duration) || duration < 0.0 || !std::isfinite(twistNorm_(velocity)) ||
+        (options.linear_jerk && (!std::isfinite(*options.linear_jerk) || *options.linear_jerk <= 0.0))) {
         return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
     }
-    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+    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 {};
-    }
-    return planner_->getSpeedLCommandTwistBase();
+    std::lock_guard<std::mutex> lock(mutex_);
+    return command_twist_snapshot_;
+}
+
+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);
                 }
-                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();
+        }
+    } 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));
     }
-    twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame));
-    speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool);
+
+    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,19 +238,20 @@ 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;
-        }
-        sanitizeVsq(vsq);
-        try {
-            traj_out = std::make_shared<SplineTraj>(path, grid, vsq);
-            (void) traj_out->timeInterval();
-            return true;
-        } catch (...) {
-            return false;
+            candidate = std::make_shared<ConstAccelTraj>(std::move(ca));
+        } else {
+            sanitizeVsq(vsq);
+            try {
+                candidate = std::make_shared<SplineTraj>(path, grid, vsq);
+                (void) candidate->timeInterval();
+            } catch (...) {
+                return false;
+            }
         }
+        return enforceSampledLimits(candidate, v_max_, a_max_, M, traj_out);
     }
 
 
@@ -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

@@ -114,13 +114,29 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity,
   updateIsMovingFlag();
 }
 
-  void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity,
-                                                     double acceleration)
+void SCurveVelocityPlanner1D::synchronizeAndReplan(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;
@@ -128,7 +144,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity,
     state_.jerk = 0.0;
     updateIsMovingFlag();
     return;
-      }
+  }
 
   // 如果测量值已经基本落在当前采样状态上，就继续沿现有 profile 走。
   // 否则每拍都从同一目标重规划，会把已经进入的 jerk phase 反复打断。
@@ -139,7 +155,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity,
     state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time);
     updateIsMovingFlag();
     return;
-      }
+  }
 
   state_.velocity = measured_velocity;
   state_.acceleration = measured_acceleration;

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,6 @@ arm {
             gain: 0.2
             margin_ratio: 0.15
             max_push: 0.25
-            weight: 0.05
           }
         }
       }
@@ -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,9 +151,161 @@ arm {
             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
+              }
+            }
+          }
+        }
+
+        motion {
+          move_j {
+            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
+              }
+            }
+          }
+        }
+      }
 }

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,9 +314,27 @@ 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();
-        drawTimeStamp(frame_to_encode);
+        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);
@@ -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;
     }
-    state_.is_streaming = true;
-    state_.is_opened = true;
+
+    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(mtx_);
-    state_.is_streaming = false;
-    state_.is_opened = false;
-    destroyOffscreen_();
+    {
+        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;
+        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;
@@ -273,14 +376,29 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
 
 bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
 {
-    std::lock_guard<std::mutex> lock(mtx_);
-    if (fetch_rgbd_fn_) {
+    FetchRgbdFn fetch_rgbd_fn;
+    bool consume_new_frame_only = false;
+    bool render_thread_active = false;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        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_) {
-            return false;
+        {
+            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;
         }
-        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,10 +491,25 @@ bool MujocoCamera::initOffscreen_()
         return false;
     }
 
-    std::lock_guard<std::mutex> world_lock(world->mutex());
-    const mjModel* model = world->model();
-    if (model == nullptr) {
-        setError_("[MujocoCamera] world model is null");
+    mjModel* model = nullptr;
+    {
+        std::lock_guard<std::mutex> world_lock(world->mutex());
+        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);
@@ -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)