fix(grpc): preserve BGR snapshot contract

(cherry picked from commit 78fd7d7a04)

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)
 
@@ -29,6 +28,37 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 include(FindExternalLib)
 set(ARCH "x86")
 setup_external_libs(${ARCH})
+# Intel oneVPL / VA-API runtime. The shared libraries in lib/ are installed
+# by setup_external_libs(); the VA-API driver plugin directory is installed
+# separately because it must retain its dri layout.
+set(INTEL_MEDIA_STACK_ROOT
+        "${PROJECT_SOURCE_DIR}/dependency/${ARCH}/third_party/intel-media-stack/vpl-2.17"
+)
+set(INTEL_MEDIA_DRIVER_DIR "${INTEL_MEDIA_STACK_ROOT}/lib/dri")
+set(INTEL_IHD_DRIVER "${INTEL_MEDIA_DRIVER_DIR}/iHD_drv_video.so")
+
+if(NOT EXISTS "${INTEL_IHD_DRIVER}")
+    message(FATAL_ERROR "Intel iHD VA-API driver not found: ${INTEL_IHD_DRIVER}")
+endif()
+
+message(STATUS "Intel media stack: ${INTEL_MEDIA_STACK_ROOT}")
+install(
+        DIRECTORY "${INTEL_MEDIA_DRIVER_DIR}/"
+        DESTINATION lib/dri
+)
+install(CODE [=[
+    find_program(CMVR_PATCHELF_EXECUTABLE patchelf REQUIRED)
+    set(_cmvr_ihd_driver
+            "${CMAKE_INSTALL_PREFIX}/lib/dri/iHD_drv_video.so"
+    )
+    execute_process(
+            COMMAND "${CMVR_PATCHELF_EXECUTABLE}"
+                    --set-rpath "$ORIGIN/.."
+                    "${_cmvr_ihd_driver}"
+            COMMAND_ERROR_IS_FATAL ANY
+    )
+]=])
+
 # 在调用 setup_external_libs 之后
 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}")
 message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}")

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.*

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

@@ -112,6 +112,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

@@ -149,4 +149,156 @@ arm {
       }
     }
   }
+
+
+  robot_arms {
+      id: "eyou_arm"
+
+      motor {
+        motor_system_id: "ethercat_motors"
+        motor_group_ids: "dual_arm_ethercat"
+        dof: 7
+        joint_names: "R_SHOULDER_P"
+        joint_names: "R_SHOULDER_R"
+        joint_names: "R_SHOULDER_Y"
+        joint_names: "R_ELBOW_R"
+        joint_names: "R_WRIST_P"
+        joint_names: "R_WRIST_Y"
+        joint_names: "R_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: "R_WRIST_R_S"
+          tcp_frame_name: "R_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: "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 }
+            }
+            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
+              weight: 0.05
+            }
+          }
+        }
+      }
+
+      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_eyou_left.pb.txt

@@ -0,0 +1,152 @@
+arm {
+  robot_arms {
+    id: "eyou_left_arm"
+
+    motor {
+      motor_system_id: "ethercat_motors"
+      motor_group_ids: "dual_arm_ethercat"
+      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
+            weight: 0.05
+          }
+        }
+      }
+    }
+
+    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_system_id: "right_arm_mujoco_motors"
-      motor_group_ids: "mujoco_right_arm"
+      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_system_id: "right_arm_mujoco_motors"
-      motor_group_ids: "mujoco_right_arm"
+      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_system_id: "right_arm_can_motors"
-      motor_group_ids: "right_arm_can"
+      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.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
@@ -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

@@ -83,8 +83,8 @@ camera {
         stream_mode: STREAM_MODE_RGBD
       }
       encoder {
-        width: 1280
+        width: 480
-        height: 720
+        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
     }
@@ -38,4 +38,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,93 @@
+motor {
+  id: "ethercat_motors"
+
+  motor_groups {
+    id: "dual_arm_ethercat"
+    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: 1 }
+      slaves { motor_id: 2 alias: 0 position: 2 }
+      slaves { motor_id: 3 alias: 0 position: 3 }
+      slaves { motor_id: 4 alias: 0 position: 4 }
+      slaves { motor_id: 5 alias: 0 position: 5 }
+      slaves { motor_id: 6 alias: 0 position: 6 }
+      slaves { motor_id: 7 alias: 0 position: 7 }
+      slaves { motor_id: 8 alias: 0 position: 8 }
+      slaves { motor_id: 9 alias: 0 position: 9 }
+      slaves { motor_id: 10 alias: 0 position: 10 }
+      slaves { motor_id: 11 alias: 0 position: 11 }
+      slaves { motor_id: 12 alias: 0 position: 12 }
+      slaves { motor_id: 13 alias: 0 position: 13 }
+      slaves { motor_id: 14 alias: 0 position: 14 }
+    }
+
+    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 }
+      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 }
+    }
+
+    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 }
+      motors { id: 8 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 9 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 10 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 11 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 12 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 13 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 14 joint_name: "L_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: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 24 joint_name: "L_SHOULDER_R" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 17 joint_name: "R_SHOULDER_R" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 30 joint_name: "HEAD_P" }
+      motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 31 joint_name: "HEAD_R" }
+      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: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 15 joint_name: "WAIST_P" }
+      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,6 +2,7 @@ device_manager {
   name: "cmvr_es"
   version: "0.1"
   description: "cmvr edge system version 0.1"
+  init_all_motors_when_no_active_joints: true
 
   devices {
     id: "mujoco_world"
@@ -74,6 +75,13 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "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
@@ -81,6 +89,20 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "eyou_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "eyou_left_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm_eyou_left.pb.txt"
+    enable: false
+  }
+
   devices {
     id: "aubo_arm"
     type: DEVICE_TYPE_ROBOT_ARM
@@ -92,7 +114,7 @@ device_manager {
     id: "huayan_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/huayan_arm.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -103,9 +125,56 @@ device_manager {
   }
   
   devices {
-    id: "agv_1"
+    id: "src1100"
     type: DEVICE_TYPE_AGV
-    config_file: "devices/agv/agv.pb.txt"
+    config_file: "devices/agv/src1100.pb.txt"
     enable: false
   }
+
+  devices {
+      id: "hikvision_cam"
+      type: DEVICE_TYPE_CAMERA
+      config_file: "devices/camera/camera.pb.txt"
+      # Host-development default: keep physical cameras disabled.
+      enable: false
+    }
+  devices {
+      id: "hikvision_thermal_cam"
+      type: DEVICE_TYPE_CAMERA
+      config_file: "devices/camera/camera.pb.txt"
+      # Host-development default: keep physical cameras disabled.
+      enable: false
+    }
+
+    devices {
+      id: "mic1"
+      type: DEVICE_TYPE_MICROPHONE
+      config_file: "devices/microphone/microphone.pb.txt"
+      # Host-development default: keep physical audio devices disabled.
+      enable: false
+    }
+
+    devices {
+      id: "spk1"
+      type: DEVICE_TYPE_SPEAKER
+      config_file: "devices/speaker/speaker.pb.txt"
+      # Host-development default: keep physical audio devices disabled.
+      enable: false
+    }
+
+
+    devices {
+          id: "real_cam1"
+          type: DEVICE_TYPE_CAMERA
+          config_file: "devices/camera/camera.pb.txt"
+          # Host-development default: keep physical cameras disabled.
+          enable: false
+        }
+    devices {
+          id: "usb_cam1"
+          type: DEVICE_TYPE_CAMERA
+          config_file: "devices/camera/camera.pb.txt"
+          # Host-development default: keep physical cameras disabled.
+          enable: false
+        }
 }

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

@@ -4,7 +4,7 @@ task_manager {
     type: TASK_TYPE_TOUCH_SCREEN
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.001
-    config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
+    config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"
     enable: false
   }
   tasks {
@@ -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,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"
     dexhand_id: "paxini_tip_1"
+    # 手部相机和外部相机的 DeviceManager ID。
     camera_id: "right_hand_cam"
+    external_camera_id: "cam5"
   }
 
   initialization {
@@ -19,46 +25,56 @@ touch_screen_task {
     joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 }
     velocity: 1.0
     acceleration: 2.0
+    # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
+    skip_position_tolerance_rad: 0.001
+    # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
+    skip_velocity_tolerance_rad_s: 0.01
   }
 
   perception {
-    apriltag {
+    # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
-      tag_size_m: 0.012
+    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 {
+    calibration {
-      camera_link: "R_CAM"
+      #  TCP P 相对于屏幕 Hand Tag H 的目标姿态
-      lambda: 0.4
+      hand_tag_to_tcp {
-      mu: 0.1
+        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
-      qdot_max: 1.0
+        m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
+        m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
+        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.001 y: 0.08 z: 0.15 }
+      # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
+      # 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 {
@@ -92,6 +108,7 @@ 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
+    # TCP 后退目标距离，单位为米。
+    distance_m: 0.02
   }
 }

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,54 +23,73 @@ 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
+    velocity: 2.0
-    acceleration: 20.0
+    acceleration: 3.0
+    # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
+    skip_position_tolerance_rad: 0.001
+    # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
+    skip_velocity_tolerance_rad_s: 0.01
   }
 
   perception {
-    apriltag {
+    # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
-      tag_size_m: 0.12
+    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 {
+    calibration {
-      camera_link: "R_CAM"
+      # TCP P 相对于屏幕 Hand Tag H 的目标姿态
-      lambda: 0.4
+      hand_tag_to_tcp {
-      mu: 0.1
+        m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
-      qdot_max: 0.8
+        m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
+        m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
+        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 }
+      # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
+      # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
-      mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
+      # 旋转组合为 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
@@ -78,7 +103,7 @@ touch_screen_task {
     speed_l {
       twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
       acceleration: 6.0
-      max_distance_m: 0.12
+      max_distance_m: 0.02
     }
     tactile {
       finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
@@ -90,8 +115,9 @@ touch_screen_task {
   }
 
   retract {
-    twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
+    twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 8.0
+    acceleration: 4.0
-    duration_s: 5.0
+    # TCP 后退目标距离，单位为米。
+    distance_m: 0.05
   }
 }

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

@@ -4,10 +4,58 @@ add_library(aubo_arm SHARED
 
 target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include)
+set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
-set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib)
+set(AUBO_SDK_INCLUDE_DIR ${AUBO_SDK_ROOT}/include)
+set(AUBO_SDK_LIB_DIR ${AUBO_SDK_ROOT}/lib)
 
-if (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h")
+if (EXISTS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk/aubo_sdkConfig.cmake")
+    list(APPEND CMAKE_PREFIX_PATH "${AUBO_SDK_LIB_DIR}/cmake")
+    find_package(Qt5Core QUIET)
+    if (NOT Qt5Core_FOUND AND NOT TARGET Qt5::Core)
+        find_library(QT5_CORE_LIBRARY
+                NAMES Qt5Core libQt5Core.so.5
+                PATHS /lib /usr/lib /usr/local/lib /lib/x86_64-linux-gnu /usr/lib/x86_64-linux-gnu
+        )
+        if (QT5_CORE_LIBRARY)
+            add_library(Qt5::Core UNKNOWN IMPORTED)
+            set_target_properties(Qt5::Core PROPERTIES
+                    IMPORTED_LOCATION "${QT5_CORE_LIBRARY}"
+            )
+        endif()
+    endif()
+    find_package(aubo_sdk REQUIRED CONFIG PATHS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk" NO_DEFAULT_PATH)
+
+    # The vendor directory contains an old private libstdc++. Keep it out of
+    # consumers' RUNPATH by staging only the AUBO runtime libraries.
+    set(AUBO_CLEAN_LIB_DIR "${CMAKE_CURRENT_BINARY_DIR}/aubo_sdk_runtime")
+    file(MAKE_DIRECTORY "${AUBO_CLEAN_LIB_DIR}")
+    foreach(AUBO_LIB
+            libaubo_sdk.so
+            libaubo_sdkd.so
+            librobot_proxy.so
+            librobot_proxyd.so)
+        file(COPY_FILE
+                "${AUBO_SDK_LIB_DIR}/${AUBO_LIB}"
+                "${AUBO_CLEAN_LIB_DIR}/${AUBO_LIB}"
+                ONLY_IF_DIFFERENT
+        )
+    endforeach()
+
+    set_target_properties(aubo_sdk::aubo_sdk aubo_sdk::robot_proxy PROPERTIES
+            MAP_IMPORTED_CONFIG_DEBUG Release
+    )
+    set_target_properties(aubo_sdk::aubo_sdk PROPERTIES
+            IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/libaubo_sdk.so"
+            IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/libaubo_sdkd.so"
+    )
+    set_target_properties(aubo_sdk::robot_proxy PROPERTIES
+            IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/librobot_proxy.so"
+            IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/librobot_proxyd.so"
+    )
+    target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
+    target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
+    target_link_libraries(aubo_arm PRIVATE aubo_sdk::aubo_sdk)
+elseif (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h")
     target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
     target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
     if (EXISTS "${AUBO_SDK_LIB_DIR}")

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

@@ -36,6 +36,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

@@ -42,6 +42,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

@@ -34,3 +34,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/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 isProtectiveStopped() const override { return protective_stopped_.load(); }
-    bool isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isEmergencyStopped() const override { return emergency_stopped_.load(); }
     bool isFault() const override { return false; }
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
@@ -73,10 +76,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 +97,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 +132,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,6 +1,8 @@
 #include "arm/motor_robot_arm/include/motor_robot_arm.h"
 
+#include <algorithm>
 #include <chrono>
+#include <cmath>
 #include <Eigen/Dense>
 #include <stdexcept>
 #include <thread>
@@ -28,6 +30,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 +139,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 +196,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 +212,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 +228,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 +271,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 +506,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 +516,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 +548,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) {
+        std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
-            const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0;
+        // A MoveJ is a rest-to-rest command.  Do not let a non-zero numerical
-            motors[i]->setTarget(sample.position[i], qd);
+        // 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 +616,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 +633,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 +656,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 +668,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                             const MotionOptions& options,
                             const FrameType frame)
 {
-    if (cartesian_velocity_controller_) {
+    if (const auto stopped = safetyStopResult_("moveL")) {
-        cartesian_velocity_controller_->shutdown();
+        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 +713,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                           << ", executable_path_m=" << trajectory.executable_path_length;
     }
 
-    return executeMoveLTrajectory_(trajectory) ? Result::success()
+    if (executeMoveLTrajectory_(trajectory)) {
-                                               : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
+        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 +727,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");
     }
@@ -422,7 +749,10 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
 
 Result MotorRobotArm::stopMotion()
 {
-    stopL(0.0);
+    const auto cartesian_stop = stopCartesianMotionAndWait_();
+    if (!cartesian_stop.ok()) {
+        return cartesian_stop;
+    }
     return stopJ(0.0);
 }
 
@@ -442,12 +772,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 +790,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 +1000,142 @@ 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");
+    }
+
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::duration<double>(move_j_settle_timeout_s_);
+    int 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();
+            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;
+                break;
+            }
+        }
+
+        stable_samples = settled ? stable_samples + 1 : 0;
+        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_;
+    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 +1203,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 +1213,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) {
+        if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
-            motors[j]->setTarget(position[j], velocity[j]);
+            return false;
         }
         next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
             std::chrono::duration<double>(dt_segment));
@@ -765,6 +1248,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;
+        motor_system_ = std::make_shared<MotorManager>(
-        for (const auto* joint_name : kJointNames) {
+            "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
-            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());
         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/robot_arm.h

@@ -34,6 +34,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;

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 cx{0.0F};
-        float   cy;
+        float cy{0.0F};
-        float   fx;
+        float fx{0.0F};
-        float   fy;
+        float fy{0.0F};
-        float   coeffs[5];
+        float coeffs[5]{};
 
     };
     struct StreamFrameData
@@ -31,12 +33,34 @@ namespace cmvr::device {
         std::vector<uint8_t> depthFrame;
         //编码格式
         std::string codec = ".h264";
-        Rs2Intrinsics intrinsics;
+        Rs2Intrinsics intrinsics{};
-        int width;
+        int width = 0;
-        int height;
+        int height = 0;
-        int fps;
+        // Depth frames may use a different resolution from the encoded color frame.
-        bool bKey;
+        int depth_width = 0;
-        bool depthKey;
+        int depth_height = 0;
+        int fps = 0;
+        bool bKey = false;
+        bool depthKey = false;
+
+        // Protocol-neutral real-time metadata. The producer fills these values
+        // when a complete encoded access unit is published.
+        uint64_t stream_epoch = 0;
+        uint64_t sequence = 0;
+        // Opaque producer-native timing/counter values. Their units and epoch
+        // are source-defined; zero means that the source did not provide them.
+        uint64_t source_timestamp = 0;
+        uint64_t source_frame_number = 0;
+        int64_t capture_monotonic_ns = 0;
+        int64_t capture_utc_ns = 0;
+        int64_t pts = 0;
+        int64_t dts = 0;
+        int32_t time_base_num = 1;
+        int32_t time_base_den = 1;
+        int64_t duration = 0;
+        bool discontinuity = false;
+        uint32_t codec_config_generation = 0;
+        std::vector<uint8_t> codec_config;
     };
     class AbstractCamera : public AbstractDevice {
     public:
@@ -64,11 +88,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;
+        std::lock_guard<std::mutex> lock(cache_mtx_);
-    state_.is_opened = false;
+        render_stop_requested_ = true;
-    destroyOffscreen_();
+    }
+    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_);
+    FetchRgbdFn fetch_rgbd_fn;
-    if (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());
+    mjModel* model = nullptr;
-    const mjModel* model = world->model();
+    {
-    if (model == nullptr) {
+        std::lock_guard<std::mutex> world_lock(world->mutex());
-        setError_("[MujocoCamera] world model is null");
+        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());
+        std::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
-        mjModel* model = world->model();
+        if (!world_lock.owns_lock()) {
-        mjData* data = world->data();
+            // Never make the simulation wait for a camera frame. The next
-        if (model == nullptr || data == nullptr) {
+            // 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

@@ -5,6 +5,8 @@
 
 #include "../include/realsense_camera.h"
 
+#include <cstring>
+
 using namespace std;
 using namespace cmvr::device;
 
@@ -174,8 +176,9 @@ bool RealsenseCamera::init() {
 
         //初始化编码器
         // 初始化RGB编码器（示例参数：640x480，30fps，H.264）
-        if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
+        if (stream_mode_ != DEPTH_MODE &&
-            CMVR_LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!";
+            !CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
+            CMVR_LOG(ERROR) << "[RealsenseCamera] (init): Failed to init RGB encoder!";
             state_.is_initialized = false;
             state_.is_error = true;
             state_.error_message = "Failed to init RGB encoder!";
@@ -247,16 +250,20 @@ bool RealsenseCamera::start() {
                 intrinsics_ = Kd_;
             }
 
-            if (align_mode_ == "color") {
+            if (stream_mode_ == RGBD_MODE) {
-                align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
+                if (align_mode_ == "color") {
-            }
+                    align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
-            else if (align_mode_ == "depth") {
+                }
-                align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
+                else if (align_mode_ == "depth") {
+                    align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
+                }
             }
 
             // 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。
             rs2::frameset probe = pipe_.wait_for_frames(kProbeTimeoutMs);
-            if (!probe.get_color_frame()) {
+            const bool needs_color = stream_mode_ == COLOR_MODE || stream_mode_ == RGBD_MODE;
+            const bool needs_depth = stream_mode_ == DEPTH_MODE || stream_mode_ == RGBD_MODE;
+            if (needs_color && !probe.get_color_frame()) {
                 last_error = "no color frame after start";
                 CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error;
                 pipe_.stop();
@@ -264,7 +271,7 @@ bool RealsenseCamera::start() {
                 std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs));
                 continue;
             }
-            if (stream_mode_ == RGBD_MODE && !probe.get_depth_frame()) {
+            if (needs_depth && !probe.get_depth_frame()) {
                 last_error = "no depth frame after start";
                 CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error;
                 pipe_.stop();
@@ -721,17 +728,19 @@ void RealsenseCamera::streaming_worker_() {
 
             rs2::frameset frames;
             frames = get_frameset(true);
-            rs2::frame color_frame = frames.get_color_frame();
+            rs2::video_frame color_frame = frames.get_color_frame();
-            rs2::frame depth_frame = frames.get_depth_frame();
+            rs2::depth_frame depth_frame = frames.get_depth_frame();
-            if (!color_frame) {
+            const bool needs_color = stream_mode_ == COLOR_MODE || stream_mode_ == RGBD_MODE;
+            const bool needs_depth = stream_mode_ == DEPTH_MODE || stream_mode_ == RGBD_MODE;
+            if (needs_color && !color_frame) {
                 state_.is_error = true;
                 state_.error_message = "missing color frame";
                 CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message;
                 break;
             }
-            if (stream_mode_ == RGBD_MODE && !depth_frame) {
+            if (needs_depth && !depth_frame) {
                 state_.is_error = true;
-                state_.error_message = "missing depth frame in RGBD mode";
+                state_.error_message = "missing depth frame";
                 CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message;
                 break;
             }
@@ -747,43 +756,90 @@ void RealsenseCamera::streaming_worker_() {
             }
 
             // 保存图像数据
-            {
+            if (color_frame) {
-                cv::Mat temp(cv::Size(width_, height_), CV_8UC3, const_cast<void*>(color_frame.get_data()), cv::Mat::AUTO_STEP);
+                cv::Mat temp(cv::Size(color_frame.get_width(), color_frame.get_height()),
+                             CV_8UC3, const_cast<void*>(color_frame.get_data()), cv::Mat::AUTO_STEP);
                 temp.copyTo(frame_data.rgbImage); // 执行深拷贝
             }
             if (depth_frame) {
-                auto temp = cv::Mat(cv::Size(width_, height_), CV_16UC1, const_cast<void*>(depth_frame.get_data()));
+                cv::Mat temp(cv::Size(depth_frame.get_width(), depth_frame.get_height()),
+                             CV_16UC1, const_cast<void*>(depth_frame.get_data()), cv::Mat::AUTO_STEP);
                 temp.copyTo(frame_data.depthImage); // 执行深拷贝
             }
 
             // 记录编码开始时间
+            if (depth_frame) {
+                frame_data.depth_width = frame_data.depthImage.cols;
+                frame_data.depth_height = frame_data.depthImage.rows;
+                frame_data.depthKey = true;
+                const size_t depth_bytes = frame_data.depthImage.total() * frame_data.depthImage.elemSize();
+                frame_data.depthFrame.resize(depth_bytes);
+                std::memcpy(frame_data.depthFrame.data(), frame_data.depthImage.data, depth_bytes);
+            }
+
             auto encode_start_time = std::chrono::high_resolution_clock::now();
 
             // rgb图像编码
-            cv::Mat rgb_to_encode = frame_data.rgbImage;
+            success = true;
-            if (encode_width_ > 0 && encode_height_ > 0 &&
+            if (needs_color) {
-                (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) {
+                cv::Mat rgb_to_encode = frame_data.rgbImage;
-                cv::resize(frame_data.rgbImage,
+                if (encode_width_ > 0 && encode_height_ > 0 &&
-                           rgb_to_encode,
+                    (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) {
-                           cv::Size(encode_width_, encode_height_),
+                    cv::resize(frame_data.rgbImage,
-                           0.0,
+                               rgb_to_encode,
-                           0.0,
+                               cv::Size(encode_width_, encode_height_),
-                           cv::INTER_LINEAR);
+                               0.0,
+                               0.0,
+                               cv::INTER_LINEAR);
+                }
+                CameraStreamEncodeOptions encode_options;
+                encode_options.draw_timestamp = enable_stream_timestamp_;
+                success = CameraStreamEncoder::encode(rgbEncoder_,
+                                                      rgb_to_encode,
+                                                      frame_data.rgbFrame,
+                                                      frame_data.bKey,
+                                                      encode_options);
             }
             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);
+            if (needs_depth && frame_data.depthFrame.empty())
+                success = false;
             if (success) {
+                const uint64_t sequence = frame_sequence++;
+                frame_data.stream_epoch = stream_epoch;
+                frame_data.sequence = sequence;
+                const rs2::frame source_frame = color_frame ? color_frame : depth_frame;
+                frame_data.source_timestamp = source_frame
+                    ? static_cast<uint64_t>(source_frame.get_timestamp()) : 0;
+                frame_data.source_frame_number = source_frame ? source_frame.get_frame_number() : 0;
+                frame_data.capture_monotonic_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                    frame_start_time.time_since_epoch()).count();
+                frame_data.capture_utc_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                    std::chrono::system_clock::now().time_since_epoch()).count();
+                frame_data.pts = static_cast<int64_t>(sequence);
+                frame_data.dts = frame_data.pts;
+                frame_data.time_base_num = 1;
+                frame_data.time_base_den = fps_;
+                frame_data.duration = 1;
                 frame_data.fps = fps_;
-                frame_data.width = encode_width_;
+                frame_data.width = needs_color ? encode_width_ : frame_data.depth_width;
-                frame_data.height = encode_height_;
+                frame_data.height = needs_color ? encode_height_ : frame_data.depth_height;
-                frame_data.codec = codec_;
+                frame_data.codec = needs_color ? codec_ : "none";
                 stream_frame_buffer_->push(frame_data);
             }
             // 计算从帧开始到现在的总耗时

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/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/zero_sim_touch_dexhand/CMakeLists.txt

@@ -0,0 +1,13 @@
+add_library(zero_sim_touch_dexhand SHARED src/zero_sim_touch_dexhand.cpp)
+
+target_include_directories(zero_sim_touch_dexhand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+add_library(cmvr_es::device::zero_sim_touch_dexhand ALIAS zero_sim_touch_dexhand)
+
+target_link_libraries(zero_sim_touch_dexhand
+        PRIVATE
+        cmvr_es::proto
+        glog
+)
+
+install(TARGETS zero_sim_touch_dexhand LIBRARY DESTINATION lib)

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

@@ -0,0 +1,57 @@
+#ifndef CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
+#define CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
+
+#include <array>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
+#include "../../abstract_dexhand.h"
+
+namespace cmvr::device {
+
+class ZeroSimTouchDexHand final : public AbstractDexHand {
+public:
+    using FingerType = AbstractDexHand::FingerType;
+    using ResultantForce = AbstractDexHand::ResultantForce;
+    using TactilePoint = AbstractDexHand::TactilePoint;
+    using TactileRegion = AbstractDexHand::TactileRegion;
+    using TactileRegionKey = AbstractDexHand::TactileRegionKey;
+    using TactileRegionData = AbstractDexHand::TactileRegionData;
+    using Status = AbstractDexHand::Status;
+
+    explicit ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg);
+    ~ZeroSimTouchDexHand() override = default;
+
+    std::string typeName() const override { return "ZeroSimTouchDexHand"; }
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+
+    Status state() const override;
+    std::string lastError() const override;
+    void getState(DexHandState& state) override;
+
+    void setAngles(const std::vector<int>& finger_joint_angles) override;
+    void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
+    std::vector<TactileRegionData> getSensorData() override;
+    TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
+    ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
+
+private:
+    TactileRegionData makeRegionData(FingerType finger, TactileRegion region);
+
+    mutable std::mutex mutex_;
+    std::vector<TactileRegionKey> polling_regions_;
+    std::array<TactilePoint, 1> tactile_points_{};
+    Status lifecycle_state_{Status::CREATED};
+    std::string last_error_;
+};
+
+// Keep the historical spelling available to callers that used the test double.
+using ZeroSImTouchDexHand = ZeroSimTouchDexHand;
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H

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

@@ -0,0 +1,96 @@
+#include "../include/zero_sim_touch_dexhand.h"
+
+#include <utility>
+
+namespace cmvr::device {
+
+ZeroSimTouchDexHand::ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg) {
+    id_ = cfg.id();
+}
+
+bool ZeroSimTouchDexHand::init() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    lifecycle_state_ = Status::STREAMING;
+    last_error_.clear();
+    return true;
+}
+
+bool ZeroSimTouchDexHand::start() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    lifecycle_state_ = Status::STREAMING;
+    last_error_.clear();
+    return true;
+}
+
+bool ZeroSimTouchDexHand::stop() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    lifecycle_state_ = Status::STOPPED;
+    return true;
+}
+
+ZeroSimTouchDexHand::Status ZeroSimTouchDexHand::state() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return lifecycle_state_;
+}
+
+std::string ZeroSimTouchDexHand::lastError() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return last_error_;
+}
+
+void ZeroSimTouchDexHand::getState(DexHandState& state) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state = DexHandState{};
+    state.is_initialized = lifecycle_state_ == Status::INITIALIZED ||
+                           lifecycle_state_ == Status::STREAMING;
+    if (!last_error_.empty()) {
+        state.hands[0].error_message.push_back(last_error_);
+    }
+}
+
+void ZeroSimTouchDexHand::setAngles(const std::vector<int>&) {
+    // The simulation has no finger actuators; commands are intentionally ignored.
+}
+
+void ZeroSimTouchDexHand::setTactilePollingRegions(
+    const std::vector<TactileRegionKey>& regions) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    polling_regions_ = regions;
+}
+
+std::vector<ZeroSimTouchDexHand::TactileRegionData> ZeroSimTouchDexHand::getSensorData() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (polling_regions_.empty()) {
+        polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP});
+    }
+
+    std::vector<TactileRegionData> data;
+    data.reserve(polling_regions_.size());
+    for (const auto& region : polling_regions_) {
+        data.push_back(makeRegionData(region.first, region.second));
+    }
+    return data;
+}
+
+ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::getSensorData(
+    const FingerType finger, const TactileRegion region) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return makeRegionData(finger, region);
+}
+
+ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce(
+    const FingerType, const TactileRegion) {
+    return TactilePoint::fromFz(0);
+}
+
+ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData(
+    const FingerType finger, const TactileRegion region) {
+    tactile_points_[0] = TactilePoint::fromFz(0);
+    TactileMatrixView view;
+    view.data = tactile_points_.data();
+    view.rows = 1;
+    view.cols = 1;
+    return {finger, region, view, "mujoco_touch_tip"};
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/CMakeLists.txt

@@ -1,6 +1,10 @@
 add_library(motor_core INTERFACE)
 
-target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices)
+target_include_directories(motor_core
+        INTERFACE
+        ${CMAKE_SOURCE_DIR}/cmvr-es
+        ${CMAKE_SOURCE_DIR}/cmvr-es/devices
+)
 
 target_link_libraries(motor_core
         INTERFACE
@@ -12,4 +16,5 @@ add_library(cmvr_es::device::motor_core ALIAS motor_core)
 add_subdirectory(drivers/ti5_canopen)
 add_subdirectory(drivers/mujoco)
 add_subdirectory(bus_runtime)
+add_subdirectory(drivers/ethercat_motor)
 add_subdirectory(manager)

cmvr-es/devices/motor/abstract_motor.h

@@ -48,13 +48,13 @@ namespace cmvr::device{
 
         DeviceKind kind() const noexcept override { return DeviceKind::Motor; }
 
-        virtual void setMode(msgs::RunMode mode) {
+        virtual bool setMode(msgs::RunMode mode) {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->setMode(node_id_, mode);
+            return protocol_->setMode(node_id_, mode);
         }
 
         virtual msgs::RunMode getMode() {
@@ -66,13 +66,22 @@ namespace cmvr::device{
             return protocol_->getMode(node_id_);
         }
 
-        virtual void torqueOff() {
+        virtual bool torqueOn() {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->torqueOff(node_id_);
+            return protocol_->torqueOn(node_id_);
+        }
+
+        virtual bool torqueOff() {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->torqueOff(node_id_);
         }
 
         virtual void setLimitQ(double ub, double lb) {
@@ -101,43 +110,69 @@ namespace cmvr::device{
         }
         // virtual void setLimitTau(double tau) = 0;
         // virtual void setLimitCurrent(double tau) = 0;
-        virtual void brake() {
+        virtual bool brakeRelease() {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->brake(node_id_);
+            return protocol_->brakeRelease(node_id_);
-        }
-        /**
-         *
-         * @param q unit : rad
-         */
-        virtual void setQ(double q) {
-            std::scoped_lock lock(mtx_);
-            if (!protocol_) {
-                CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
-            }
-            protocol_->setQ(node_id_, q);
         }
 
-        virtual void setTarget(double q,double qd) {
+        virtual bool quickStop() {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->setTarget(node_id_,q, qd);
+            return protocol_->quickStop(node_id_);
+        }
+        virtual bool commandProfilePosition(double target_q,
+                                            double max_qd = 0.0,
+                                            double max_qdd = 0.0) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandProfilePosition(node_id_, target_q, max_qd, max_qdd);
         }
 
-        virtual void setTarget(double qd) {
+        virtual bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->setTarget(node_id_, qd);
+            return protocol_->commandProfileVelocity(node_id_, target_qd, max_qdd);
+        }
+
+        virtual bool commandCyclicPosition(double target_q,
+                                           double target_qd = 0.0) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandCyclicPosition(node_id_, target_q, target_qd);
+        }
+
+        virtual bool commandCyclicVelocity(double target_qd) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandCyclicVelocity(node_id_, target_qd);
+        }
+
+        virtual bool commandCyclicTorque(double target_tau) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandCyclicTorque(node_id_, target_tau);
         }
 
         virtual bool calibrateZeroQ() {
@@ -156,16 +191,6 @@ namespace cmvr::device{
             }
             return  protocol_->reachedTargetQ(node_id_);
         }
-        // rad /s
-        virtual void setQd(double qd) {
-            std::scoped_lock lock(mtx_);
-            if (!protocol_) {
-                CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
-            }
-            return  protocol_->setQd(node_id_,qd);
-        }
-        // virtual void setQdd(double qdd) = 0;  // rad /s^2
         // virtual void setTau(double tau) = 0;   // N m
         // virtual void clear_err() = 0;
         // virtual void getStatus() = 0;
@@ -189,14 +214,14 @@ namespace cmvr::device{
 
 
         // 使用的通讯协议
-        virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
+        virtual bool setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
             std::scoped_lock lock(mtx_);
             protocol_ = std::move(protocol);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->initNode(node_id_);
+            return protocol_->initNode(node_id_);
         }
 
         uint8_t id() const {

cmvr-es/devices/motor/bus_runtime/CMakeLists.txt

@@ -4,7 +4,18 @@ add_library(motor_bus_runtime SHARED
         ethercat/src/ethercat_motor_bus_runtime.cpp
 )
 
-target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+set(IGH_ETHERCAT_ROOT
+        ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0
+)
+
+target_include_directories(motor_bus_runtime
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+        PRIVATE
+        ${IGH_ETHERCAT_ROOT}/include
+)
+
+target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib)
 
 target_link_libraries(motor_bus_runtime
         PUBLIC
@@ -12,9 +23,28 @@ target_link_libraries(motor_bus_runtime
         cmvr_es::device::motor_core
         cmvr_es::mujoco_world
         PRIVATE
+        ethercat
         cmvr_es::device::canbus
         glog
 )
 
 add_library(cmvr_es::device::motor_bus_runtime ALIAS motor_bus_runtime)
 install(TARGETS motor_bus_runtime LIBRARY DESTINATION lib)
+
+add_executable(ethercat_motor_bus_runtime_real_test
+        ethercat/src/ethercat_motor_bus_runtime_real_test.cpp
+)
+
+target_include_directories(ethercat_motor_bus_runtime_real_test
+        PRIVATE
+        ${CMAKE_SOURCE_DIR}/cmvr-es
+)
+
+target_link_libraries(ethercat_motor_bus_runtime_real_test
+        PRIVATE
+        cmvr_es::device::motor_bus_runtime
+        gtest
+        gtest_main
+        pthread
+        glog
+)

cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp

@@ -68,16 +68,16 @@ bool CanMotorBusRuntime::start()
         return false;
     }
 
-    auto ret = sender_->Start();
+    auto ret = receiver_->Start();
     if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
         stop();
         return false;
     }
 
-    ret = receiver_->Start();
+    ret = sender_->Start();
     if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
         stop();
         return false;
     }

cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h

@@ -1,15 +1,45 @@
 #ifndef CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
 #define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
 
+#include <atomic>
+#include <chrono>
+#include <cstddef>
+#include <cstdint>
+#include <cstring>
+#include <mutex>
 #include <string>
+#include <thread>
+#include <type_traits>
 #include <unordered_map>
+#include <vector>
 
 #include "../../abstract_motor_bus_runtime.h"
+#include "motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h"
+
+typedef struct ec_domain ec_domain_t;
+typedef struct ec_master ec_master_t;
+typedef struct ec_slave_config ec_slave_config_t;
 
 namespace cmvr::device {
 
 class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime {
 public:
+    struct PdoWrite {
+        int motor_id{0};
+        std::uint16_t index{0};
+        std::uint8_t subindex{0};
+        std::uint8_t bit_length{0};
+        std::uint64_t raw_value{0};
+    };
+
+    struct PdoRead {
+        int motor_id{0};
+        std::uint16_t index{0};
+        std::uint8_t subindex{0};
+        std::uint8_t bit_length{0};
+        std::uint64_t raw_value{0};
+    };
+
     bool init(const config::MotorGroupConfig& group_cfg) override;
     bool start() override;
     void stop() override;
@@ -17,12 +47,194 @@ public:
 
     const std::string& id() const { return id_; }
     const config::EtherCATConfig& config() const { return config_; }
+    void setPdoMapping(EthercatPdoMapping mapping);
+    const EthercatPdoMapping& pdoMapping() const { return pdo_mapping_; }
     const config::EthercatSlaveConfig* slaveForMotor(int motor_id) const;
+    bool hasMotor(int motor_id) const;
+
+    bool hasPdoEntry(int motor_id, std::uint16_t index, std::uint8_t subindex) const;
+
+    template <typename T>
+    bool writePdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value)
+    {
+        return writePdoRaw_(motor_id, index, subindex, valueBitLength_<T>(),
+                            toRawValue_(value));
+    }
+
+    template <typename T>
+    static PdoWrite makePdoWrite(int motor_id,
+                                 std::uint16_t index,
+                                 std::uint8_t subindex,
+                                 T value)
+    {
+        return PdoWrite{motor_id, index, subindex, valueBitLength_<T>(), toRawValue_(value)};
+    }
+
+    bool writePdosAtomic(const PdoWrite* writes, std::size_t count);
+
+    template <typename T>
+    static PdoRead makePdoRead(int motor_id,
+                               std::uint16_t index,
+                               std::uint8_t subindex)
+    {
+        return PdoRead{motor_id, index, subindex, valueBitLength_<T>(), 0};
+    }
+
+    template <typename T>
+    static T pdoReadValue(const PdoRead& read)
+    {
+        return fromRawValue_<T>(read.raw_value);
+    }
+
+    bool readPdosAtomic(PdoRead* reads, std::size_t count) const;
+
+    std::uint64_t commandGeneration() const { return command_generation_.load(); }
+    std::uint64_t sentCommandGeneration() const { return sent_command_generation_.load(); }
+    bool isHealthy() const { return healthy_.load(); }
+
+    template <typename T>
+    bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const
+    {
+        std::uint64_t raw = 0;
+        if (!readPdoRaw_(motor_id, index, subindex, valueBitLength_<T>(), raw)) {
+            return false;
+        }
+        value = fromRawValue_<T>(raw);
+        return true;
+    }
+
+    template <typename T>
+    bool writeSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value)
+    {
+        return writeSdoRaw_(motor_id, index, subindex, valueBitLength_<T>(),
+                            toRawValue_(value));
+    }
+
+    template <typename T>
+    bool readSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value)
+    {
+        std::uint64_t raw = 0;
+        if (!readSdoRaw_(motor_id, index, subindex, valueBitLength_<T>(), raw)) {
+            return false;
+        }
+        value = fromRawValue_<T>(raw);
+        return true;
+    }
 
 private:
+    struct PdoEntryRuntime {
+        EthercatPdoEntryConfig cfg;
+        unsigned int offset{0};
+        bool rx{false};
+        std::uint64_t value{0};
+    };
+
+    struct SlaveRuntime {
+        config::EthercatSlaveConfig cfg;
+        ec_slave_config_t* slave_config{nullptr};
+        std::unordered_map<std::uint32_t, PdoEntryRuntime> pdo_entries;
+    };
+
+    struct BusHealthState {
+        bool initialized{false};
+        bool healthy{false};
+        unsigned int slaves_responding{0};
+        unsigned int master_al_states{0};
+        bool link_up{false};
+        int domain_result{0};
+        unsigned int working_counter{0};
+        unsigned int wc_state{0};
+    };
+
+    struct SlaveHealthState {
+        bool initialized{false};
+        bool healthy{false};
+        int result{0};
+        bool online{false};
+        bool operational{false};
+        unsigned int al_state{0};
+    };
+
+    bool configureSlave_(SlaveRuntime& slave);
+    bool configureDc_();
+    bool waitSlavesOperational_();
+    void cyclicLoop_();
+    void monitorBusHealth_();
+    void readFeedbackLocked_();
+    void writeCommandsLocked_();
+    void releaseMaster_();
+    bool hasValidPdoMapping_() const;
+    bool writePdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                      std::uint8_t bit_len, std::uint64_t value);
+    bool readPdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                     std::uint8_t bit_len, std::uint64_t& value) const;
+    bool writeSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                      std::uint8_t bit_len, std::uint64_t value);
+    bool readSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                     std::uint8_t bit_len, std::uint64_t& value);
+
+    template <typename T>
+    static constexpr std::uint8_t valueBitLength_()
+    {
+        using ValueType = std::remove_cv_t<T>;
+        static_assert(std::is_integral_v<ValueType>, "EtherCAT object values must be integral");
+        static_assert(!std::is_same_v<ValueType, bool>, "bool is not a valid EtherCAT object value");
+        static_assert(sizeof(ValueType) == 1 || sizeof(ValueType) == 2 || sizeof(ValueType) == 4,
+                      "only 8/16/32-bit EtherCAT object values are supported");
+        return static_cast<std::uint8_t>(sizeof(ValueType) * 8);
+    }
+
+    template <typename T>
+    static std::uint64_t toRawValue_(T value)
+    {
+        using ValueType = std::remove_cv_t<T>;
+        using UnsignedType = std::make_unsigned_t<ValueType>;
+        return static_cast<std::uint64_t>(static_cast<UnsignedType>(value));
+    }
+
+    template <typename T>
+    static T fromRawValue_(std::uint64_t raw)
+    {
+        using ValueType = std::remove_cv_t<T>;
+        using UnsignedType = std::make_unsigned_t<ValueType>;
+        const auto unsigned_value = static_cast<UnsignedType>(raw);
+        ValueType value{};
+        std::memcpy(&value, &unsigned_value, sizeof(ValueType));
+        return value;
+    }
+
+    static std::uint32_t pdoEntryKey_(std::uint16_t index, std::uint8_t subindex);
+    static std::string hexIndex_(std::uint32_t index);
+    static std::uint64_t maskValue_(std::uint64_t value, std::uint8_t bit_len);
+    static bool isSupportedBitLength_(std::uint8_t bit_len);
+    static std::uint64_t readEntryValue_(const std::uint8_t* domain_data,
+                                         const PdoEntryRuntime& entry);
+    static void writeEntryValue_(std::uint8_t* domain_data,
+                                 const PdoEntryRuntime& entry);
+    static std::uint64_t steadyTimeNs_();
+    static std::uint64_t timePointNs_(std::chrono::steady_clock::time_point time_point);
+    static std::uint32_t usToNs_(std::uint32_t value_us);
+    static std::int32_t usToNs_(std::int32_t value_us);
+
     std::string id_;
     config::EtherCATConfig config_;
-    std::unordered_map<int, const config::EthercatSlaveConfig*> slaves_by_motor_id_;
+    EthercatPdoMapping pdo_mapping_;
+    std::unordered_map<int, SlaveRuntime> slaves_by_motor_id_;
+
+    ec_master_t* master_{nullptr};
+    ec_domain_t* domain_{nullptr};
+    std::uint8_t* domain_data_{nullptr};
+
+    mutable std::mutex data_mutex_;
+    std::thread cyclic_thread_;
+    std::atomic<bool> running_{false};
+    std::atomic<bool> healthy_{false};
+    std::atomic<bool> health_monitor_enabled_{false};
+    std::atomic<std::uint64_t> command_generation_{0};
+    std::atomic<std::uint64_t> sent_command_generation_{0};
+    BusHealthState last_bus_health_;
+    std::unordered_map<int, SlaveHealthState> last_slave_health_;
+    bool initialized_{false};
     bool started_{false};
 };
 

cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h

@@ -0,0 +1,35 @@
+#ifndef CMVR_ES_ETHERCAT_PDO_MAPPING_H
+#define CMVR_ES_ETHERCAT_PDO_MAPPING_H
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace cmvr::device {
+
+struct EthercatPdoEntryConfig {
+    std::uint16_t index{0};
+    std::uint8_t subindex{0};
+    std::uint8_t bit_len{0};
+    std::string name;
+    bool padding{false};
+};
+
+struct EthercatPdoConfig {
+    std::uint16_t index{0};
+    std::uint8_t sync_manager{0};
+    bool rx{false};
+    std::vector<EthercatPdoEntryConfig> entries;
+};
+
+struct EthercatPdoMapping {
+    std::uint32_t vendor_id{0};
+    std::uint32_t product_code{0};
+    std::string name;
+    std::vector<EthercatPdoConfig> rx_pdos;
+    std::vector<EthercatPdoConfig> tx_pdos;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_ETHERCAT_PDO_MAPPING_H

cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp

@@ -0,0 +1,128 @@
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <string>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
+
+namespace cmvr::device {
+namespace {
+
+
+
+config::MotorGroupConfig createSingleSlaveGroup()
+{
+    config::MotorGroupConfig group;
+    group.set_id("ethercat_real_test");
+    group.set_bus_type(config::MOTOR_BUS_ETHERCAT);
+    group.set_vendor(config::MOTOR_VENDOR_EYOU);
+    group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402);
+
+    auto* ethercat = group.mutable_ethercat();
+    ethercat->set_master_index(0);
+    ethercat->set_cycle_us(1000);
+    ethercat->set_slave_op_timeout_ms(12000);
+    ethercat->set_slave_state_poll_period_ms(10);
+
+    auto* dc = ethercat->mutable_dc();
+    dc->set_enable(true);
+    dc->set_reference_motor_id(1);
+    dc->set_sync0_cycle_us(1000);
+    dc->set_sync0_shift_us(0);
+    dc->set_sync_reference_clock_period(1);
+    dc->set_assign_activate(768);
+    dc->set_sync_monitor_period_ms(1000);
+
+    auto* slave = ethercat->add_slaves();
+    slave->set_motor_id(1);
+    slave->set_alias(0);
+    slave->set_position(0);
+
+    return group;
+}
+
+} // namespace
+
+TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
+{
+
+
+    EthercatMotorBusRuntime runtime;
+    runtime.setPdoMapping(createEyouCia402PdoMapping());
+
+    ASSERT_TRUE(runtime.init(createSingleSlaveGroup()));
+    EXPECT_EQ(runtime.busType(), config::MOTOR_BUS_ETHERCAT);
+    EXPECT_TRUE(runtime.hasMotor(1));
+    EXPECT_NE(runtime.slaveForMotor(1), nullptr);
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_CONTROL_WORD_6040, 0x00));
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_STATUS_WORD_6041, 0x00));
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_OPERATION_MODE_6060, 0x00));
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00));
+
+    const bool started = runtime.start();
+    EXPECT_TRUE(started);
+    if (!started) {
+        runtime.stop();
+        return;
+    }
+
+    const std::array command_writes{
+        EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
+            1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000),
+        EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
+            1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0),
+    };
+    auto invalid_writes = command_writes;
+    invalid_writes[1].bit_length = 16;
+
+    const auto generation_before = runtime.commandGeneration();
+    EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size()));
+    EXPECT_EQ(runtime.commandGeneration(), generation_before);
+    EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size()));
+    EXPECT_EQ(runtime.commandGeneration(), generation_before + 1);
+
+    const int settle_ms = 1000;
+    std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms));
+    EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1);
+    EXPECT_TRUE(runtime.isHealthy());
+
+    std::array feedback_reads{
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            1, msgs::CIA402_STATUS_WORD_6041, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
+            1, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
+    };
+    auto invalid_feedback_reads = feedback_reads;
+    invalid_feedback_reads[1].bit_length = 16;
+    EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(),
+                                        invalid_feedback_reads.size()));
+    ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size()));
+
+    const auto statusword =
+        EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(feedback_reads[0]);
+    const auto mode_display =
+        EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(feedback_reads[1]);
+
+    std::cout << "CIA402 statusword: 0x" << std::hex << statusword
+              << ", mode display: " << std::dec << static_cast<int>(mode_display)
+              << std::endl;
+
+    const int hold_ms = 10000;
+    if (hold_ms > 0) {
+        std::cout << "Holding EtherCAT runtime for " << hold_ms
+                  << " ms. Check slave state in another terminal." << std::endl;
+        std::this_thread::sleep_for(std::chrono::milliseconds(hold_ms));
+    }
+
+    runtime.stop();
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt

@@ -0,0 +1,50 @@
+add_library(ethercat_motor_driver SHARED
+        src/cia402/cia402_protocol.cpp
+        src/cia402/cia402_status_monitor.cpp
+        src/vendor/eyou/eyou_motor.cpp
+        src/vendor/eyou/eyou_motor_adapter.cpp
+)
+
+target_include_directories(ethercat_motor_driver
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+target_link_libraries(ethercat_motor_driver
+        PUBLIC
+        cmvr_es::device::motor_core
+        cmvr_es::device::motor_bus_runtime
+        PRIVATE
+        cmvr_es::proto
+        glog
+)
+
+add_library(cmvr_es::device::ethercat_motor_driver ALIAS ethercat_motor_driver)
+install(TARGETS ethercat_motor_driver LIBRARY DESTINATION lib)
+
+add_executable(eyou_motor_real_test
+        src/vendor/eyou/eyou_motor_real_test.cpp
+)
+
+target_link_libraries(eyou_motor_real_test
+        PRIVATE
+        cmvr_es::device::ethercat_motor_driver
+        gtest
+        gtest_main
+        pthread
+        glog
+)
+
+add_executable(eyou_motor_device_manager_real_test
+        src/vendor/eyou/eyou_motor_device_manager_real_test.cpp
+)
+
+target_link_libraries(eyou_motor_device_manager_real_test
+        PRIVATE
+        cmvr_es::device_manager
+        cmvr_es::device::motor_manager
+        gtest
+        gtest_main
+        pthread
+        glog
+)

cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h

@@ -0,0 +1,166 @@
+#ifndef CMVR_ES_CIA402_OBJECTS_H
+#define CMVR_ES_CIA402_OBJECTS_H
+
+#include <cstdint>
+
+namespace cmvr::device::cia402 {
+
+union Controlword {
+    std::uint16_t value;
+    struct {
+        std::uint16_t switch_on : 1;
+        std::uint16_t enable_voltage : 1;
+        std::uint16_t quick_stop : 1;
+        std::uint16_t enable_operation : 1;
+        std::uint16_t new_set_point : 1;
+        std::uint16_t change_set_immediately : 1;
+        std::uint16_t relative : 1;
+        std::uint16_t fault_reset : 1;
+        std::uint16_t halt : 1;
+        std::uint16_t reserved : 2;
+        std::uint16_t manufacturer_specific : 5;
+    };
+};
+
+union Statusword {
+    std::uint16_t value;
+    struct {
+        std::uint16_t ready_to_switch_on : 1;
+        std::uint16_t switched_on : 1;
+        std::uint16_t operation_enabled : 1;
+        std::uint16_t fault : 1;
+        std::uint16_t voltage_enabled : 1;
+        std::uint16_t quick_stop : 1;
+        std::uint16_t switch_on_disabled : 1;
+        std::uint16_t warning : 1;
+        std::uint16_t manufacturer_specific_8 : 1;
+        std::uint16_t remote : 1;
+        std::uint16_t target_reached : 1;
+        std::uint16_t internal_limit_active : 1;
+        std::uint16_t operation_mode_specific : 2;
+        std::uint16_t manufacturer_specific : 2;
+    };
+};
+
+static_assert(sizeof(Controlword) == sizeof(std::uint16_t));
+static_assert(sizeof(Statusword) == sizeof(std::uint16_t));
+
+enum class DeviceState {
+    SwitchOnDisabled,
+    ReadyToSwitchOn,
+    SwitchedOn,
+    OperationEnabled,
+};
+
+namespace detail {
+
+struct StateRule {
+    std::uint16_t relevant_bits;
+    std::uint16_t expected_bits;
+};
+
+inline StateRule stateRule(const DeviceState state)
+{
+    // CiA402 device states are matched by selected 0x6041 statusword bits.
+    switch (state) {
+        case DeviceState::SwitchOnDisabled:
+            return {0x004F, 0x0040};
+        case DeviceState::ReadyToSwitchOn:
+            return {0x006F, 0x0021};
+        case DeviceState::SwitchedOn:
+            return {0x006F, 0x0023};
+        case DeviceState::OperationEnabled:
+            return {0x006F, 0x0027};
+    }
+    return {0x006F, 0x0000};
+}
+
+} // namespace detail
+
+inline Controlword controlword(const std::uint16_t value)
+{
+    Controlword cw{};
+    cw.value = value;
+    return cw;
+}
+
+inline Statusword statusword(const std::uint16_t value)
+{
+    Statusword sw{};
+    sw.value = value;
+    return sw;
+}
+
+inline Controlword shutdownControlword()
+{
+    Controlword cw{};
+    cw.quick_stop = 1;
+    cw.enable_voltage = 1;
+    return cw;
+}
+
+inline Controlword switchOnControlword()
+{
+    auto cw = shutdownControlword();
+    cw.switch_on = 1;
+    return cw;
+}
+
+inline Controlword enableOperationControlword()
+{
+    auto cw = switchOnControlword();
+    cw.enable_operation = 1;
+    return cw;
+}
+
+inline Controlword quickStopControlword()
+{
+    auto cw = enableOperationControlword();
+    cw.quick_stop = 0;
+    return cw;
+}
+
+inline Controlword faultResetControlword()
+{
+    Controlword cw{};
+    cw.fault_reset = 1;
+    return cw;
+}
+
+inline Controlword profilePositionControlword(const bool new_set_point)
+{
+    auto cw = enableOperationControlword();
+    cw.change_set_immediately = 1;
+    cw.new_set_point = new_set_point ? 1 : 0;
+    return cw;
+}
+
+inline bool hasState(const Statusword status, const DeviceState state)
+{
+    const auto rule = detail::stateRule(state);
+    return (status.value & rule.relevant_bits) == rule.expected_bits;
+}
+
+inline bool isSwitchOnDisabled(const Statusword status)
+{
+    return hasState(status, DeviceState::SwitchOnDisabled);
+}
+
+inline bool isOperationEnabled(const Statusword status)
+{
+    return hasState(status, DeviceState::OperationEnabled);
+}
+
+inline bool targetReached(const Statusword status)
+{
+    return status.target_reached != 0;
+}
+
+inline bool setPointAcknowledged(const Statusword status)
+{
+    return (status.value & (1U << 12U)) != 0;
+}
+
+} // namespace cmvr::device::cia402
+
+#endif // CMVR_ES_CIA402_OBJECTS_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h

@@ -0,0 +1,149 @@
+#ifndef CMVR_ES_CIA402_PROTOCOL_H
+#define CMVR_ES_CIA402_PROTOCOL_H
+
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <unordered_map>
+#include <vector>
+
+#include "cmvr/config/motor_config/motor_config.pb.h"
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
+#include "devices/motor/motor_protocol_interface.h"
+
+namespace cmvr::device {
+
+class Cia402StatusMonitor;
+
+class Cia402Protocol final : public MotorProtocolInterface {
+public:
+    struct CyclicPositionCommand {
+        std::uint8_t node_id{0};
+        double target_q{0.0};
+        double target_qd{0.0};
+    };
+
+    struct MotorFeedback {
+        std::uint8_t node_id{0};
+        double q{0.0};
+        double qd{0.0};
+    };
+
+    explicit Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
+                            const config::Cia402ProtocolConfig& config);
+    ~Cia402Protocol() override;
+
+    bool initNode(std::uint8_t node_id) override;
+
+    bool setMode(std::uint8_t node_id, msgs::RunMode mode) override;
+    msgs::RunMode getMode(std::uint8_t node_id) override;
+    void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override;
+    void setLimitQd(std::uint8_t node_id, double qd) override;
+    void setLimitQ(std::uint8_t node_id, double ub, double lb) override;
+    bool calibrateZeroQ(std::uint8_t node_id) override;
+    bool reachedTargetQ(std::uint8_t node_id) override;
+    bool commandProfilePosition(std::uint8_t node_id,
+                                double target_q,
+                                double max_qd,
+                                double max_qdd) override;
+    bool commandProfileVelocity(std::uint8_t node_id,
+                                double target_qd,
+                                double max_qdd) override;
+    bool commandCyclicPosition(std::uint8_t node_id,
+                               double target_q,
+                               double target_qd) override;
+    bool commandCyclicPositionsAtomic(const CyclicPositionCommand* commands,
+                                      std::size_t count);
+    bool readFeedbacksAtomic(MotorFeedback* feedbacks, std::size_t count) const;
+    bool commandCyclicVelocity(std::uint8_t node_id,
+                               double target_qd) override;
+    bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override;
+    void setMotorConversion(std::uint8_t node_id,
+                            double encoder_counts_per_rev,
+                            double gear_ratio) override;
+    bool torqueOn(std::uint8_t node_id) override;
+    bool torqueOff(std::uint8_t node_id) override;
+    bool brakeRelease(std::uint8_t node_id) override;
+    bool quickStop(std::uint8_t node_id) override;
+
+    double getQ(std::uint8_t node_id) override;
+    double getQd(std::uint8_t node_id) override;
+    bool syncTargetToActualPosition(std::uint8_t node_id);
+
+private:
+    struct NodeState {
+        msgs::RunMode mode{msgs::RUN_MODE_CYCLIC_SYNC_POSITION};
+        cia402::Controlword controlword{};
+        std::int32_t target_position{0};
+        std::int32_t target_velocity{0};
+        std::int16_t target_torque{0};
+        std::int32_t profile_velocity{0};
+        std::int32_t profile_acceleration{0};
+        std::int32_t profile_deceleration{0};
+        double limit_q_lb{0.0};
+        double limit_q_ub{0.0};
+        double limit_qd{0.0};
+        double limit_qdd{0.0};
+        double encoder_counts_per_rev{0.0};
+        double gear_ratio{0.0};
+    };
+
+    static std::int8_t toCia402Mode_(msgs::RunMode mode);
+    static msgs::RunMode fromCia402Mode_(std::int8_t mode);
+    static cia402::Controlword nextControlword_(cia402::Statusword statusword);
+    static bool isOperationEnabled_(cia402::Statusword statusword);
+    static bool targetReached_(cia402::Statusword statusword);
+
+    std::int32_t radToCounts_(double angle_rad, const NodeState& state) const;
+    double countsToRad_(std::int32_t counts, const NodeState& state) const;
+    std::int32_t radPerSecToCounts_(double velocity_rad_s, const NodeState& state) const;
+    std::int32_t radPerSec2ToCounts_(double acceleration_rad_s2, const NodeState& state) const;
+    double countsToRadPerSec_(std::int32_t velocity_counts_s, const NodeState& state) const;
+    NodeState& nodeState_(std::uint8_t node_id);
+    const NodeState* findNodeState_(std::uint8_t node_id) const;
+    bool hasValidConversion_(std::uint8_t node_id, const NodeState& state) const;
+    bool validateNodePdos_(std::uint8_t node_id) const;
+    bool readStatusword_(std::uint8_t node_id, std::uint16_t& statusword) const;
+    bool readActualPosition_(std::uint8_t node_id, std::int32_t& actual_position) const;
+    bool readActualVelocity_(std::uint8_t node_id, std::int32_t& actual_velocity) const;
+    bool readModeDisplay_(std::uint8_t node_id, std::int8_t& mode_display) const;
+    bool writeControlword_(std::uint8_t node_id, cia402::Controlword controlword);
+    bool writeControlwordAndWait_(std::uint8_t node_id,
+                                  cia402::Controlword controlword,
+                                  cia402::DeviceState target_state,
+                                  const char* state_name);
+    bool waitStatus_(std::uint8_t node_id,
+                     cia402::DeviceState target_state,
+                     const char* state_name) const;
+    bool waitMode_(std::uint8_t node_id, std::int8_t target_mode) const;
+    bool waitSetPointAcknowledged_(std::uint8_t node_id, bool acknowledged) const;
+    bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_name) const;
+    bool writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
+    bool writeVelocityLimitToDictionary_(std::uint8_t node_id, const NodeState& state) const;
+    bool writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
+    bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state);
+    bool writeTargetsForMode_(std::uint8_t node_id,
+                              msgs::RunMode mode,
+                              const NodeState& state) const;
+    bool appendTargetWritesForMode_(
+        std::uint8_t node_id,
+        msgs::RunMode mode,
+        const NodeState& state,
+        EthercatMotorBusRuntime::PdoWrite* writes,
+        std::size_t capacity,
+        std::size_t& count) const;
+    bool writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state);
+    bool writeNode_(std::uint8_t node_id, NodeState& state);
+
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
+    std::unique_ptr<Cia402StatusMonitor> status_monitor_;
+    config::Cia402ProtocolConfig config_;
+    std::unordered_map<std::uint8_t, NodeState> nodes_;
+    std::mutex cyclic_position_mutex_;
+    std::vector<EthercatMotorBusRuntime::PdoWrite> cyclic_position_writes_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CIA402_PROTOCOL_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h

@@ -0,0 +1,78 @@
+#ifndef CMVR_ES_CIA402_STATUS_MONITOR_H
+#define CMVR_ES_CIA402_STATUS_MONITOR_H
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <unordered_map>
+
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+
+namespace cmvr::device {
+
+class Cia402StatusMonitor final {
+public:
+    Cia402StatusMonitor(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
+                        std::chrono::milliseconds poll_period);
+    ~Cia402StatusMonitor();
+
+    Cia402StatusMonitor(const Cia402StatusMonitor&) = delete;
+    Cia402StatusMonitor& operator=(const Cia402StatusMonitor&) = delete;
+
+    void addNode(std::uint8_t node_id);
+    void setExpectedOperationEnabled(std::uint8_t node_id, bool expected);
+    bool isNodeOperational(std::uint8_t node_id) const;
+
+private:
+    struct StatusSample {
+        bool read_ok{false};
+        bool transport_healthy{false};
+        bool expected_operation_enabled{false};
+        bool operation_enabled{false};
+        bool status_problem{false};
+        bool command_blocked{false};
+        std::uint16_t statusword{0};
+        std::uint16_t error_code{0};
+        std::int8_t mode_display{0};
+        std::int32_t actual_position{0};
+        std::int32_t actual_velocity{0};
+        std::int16_t actual_torque{0};
+    };
+
+    struct NodeMonitorState {
+        bool expected_operation_enabled{false};
+        bool has_last_sample{false};
+        StatusSample last_sample;
+    };
+
+    void monitorLoop_();
+    void monitorNode_(std::uint8_t node_id, bool expected_operation_enabled);
+    bool readStatusSample_(std::uint8_t node_id,
+                           bool expected_operation_enabled,
+                           StatusSample& sample) const;
+    void reportErrorCodeTransition_(std::uint8_t node_id,
+                                    bool had_previous,
+                                    const StatusSample& previous,
+                                    const StatusSample& current) const;
+    void reportStatuswordTransition_(std::uint8_t node_id,
+                                     bool had_previous,
+                                     const StatusSample& previous,
+                                     const StatusSample& current) const;
+    static const char* deviceStateName_(std::uint16_t statusword);
+    static const char* errorCodeDescription_(std::uint16_t error_code);
+
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
+    std::chrono::milliseconds poll_period_;
+    std::mutex monitor_mutex_;
+    mutable std::mutex states_mutex_;
+    std::unordered_map<std::uint8_t, NodeMonitorState> states_;
+    std::atomic<bool> running_{true};
+    std::thread monitor_thread_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CIA402_STATUS_MONITOR_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h

@@ -0,0 +1,81 @@
+#ifndef CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
+#define CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
+
+#include <cstdint>
+#include <string>
+#include <utility>
+
+#include "cmvr/msgs/canopen.pb.h"
+#include "cmvr/msgs/cia402.pb.h"
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h"
+
+namespace cmvr::device {
+
+namespace eyou_cia402_pdo_mapping_detail {
+
+inline constexpr std::uint32_t VENDOR_ID = 0x00001097;
+inline constexpr std::uint32_t PRODUCT_CODE = 0x00002406;
+
+inline EthercatPdoEntryConfig entry(const std::uint16_t index,
+                                    const std::uint8_t subindex,
+                                    const std::uint8_t bit_len,
+                                    std::string name)
+{
+    EthercatPdoEntryConfig cfg;
+    cfg.index = index;
+    cfg.subindex = subindex;
+    cfg.bit_len = bit_len;
+    cfg.name = std::move(name);
+    cfg.padding = index == 0 || bit_len == 0;
+    return cfg;
+}
+
+} // namespace eyou_cia402_pdo_mapping_detail
+
+inline EthercatPdoMapping createEyouCia402PdoMapping()
+{
+    using namespace eyou_cia402_pdo_mapping_detail;
+
+    EthercatPdoConfig rx_pdo;
+    rx_pdo.index = msgs::CANOPEN_RPDO2_MAP_1601;
+    rx_pdo.sync_manager = 2;
+    rx_pdo.rx = true;
+    rx_pdo.entries = {
+        entry(msgs::CIA402_CONTROL_WORD_6040, 0x00, 16, "Control Word"),
+        entry(msgs::CIA402_TARGET_POSITION_607A, 0x00, 32, "Target Position"),
+        entry(msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, 32, "Target Velocity"),
+        entry(msgs::CIA402_TARGET_TORQUE_6071, 0x00, 16, "Target Torque"),
+        entry(msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, 32, "Profile Acceleration"),
+        entry(msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, 32, "Profile Deceleration"),
+        entry(msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, 32, "Profile Velocity"),
+        entry(msgs::CIA402_TORQUE_SLOPE_6087, 0x00, 32, "Torque Slope"),
+        entry(msgs::CIA402_OPERATION_MODE_6060, 0x00, 8, "Mode Of Operation"),
+        entry(0x0000, 0x00, 8, "Padding"),
+    };
+
+    EthercatPdoConfig tx_pdo;
+    tx_pdo.index = msgs::CANOPEN_TPDO1_MAP_1A00;
+    tx_pdo.sync_manager = 3;
+    tx_pdo.rx = false;
+    tx_pdo.entries = {
+        entry(msgs::CIA402_STATUS_WORD_6041, 0x00, 16, "Status Word"),
+        entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"),
+        entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"),
+        entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"),
+        entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"),
+        entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"),
+        entry(0x0000, 0x00, 8, "Padding"),
+    };
+
+    EthercatPdoMapping mapping;
+    mapping.vendor_id = VENDOR_ID;
+    mapping.product_code = PRODUCT_CODE;
+    mapping.name = "EYOU ServoModule ECAT V145 CiA402";
+    mapping.rx_pdos.push_back(std::move(rx_pdo));
+    mapping.tx_pdos.push_back(std::move(tx_pdo));
+    return mapping;
+}
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_EYOU_CIA402_PDO_MAPPING_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h

@@ -0,0 +1,53 @@
+#ifndef CMVR_ES_EYOU_MOTOR_H
+#define CMVR_ES_EYOU_MOTOR_H
+
+#include <cstdint>
+#include <memory>
+#include <vector>
+
+#include "cmvr/config/motor_config/motor_config.pb.h"
+#include "devices/motor/abstract_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+
+namespace cmvr::device {
+
+class EyouMotor final : public AbstractMotor {
+public:
+    EyouMotor(const config::MotorConfigItem& config,
+              std::shared_ptr<Cia402Protocol> cia402_protocol,
+              std::unique_ptr<EyouMotorAdapter> vendor_adapter);
+
+    std::string typeName() const override { return "EyouMotor"; }
+    bool init() override;
+    void setLimitQ(double ub, double lb) override;
+    void setLimitQd(double qd) override;
+    bool calibrateZeroQ() override;
+    bool brakeRelease() override;
+
+    static bool commandCyclicPositionsAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        const std::vector<double>& positions,
+        const std::vector<double>& velocities);
+    static bool readFeedbacksAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        std::vector<double>& positions,
+        std::vector<double>& velocities);
+
+private:
+    bool hasDependencies_() const;
+    bool hasValidConversion_() const;
+    bool writeVendorPositionLimits_() const;
+    bool writeVendorVelocityLimit_() const;
+    std::int32_t radToCounts_(double angle_rad) const;
+    std::uint32_t radPerSecToCounts_(double velocity_rad_s) const;
+
+    std::shared_ptr<Cia402Protocol> cia402_protocol_;
+    std::unique_ptr<EyouMotorAdapter> vendor_adapter_;
+    double encoder_counts_per_rev_{0.0};
+    double gear_ratio_{0.0};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_EYOU_MOTOR_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h

@@ -0,0 +1,34 @@
+#ifndef CMVR_ES_EYOU_MOTOR_ADAPTER_H
+#define CMVR_ES_EYOU_MOTOR_ADAPTER_H
+
+#include <cstdint>
+#include <memory>
+
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h"
+
+namespace cmvr::device {
+
+class EyouMotorAdapter final : public MotorVendorAdapter {
+public:
+    explicit EyouMotorAdapter(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime);
+    ~EyouMotorAdapter() override = default;
+
+    bool initNode(std::uint8_t node_id) override;
+    bool writePositionLimits(std::uint8_t node_id,
+                             std::int32_t lower_limit,
+                             std::int32_t upper_limit) override;
+    bool writeVelocityLimit(std::uint8_t node_id,
+                            std::uint32_t velocity_limit) override;
+    bool calibrateZero(std::uint8_t node_id,
+                       std::int64_t counts_per_joint_revolution,
+                       std::int32_t& zeroed_position) override;
+    bool brakeRelease(std::uint8_t node_id) override;
+
+private:
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_EYOU_MOTOR_ADAPTER_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h

@@ -0,0 +1,20 @@
+#ifndef CMVR_ES_EYOU_OBJECTS_H
+#define CMVR_ES_EYOU_OBJECTS_H
+
+#include <cstdint>
+
+namespace cmvr::device::eyou {
+
+inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003;
+
+inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014;
+
+inline constexpr std::uint16_t EYOU_OVER_SPEED_THRESHOLD_2024 = 0x2024;
+inline constexpr std::uint16_t EYOU_FIRST_ENCODER_VALUE_202A = 0x202A;
+inline constexpr std::uint16_t EYOU_SECOND_ENCODER_VALUE_202B = 0x202B;
+
+inline constexpr std::uint16_t EYOU_STORE_PARAMETERS_1010 = 0x1010;
+
+} // namespace cmvr::device::eyou
+
+#endif // CMVR_ES_EYOU_OBJECTS_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h

@@ -0,0 +1,26 @@
+#ifndef CMVR_ES_MOTOR_VENDOR_ADAPTER_H
+#define CMVR_ES_MOTOR_VENDOR_ADAPTER_H
+
+#include <cstdint>
+
+namespace cmvr::device {
+
+class MotorVendorAdapter {
+public:
+    virtual ~MotorVendorAdapter() = default;
+
+    virtual bool initNode(std::uint8_t node_id) = 0;
+    virtual bool writePositionLimits(std::uint8_t node_id,
+                                     std::int32_t lower_limit,
+                                     std::int32_t upper_limit) = 0;
+    virtual bool writeVelocityLimit(std::uint8_t node_id,
+                                    std::uint32_t velocity_limit) = 0;
+    virtual bool calibrateZero(std::uint8_t node_id,
+                               std::int64_t counts_per_joint_revolution,
+                               std::int32_t& zeroed_position) = 0;
+    virtual bool brakeRelease(std::uint8_t node_id) = 0;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_MOTOR_VENDOR_ADAPTER_H

cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp

@@ -0,0 +1,298 @@
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h"
+
+#include <algorithm>
+#include <array>
+#include <iomanip>
+#include <utility>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "common/base/logging/logger.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
+
+namespace cmvr::device {
+
+Cia402StatusMonitor::Cia402StatusMonitor(
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
+    const std::chrono::milliseconds poll_period)
+    : bus_runtime_(std::move(bus_runtime)),
+      poll_period_(std::max(poll_period, std::chrono::milliseconds{1})),
+      monitor_thread_(&Cia402StatusMonitor::monitorLoop_, this)
+{
+}
+
+Cia402StatusMonitor::~Cia402StatusMonitor()
+{
+    running_.store(false);
+    if (monitor_thread_.joinable()) {
+        monitor_thread_.join();
+    }
+}
+
+void Cia402StatusMonitor::addNode(const std::uint8_t node_id)
+{
+    {
+        std::lock_guard<std::mutex> lock(states_mutex_);
+        states_.try_emplace(node_id);
+    }
+    monitorNode_(node_id, false);
+}
+
+void Cia402StatusMonitor::setExpectedOperationEnabled(
+    const std::uint8_t node_id,
+    const bool expected)
+{
+    {
+        std::lock_guard<std::mutex> lock(states_mutex_);
+        states_[node_id].expected_operation_enabled = expected;
+    }
+    monitorNode_(node_id, expected);
+}
+
+bool Cia402StatusMonitor::isNodeOperational(const std::uint8_t node_id) const
+{
+    if (!bus_runtime_ || !bus_runtime_->isHealthy()) {
+        return false;
+    }
+    std::lock_guard<std::mutex> lock(states_mutex_);
+    const auto it = states_.find(node_id);
+    return it != states_.end() && it->second.has_last_sample &&
+           it->second.last_sample.read_ok &&
+           it->second.last_sample.transport_healthy &&
+           it->second.last_sample.operation_enabled &&
+           !it->second.last_sample.command_blocked;
+}
+
+void Cia402StatusMonitor::monitorLoop_()
+{
+    while (running_.load()) {
+        std::array<std::pair<std::uint8_t, bool>, 256> nodes{};
+        std::size_t node_count = 0;
+        {
+            std::lock_guard<std::mutex> lock(states_mutex_);
+            for (const auto& [node_id, state] : states_) {
+                if (node_count >= nodes.size()) {
+                    break;
+                }
+                nodes[node_count++] = {node_id, state.expected_operation_enabled};
+            }
+        }
+        for (std::size_t i = 0; i < node_count; ++i) {
+            monitorNode_(nodes[i].first, nodes[i].second);
+        }
+        std::this_thread::sleep_for(poll_period_);
+    }
+}
+
+void Cia402StatusMonitor::monitorNode_(
+    const std::uint8_t node_id,
+    const bool expected_operation_enabled)
+{
+    std::lock_guard<std::mutex> monitor_lock(monitor_mutex_);
+    StatusSample current;
+    readStatusSample_(node_id, expected_operation_enabled, current);
+
+    StatusSample previous;
+    bool had_previous = false;
+    {
+        std::lock_guard<std::mutex> lock(states_mutex_);
+        auto& state = states_[node_id];
+        had_previous = state.has_last_sample;
+        previous = state.last_sample;
+        state.last_sample = current;
+        state.has_last_sample = true;
+    }
+
+    if (!current.transport_healthy) {
+        return;
+    }
+    if (!current.read_ok) {
+        if (!had_previous || previous.read_ok) {
+            CMVR_LOG(ERROR) << "[Cia402StatusMonitor] failed to read node status snapshot"
+                            << ", node=" << static_cast<int>(node_id);
+        }
+        return;
+    }
+    if (had_previous && !previous.read_ok) {
+        CMVR_LOG(INFO) << "[Cia402StatusMonitor] node status snapshot recovered"
+                       << ", node=" << static_cast<int>(node_id);
+    }
+
+    reportErrorCodeTransition_(node_id, had_previous, previous, current);
+    reportStatuswordTransition_(node_id, had_previous, previous, current);
+}
+
+void Cia402StatusMonitor::reportErrorCodeTransition_(
+    const std::uint8_t node_id,
+    const bool had_previous,
+    const StatusSample& previous,
+    const StatusSample& current) const
+{
+    const bool error_code_changed =
+        !had_previous || !previous.read_ok ||
+        previous.error_code != current.error_code;
+    if (current.error_code != 0 && error_code_changed) {
+        CMVR_LOG(ERROR) << "[Cia402StatusMonitor] [error code] 0x"
+                        << std::hex << std::uppercase << std::setw(4)
+                        << std::setfill('0') << current.error_code
+                        << std::dec << std::nouppercase << std::setfill(' ')
+                        << ' ' << errorCodeDescription_(current.error_code)
+                        << ", node=" << static_cast<int>(node_id);
+    } else if (current.error_code == 0 && had_previous && previous.read_ok &&
+               previous.error_code != 0) {
+        CMVR_LOG(INFO) << "[Cia402StatusMonitor] [error code] recovered"
+                       << ", node=" << static_cast<int>(node_id)
+                       << ", previous_code=0x" << std::hex << std::uppercase
+                       << std::setw(4) << std::setfill('0') << previous.error_code
+                       << std::dec << std::nouppercase << std::setfill(' ');
+    }
+}
+
+void Cia402StatusMonitor::reportStatuswordTransition_(
+    const std::uint8_t node_id,
+    const bool had_previous,
+    const StatusSample& previous,
+    const StatusSample& current) const
+{
+    const bool device_state_changed =
+        !had_previous || !previous.read_ok ||
+        (previous.statusword & 0x006F) != (current.statusword & 0x006F);
+    const bool status_changed =
+        device_state_changed ||
+        previous.status_problem != current.status_problem ||
+        (previous.statusword & 0x0888) != (current.statusword & 0x0888);
+    if (current.status_problem && status_changed) {
+        const auto status = cia402::statusword(current.statusword);
+        CMVR_LOG(WARNING) << "[Cia402StatusMonitor] [statusword] 0x"
+                          << std::hex << std::uppercase << std::setw(4)
+                          << std::setfill('0') << current.statusword
+                          << std::dec << std::nouppercase << std::setfill(' ')
+                          << ' ' << deviceStateName_(current.statusword)
+                          << ", node=" << static_cast<int>(node_id)
+                          << (status.fault != 0 ? ", fault" : "")
+                          << (status.warning != 0 ? ", warning" : "")
+                          << (status.internal_limit_active != 0
+                                  ? ", internal limit active"
+                                  : "")
+                          << (current.expected_operation_enabled &&
+                                      !current.operation_enabled
+                                  ? ", operation not enabled"
+                                  : "");
+    } else if (!current.status_problem && had_previous && previous.read_ok &&
+               previous.status_problem) {
+        CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] recovered"
+                       << ", node=" << static_cast<int>(node_id)
+                       << ", current_statusword=0x" << std::hex << std::uppercase
+                       << std::setw(4) << std::setfill('0') << current.statusword
+                       << std::dec << std::nouppercase << std::setfill(' ')
+                       << ' ' << deviceStateName_(current.statusword)
+                       << ", previous_statusword=0x" << std::hex << std::uppercase
+                       << std::setw(4) << std::setfill('0') << previous.statusword
+                       << std::dec << std::nouppercase << std::setfill(' ');
+    } else if (device_state_changed) {
+        CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] 0x"
+                       << std::hex << std::uppercase << std::setw(4)
+                       << std::setfill('0') << current.statusword
+                       << std::dec << std::nouppercase << std::setfill(' ')
+                       << ' ' << deviceStateName_(current.statusword)
+                       << ", node=" << static_cast<int>(node_id);
+    }
+}
+
+bool Cia402StatusMonitor::readStatusSample_(
+    const std::uint8_t node_id,
+    const bool expected_operation_enabled,
+    StatusSample& sample) const
+{
+    sample.expected_operation_enabled = expected_operation_enabled;
+    sample.transport_healthy = bus_runtime_ && bus_runtime_->isHealthy();
+    if (!sample.transport_healthy) {
+        return false;
+    }
+
+    std::array reads{
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            node_id, msgs::CIA402_STATUS_WORD_6041, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            node_id, msgs::CIA402_ERROR_CODE_603F, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
+            node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
+            node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
+            node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int16_t>(
+            node_id, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00),
+    };
+    if (!bus_runtime_->readPdosAtomic(reads.data(), reads.size())) {
+        return false;
+    }
+
+    sample.statusword = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[0]);
+    sample.error_code = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[1]);
+    sample.mode_display = EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(reads[2]);
+    sample.actual_position = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[3]);
+    sample.actual_velocity = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[4]);
+    sample.actual_torque = EthercatMotorBusRuntime::pdoReadValue<std::int16_t>(reads[5]);
+    sample.read_ok = true;
+
+    const auto status = cia402::statusword(sample.statusword);
+    sample.operation_enabled = cia402::isOperationEnabled(status);
+    sample.status_problem = status.fault != 0 || status.warning != 0 ||
+                            status.internal_limit_active != 0 ||
+                            (expected_operation_enabled && !sample.operation_enabled);
+    sample.command_blocked = status.fault != 0 || status.warning != 0 ||
+                             sample.error_code != 0 ||
+                             (expected_operation_enabled && !sample.operation_enabled);
+    return true;
+}
+
+const char* Cia402StatusMonitor::deviceStateName_(const std::uint16_t statusword)
+{
+    if ((statusword & 0x004F) == 0x000F) {
+        return "FaultReactionActive";
+    }
+    if ((statusword & 0x004F) == 0x0008) {
+        return "Fault";
+    }
+    if ((statusword & 0x006F) == 0x0007) {
+        return "QuickStopActive";
+    }
+    const auto status = cia402::statusword(statusword);
+    if (cia402::isOperationEnabled(status)) {
+        return "OperationEnabled";
+    }
+    if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) {
+        return "SwitchedOn";
+    }
+    if (cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) {
+        return "ReadyToSwitchOn";
+    }
+    if (cia402::isSwitchOnDisabled(status)) {
+        return "SwitchOnDisabled";
+    }
+    return "NotReadyToSwitchOn";
+}
+
+const char* Cia402StatusMonitor::errorCodeDescription_(const std::uint16_t error_code)
+{
+    switch (error_code) {
+        case 0x0000:
+            return "no error";
+        case 0x2310:
+            return "continuous over-current";
+        case 0x3210:
+            return "DC bus over-voltage";
+        case 0x3220:
+            return "DC bus under-voltage";
+        case 0x4210:
+            return "device over-temperature";
+        case 0x4310:
+            return "drive over-temperature";
+        case 0x8611:
+            return "position following error";
+        default:
+            return "unknown or vendor-specific error";
+    }
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp

@@ -0,0 +1,277 @@
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <cstdint>
+#include <functional>
+#include <mutex>
+#include <utility>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+
+EyouMotor::EyouMotor(const config::MotorConfigItem& config,
+                     std::shared_ptr<Cia402Protocol> cia402_protocol,
+                     std::unique_ptr<EyouMotorAdapter> vendor_adapter)
+    : cia402_protocol_(std::move(cia402_protocol)),
+      vendor_adapter_(std::move(vendor_adapter))
+{
+    info_.id = config.id();
+    info_.joint_name = config.joint_name();
+    info_.limit_q_lb = config.limit_q_lb();
+    info_.limit_q_ub = config.limit_q_ub();
+    info_.limit_qd = config.limit_qd();
+    info_.limit_qdd = config.limit_qdd();
+    encoder_counts_per_rev_ = config.encoder_counts_per_rev();
+    gear_ratio_ = config.gear_ratio();
+    node_id_ = static_cast<std::uint8_t>(info_.id);
+    id_ = info_.joint_name;
+    protocol_ = cia402_protocol_;
+}
+
+bool EyouMotor::init()
+{
+    std::scoped_lock lock(mtx_);
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return false;
+    }
+
+    if (!cia402_protocol_->initNode(node_id_)) {
+        CMVR_LOG(ERROR) << "[EyouMotor] failed to init CiA402 node: " << info_.joint_name;
+        return false;
+    }
+    if (!vendor_adapter_->initNode(node_id_)) {
+        CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name;
+        return false;
+    }
+    cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_);
+
+    cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
+    if (!writeVendorVelocityLimit_()) {
+        return false;
+    }
+    if (info_.limit_qdd > 0.0) {
+        cia402_protocol_->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
+    }
+    if (!writeVendorPositionLimits_()) {
+        return false;
+    }
+    return true;
+}
+
+void EyouMotor::setLimitQ(const double ub, const double lb)
+{
+    std::scoped_lock lock(mtx_);
+    info_.limit_q_ub = ub;
+    info_.limit_q_lb = lb;
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return;
+    }
+    writeVendorPositionLimits_();
+}
+
+void EyouMotor::setLimitQd(const double qd)
+{
+    std::scoped_lock lock(mtx_);
+    info_.limit_qd = qd;
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return;
+    }
+    cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
+    writeVendorVelocityLimit_();
+}
+
+bool EyouMotor::calibrateZeroQ()
+{
+    std::scoped_lock lock(mtx_);
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return false;
+    }
+
+    const auto counts_per_joint_revolution = static_cast<std::int64_t>(
+        std::llround(encoder_counts_per_rev_ * gear_ratio_));
+    std::int32_t zeroed_position = 0;
+    if (!vendor_adapter_->calibrateZero(node_id_, counts_per_joint_revolution,
+                                        zeroed_position)) {
+        CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name;
+        return false;
+    }
+    if (!cia402_protocol_->syncTargetToActualPosition(node_id_)) {
+        return false;
+    }
+    if (!writeVendorPositionLimits_()) {
+        return false;
+    }
+    return true;
+}
+
+bool EyouMotor::commandCyclicPositionsAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    const std::vector<double>& positions,
+    const std::vector<double>& velocities)
+{
+    if (motors.empty() || motors.size() != positions.size() ||
+        motors.size() != velocities.size() || motors.size() > 256) {
+        return false;
+    }
+
+    std::array<EyouMotor*, 256> lock_order{};
+    std::array<Cia402Protocol::CyclicPositionCommand, 256> commands{};
+    std::shared_ptr<Cia402Protocol> protocol;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
+        if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
+            return false;
+        }
+        if (!protocol) {
+            protocol = motor->cia402_protocol_;
+        } else if (protocol.get() != motor->cia402_protocol_.get()) {
+            CMVR_LOG(ERROR) << "[EyouMotor] batch target motors belong to different "
+                            << "CiA402 protocols";
+            return false;
+        }
+        for (std::size_t previous = 0; previous < i; ++previous) {
+            if (lock_order[previous] == motor.get()) {
+                return false;
+            }
+        }
+        lock_order[i] = motor.get();
+        commands[i] = Cia402Protocol::CyclicPositionCommand{
+            motor->node_id_, positions[i], velocities[i]};
+    }
+
+    std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
+              std::less<EyouMotor*>{});
+    std::array<std::unique_lock<std::mutex>, 256> locks{};
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
+    }
+    return protocol && protocol->commandCyclicPositionsAtomic(commands.data(), motors.size());
+}
+
+bool EyouMotor::readFeedbacksAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    std::vector<double>& positions,
+    std::vector<double>& velocities)
+{
+    if (motors.empty() || motors.size() > 256) {
+        return false;
+    }
+
+    std::array<EyouMotor*, 256> lock_order{};
+    std::array<Cia402Protocol::MotorFeedback, 256> feedbacks{};
+    std::shared_ptr<Cia402Protocol> protocol;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
+        if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
+            return false;
+        }
+        if (!protocol) {
+            protocol = motor->cia402_protocol_;
+        } else if (protocol.get() != motor->cia402_protocol_.get()) {
+            CMVR_LOG(ERROR) << "[EyouMotor] batch feedback motors belong to different "
+                            << "CiA402 protocols";
+            return false;
+        }
+        for (std::size_t previous = 0; previous < i; ++previous) {
+            if (lock_order[previous] == motor.get()) {
+                return false;
+            }
+        }
+        lock_order[i] = motor.get();
+        feedbacks[i].node_id = motor->node_id_;
+    }
+
+    std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
+              std::less<EyouMotor*>{});
+    std::array<std::unique_lock<std::mutex>, 256> locks{};
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
+    }
+    if (!protocol || !protocol->readFeedbacksAtomic(feedbacks.data(), motors.size())) {
+        return false;
+    }
+
+    positions.resize(motors.size());
+    velocities.resize(motors.size());
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        positions[i] = feedbacks[i].q;
+        velocities[i] = feedbacks[i].qd;
+    }
+    return true;
+}
+
+bool EyouMotor::brakeRelease()
+{
+    std::scoped_lock lock(mtx_);
+    if (!hasDependencies_()) {
+        return false;
+    }
+    return vendor_adapter_->brakeRelease(node_id_);
+}
+
+bool EyouMotor::hasDependencies_() const
+{
+    if (!cia402_protocol_ || !vendor_adapter_) {
+        CMVR_LOG(ERROR) << "[EyouMotor] missing protocol or vendor adapter: "
+                        << info_.joint_name;
+        return false;
+    }
+    if (cia402_protocol_->comm_proto != MotorProtocolInterface::CommProto::ETHERCAT) {
+        CMVR_LOG(ERROR) << "[EyouMotor] invalid protocol for motor: " << info_.joint_name;
+        return false;
+    }
+    return true;
+}
+
+bool EyouMotor::hasValidConversion_() const
+{
+    if (encoder_counts_per_rev_ > 0.0 && gear_ratio_ > 0.0) {
+        return true;
+    }
+    CMVR_LOG(ERROR) << "[EyouMotor] missing encoder conversion config: "
+                    << info_.joint_name
+                    << ", encoder_counts_per_rev=" << encoder_counts_per_rev_
+                    << ", gear_ratio=" << gear_ratio_;
+    return false;
+}
+
+bool EyouMotor::writeVendorPositionLimits_() const
+{
+    if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) ||
+        info_.limit_q_ub <= info_.limit_q_lb) {
+        return true;
+    }
+
+    cia402_protocol_->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
+    return vendor_adapter_->writePositionLimits(node_id_,
+                                                radToCounts_(info_.limit_q_lb),
+                                                radToCounts_(info_.limit_q_ub));
+}
+
+bool EyouMotor::writeVendorVelocityLimit_() const
+{
+    if (!std::isfinite(info_.limit_qd) || info_.limit_qd <= 0.0) {
+        return true;
+    }
+
+    return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd));
+}
+
+std::int32_t EyouMotor::radToCounts_(const double angle_rad) const
+{
+    const double rev = angle_rad / (2.0 * M_PI);
+    return static_cast<std::int32_t>(
+        std::llround(rev * gear_ratio_ * encoder_counts_per_rev_));
+}
+
+std::uint32_t EyouMotor::radPerSecToCounts_(const double velocity_rad_s) const
+{
+    const double rev_per_sec = std::abs(velocity_rad_s) / (2.0 * M_PI);
+    return static_cast<std::uint32_t>(
+        std::llround(rev_per_sec * gear_ratio_ * encoder_counts_per_rev_));
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp

@@ -0,0 +1,376 @@
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+
+#include <chrono>
+#include <cmath>
+#include <cstdlib>
+#include <limits>
+#include <thread>
+#include <utility>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "common/base/logging/logger.h"
+#include "common/math/support_functions.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h"
+
+namespace cmvr::device {
+
+EyouMotorAdapter::EyouMotorAdapter(
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime)
+    : bus_runtime_(std::move(bus_runtime))
+{
+}
+
+bool EyouMotorAdapter::initNode(const std::uint8_t node_id)
+{
+    return bus_runtime_ && bus_runtime_->hasMotor(node_id);
+}
+
+bool EyouMotorAdapter::writePositionLimits(const std::uint8_t node_id,
+                                            const std::int32_t lower_limit,
+                                            const std::int32_t upper_limit)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    const auto write_limits = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                                                     0x00, 0) &&
+               bus_runtime_->writeSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x02, upper_limit) &&
+               bus_runtime_->writeSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x01, lower_limit) &&
+               bus_runtime_->writeSdo<std::uint32_t>(
+                   node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                   0x00, 0x4C494D54);
+    };
+    const auto readback_matches = [&]() {
+        std::int32_t actual_lower = 0;
+        std::int32_t actual_upper = 0;
+        return bus_runtime_->readSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x01, actual_lower) &&
+               bus_runtime_->readSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x02, actual_upper) &&
+               actual_lower == lower_limit &&
+               actual_upper == upper_limit;
+    };
+
+    const bool ok = write_limits() && readback_matches();
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write software position "
+                        << "limits, node=" << static_cast<int>(node_id)
+                        << ", soft_limit_state=" << 0x4C494D54
+                        << ", lower=" << lower_limit
+                        << ", upper=" << upper_limit;
+    }
+    return ok;
+}
+
+bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id,
+                                           const std::uint32_t velocity_limit)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    std::uint32_t actual_velocity_limit = 0;
+    const bool ok =
+        bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
+                                              0x00, velocity_limit) &&
+        bus_runtime_->readSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
+                                             0x00, actual_velocity_limit) &&
+        actual_velocity_limit == velocity_limit;
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write over speed "
+                        << "threshold, node=" << static_cast<int>(node_id)
+                        << ", expected=" << velocity_limit
+                        << ", actual=" << actual_velocity_limit;
+    }
+    return ok;
+}
+
+bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id,
+                                      const std::int64_t counts_per_joint_revolution,
+                                      std::int32_t& zeroed_position)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    const auto& zero_config = bus_runtime_->config().zero_calibration();
+    const auto home_offset_timeout =
+        std::chrono::milliseconds{zero_config.timeout_ms()};
+    const auto home_offset_poll_period =
+        std::chrono::milliseconds{zero_config.poll_period_ms()};
+    const auto home_offset_stable_samples = zero_config.stable_sample_count();
+    const auto home_offset_position_tolerance_counts =
+        zero_config.position_tolerance_counts();
+    const auto home_offset_stable_delta_counts =
+        zero_config.stable_delta_counts();
+
+    std::uint32_t original_soft_limit_state = 0;
+    std::int32_t original_home_offset = 0;
+    std::int32_t original_position = 0;
+    if (!bus_runtime_->readSdo<std::uint32_t>(
+            node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+            0x00, original_soft_limit_state) ||
+        !bus_runtime_->readSdo<std::int32_t>(
+            node_id, msgs::CIA402_HOME_OFFSET_607C,
+            0x00, original_home_offset) ||
+        !bus_runtime_->readSdo<std::int32_t>(
+            node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+            0x00, original_position)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to snapshot calibration state, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    // EYOU applies HomeOffset additively, so clearing it exposes this raw position.
+    const auto expected_cleared_position_wide =
+        static_cast<std::int64_t>(original_position) -
+        static_cast<std::int64_t>(original_home_offset);
+    if (expected_cleared_position_wide < std::numeric_limits<std::int32_t>::min() ||
+        expected_cleared_position_wide > std::numeric_limits<std::int32_t>::max()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] cleared position would overflow, node="
+                        << static_cast<int>(node_id)
+                        << ", original_position=" << original_position
+                        << ", original_home_offset=" << original_home_offset;
+        return false;
+    }
+    const auto expected_cleared_position =
+        static_cast<std::int32_t>(expected_cleared_position_wide);
+
+    const auto write_home_offset = [&](const std::int32_t value) {
+        return bus_runtime_->writeSdo<std::int32_t>(
+            node_id, msgs::CIA402_HOME_OFFSET_607C, 0x00, value);
+    };
+    const auto save_parameters = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(
+            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
+            0x01, 0x65766173);
+    };
+    const auto wait_for_soft_limit = [&](const std::uint32_t expected_state) {
+        const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
+        do {
+            std::uint32_t actual_soft_limit_state = 0;
+            if (bus_runtime_->readSdo<std::uint32_t>(
+                    node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                    0x00, actual_soft_limit_state) &&
+                actual_soft_limit_state == expected_state) {
+                return true;
+            }
+            std::this_thread::sleep_for(home_offset_poll_period);
+        } while (std::chrono::steady_clock::now() < deadline);
+        return false;
+    };
+    const auto restore_soft_limit = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(
+                   node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                   0x00, original_soft_limit_state) &&
+               wait_for_soft_limit(original_soft_limit_state);
+    };
+    const auto wait_for_position = [&](const char* phase,
+                                       const std::int32_t expected_offset,
+                                       const std::int32_t expected_position,
+                                       std::int32_t& observed_position) {
+        const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
+        std::uint32_t stable_samples = 0;
+        bool has_previous_position = false;
+        std::int32_t previous_position = 0;
+        std::int32_t observed_offset = 0;
+
+        do {
+            const bool read_ok =
+                bus_runtime_->readSdo<std::int32_t>(
+                    node_id, msgs::CIA402_HOME_OFFSET_607C,
+                    0x00, observed_offset) &&
+                bus_runtime_->readSdo<std::int32_t>(
+                    node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+                    0x00, observed_position);
+            const bool position_stable =
+                !has_previous_position ||
+                SupportFunctions::cyclicAbsoluteDifference(
+                    observed_position, previous_position,
+                    counts_per_joint_revolution) <= home_offset_stable_delta_counts;
+            const bool sample_matches =
+                read_ok && observed_offset == expected_offset &&
+                SupportFunctions::cyclicAbsoluteDifference(
+                    observed_position, expected_position,
+                    counts_per_joint_revolution) <= home_offset_position_tolerance_counts &&
+                position_stable;
+
+            stable_samples = sample_matches ? stable_samples + 1 : 0;
+            if (stable_samples >= home_offset_stable_samples) {
+                return true;
+            }
+
+            if (read_ok) {
+                previous_position = observed_position;
+                has_previous_position = true;
+            }
+            std::this_thread::sleep_for(home_offset_poll_period);
+        } while (std::chrono::steady_clock::now() < deadline);
+
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] timed out waiting for home offset state, node="
+                        << static_cast<int>(node_id)
+                        << ", phase=" << phase
+                        << ", expected_offset=" << expected_offset
+                        << ", actual_offset=" << observed_offset
+                        << ", expected_position=" << expected_position
+                        << ", actual_position=" << observed_position
+                        << ", cyclic_position_distance="
+                        << SupportFunctions::cyclicAbsoluteDifference(
+                               observed_position, expected_position,
+                               counts_per_joint_revolution)
+                        << ", counts_per_joint_revolution="
+                        << counts_per_joint_revolution
+                        << ", stable_samples=" << stable_samples;
+        return false;
+    };
+    // Follow EYOU's required clear -> set -> save sequence during rollback too.
+    const auto rollback = [&](const char* failed_phase) {
+        const bool clear_written = write_home_offset(0);
+        std::int32_t cleared_position = 0;
+        const bool clear_applied =
+            clear_written &&
+            wait_for_position("rollback_clear_home_offset", 0,
+                              expected_cleared_position, cleared_position);
+        const bool offset_written = write_home_offset(original_home_offset);
+        std::int32_t restored_position = 0;
+        const bool offset_applied =
+            offset_written &&
+            wait_for_position("rollback_apply_home_offset", original_home_offset,
+                              original_position, restored_position);
+        const bool parameters_saved = offset_written && save_parameters();
+        const bool saved_state_confirmed =
+            parameters_saved &&
+            wait_for_position("rollback_save_home_offset", original_home_offset,
+                              original_position, restored_position);
+        const bool soft_limit_restored = restore_soft_limit();
+        const bool rollback_ok = clear_applied && offset_applied &&
+                                 saved_state_confirmed && soft_limit_restored;
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] calibration failed and original state was "
+            << (rollback_ok ? "restored" : "not fully restored")
+            << ", node=" << static_cast<int>(node_id)
+            << ", phase=" << failed_phase
+            << ", original_home_offset=" << original_home_offset
+            << ", original_soft_limit_state=" << original_soft_limit_state
+            << ", clear_applied=" << clear_applied
+            << ", offset_applied=" << offset_applied
+            << ", saved_state_confirmed=" << saved_state_confirmed
+            << ", soft_limit_restored=" << soft_limit_restored;
+        return false;
+    };
+
+    if (!bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                                               0x00, 0)) {
+        const bool soft_limit_restored = restore_soft_limit();
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position "
+                        << "limit before home offset calibration, node="
+                        << static_cast<int>(node_id)
+                        << ", soft_limit_restored=" << soft_limit_restored;
+        return false;
+    }
+    if (!wait_for_soft_limit(0)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] software position limit did not disable, node="
+                        << static_cast<int>(node_id);
+        return rollback("disable_soft_limit");
+    }
+
+    if (!write_home_offset(0)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node="
+                        << static_cast<int>(node_id);
+        return rollback("clear_home_offset");
+    }
+
+    std::int32_t actual_position = 0;
+    if (!wait_for_position("clear_home_offset", 0,
+                           expected_cleared_position, actual_position)) {
+        return rollback("wait_for_cleared_position");
+    }
+    if (actual_position == std::numeric_limits<std::int32_t>::min()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home "
+                        << "offset calibration, node=" << static_cast<int>(node_id)
+                        << ", actual_position=" << actual_position;
+        return rollback("negate_actual_position");
+    }
+
+    const auto home_offset = static_cast<std::int32_t>(-actual_position);
+    if (!write_home_offset(home_offset)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node="
+                        << static_cast<int>(node_id)
+                        << ", home_offset=" << home_offset;
+        return rollback("write_home_offset");
+    }
+
+    if (!wait_for_position("apply_home_offset", home_offset, 0,
+                           zeroed_position)) {
+        return rollback("wait_for_zero_before_save");
+    }
+
+    if (!save_parameters()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node="
+                        << static_cast<int>(node_id);
+        return rollback("save_parameters");
+    }
+
+    if (!wait_for_position("save_home_offset", home_offset, 0,
+                           zeroed_position)) {
+        return rollback("wait_for_zero_after_save");
+    }
+
+    if (!restore_soft_limit()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position "
+                        << "limit after home offset calibration, node="
+                        << static_cast<int>(node_id)
+                        << ", original_soft_limit_state=" << original_soft_limit_state;
+        return rollback("restore_soft_limit");
+    }
+
+    CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node="
+                   << static_cast<int>(node_id)
+                   << ", original_home_offset=" << original_home_offset
+                   << ", cleared_position=" << actual_position
+                   << ", home_offset=" << home_offset
+                   << ", zeroed_position=" << zeroed_position
+                   << ", soft_limit_state=" << original_soft_limit_state;
+    return true;
+}
+
+bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    if (!bus_runtime_->writeSdo<std::uint8_t>(
+            node_id, eyou::EYOU_BRAKE_CONTROL_2014,
+            0x01,
+            1)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to release brake, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    const auto deadline =
+        std::chrono::steady_clock::now() + std::chrono::milliseconds{1000};
+    do {
+        std::uint8_t brake_state = 0;
+        if (bus_runtime_->readSdo<std::uint8_t>(
+                node_id, eyou::EYOU_BRAKE_CONTROL_2014,
+                0x02, brake_state) &&
+            (brake_state == 1 || brake_state == 2)) {
+            return true;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds{10});
+    } while (std::chrono::steady_clock::now() < deadline);
+
+    CMVR_LOG(ERROR) << "[EyouMotorAdapter] brake release timeout, node="
+                    << static_cast<int>(node_id);
+    return false;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp

@@ -0,0 +1,380 @@
+#include <array>
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "common/config/config_files.h"
+#include "devices/motor/abstract_motor.h"
+#include "devices/motor/manager/include/motor_manager.h"
+#include "manager/device_manager/include/device_manager.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr const char* kMotorManagerId = "right_arm_ethercat_motors";
+constexpr const char* kMotorConfigFile =
+    "devices/motor/ethercat_motors_four_real_test.pb.txt";
+constexpr std::array<int, 4> kFourMotorIds{1, 2, 3, 4};
+constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
+constexpr std::chrono::milliseconds kStatsSamplePeriod{10};
+constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500};
+constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kRaisedCosineCoefficientRad = 0.2;
+constexpr double kFourMotorPeriodS = 2.0;
+constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
+constexpr double kMinimumPositionExcursionRad = 0.2;
+constexpr double kMaximumAbsoluteTrackingErrorRad = 0.15;
+constexpr double kMaximumRmsTrackingErrorRad = 0.08;
+constexpr double kMaximumErrorSpreadRad = 0.10;
+constexpr double kFinalPositionToleranceRad = 0.05;
+
+class DeviceManagerDestroyGuard {
+public:
+    ~DeviceManagerDestroyGuard()
+    {
+        DeviceManager::destroyInstance();
+    }
+};
+
+class MotorManagerStopGuard {
+public:
+    explicit MotorManagerStopGuard(std::shared_ptr<MotorManager> motor_manager)
+        : motor_manager_(std::move(motor_manager))
+    {
+    }
+
+    ~MotorManagerStopGuard()
+    {
+        if (motor_manager_ && !motor_manager_->stop()) {
+            std::cerr << "failed to stop motor manager during test cleanup" << std::endl;
+        }
+    }
+
+private:
+    std::shared_ptr<MotorManager> motor_manager_;
+};
+
+class MultiMotorSafetyGuard {
+public:
+    explicit MultiMotorSafetyGuard(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors)
+        : motors_(motors)
+    {
+    }
+
+    ~MultiMotorSafetyGuard()
+    {
+        if (armed_) {
+            stop();
+        }
+    }
+
+    bool stop()
+    {
+        bool all_ok = true;
+        for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
+            if (*it && !(*it)->quickStop()) {
+                all_ok = false;
+            }
+        }
+        for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
+            if (*it && !(*it)->torqueOff()) {
+                all_ok = false;
+            }
+        }
+        armed_ = !all_ok;
+        return all_ok;
+    }
+
+private:
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors_;
+    bool armed_{true};
+};
+
+struct TrackingErrorStats {
+    std::int64_t sample_count{0};
+    double sum_error{0.0};
+    double sum_error_sq{0.0};
+    double max_abs_error{0.0};
+    double sin_projection{0.0};
+    double cos_projection{0.0};
+
+    void add(const double error, const double theta)
+    {
+        ++sample_count;
+        sum_error += error;
+        sum_error_sq += error * error;
+        max_abs_error = std::max(max_abs_error, std::fabs(error));
+        sin_projection += error * std::sin(theta);
+        cos_projection += error * std::cos(theta);
+    }
+
+    double mean() const
+    {
+        return sample_count > 0 ? sum_error / static_cast<double>(sample_count) : 0.0;
+    }
+
+    double rms() const
+    {
+        return sample_count > 0
+                   ? std::sqrt(sum_error_sq / static_cast<double>(sample_count))
+                   : 0.0;
+    }
+
+    double fundamentalAmplitude() const
+    {
+        if (sample_count == 0) {
+            return 0.0;
+        }
+        const double scale = 2.0 / static_cast<double>(sample_count);
+        return scale * std::sqrt(sin_projection * sin_projection +
+                                 cos_projection * cos_projection);
+    }
+
+    double phaseRad() const
+    {
+        return std::atan2(cos_projection, sin_projection);
+    }
+};
+
+double normalizePhaseRad(double phase)
+{
+    while (phase > kPi) {
+        phase -= 2.0 * kPi;
+    }
+    while (phase < -kPi) {
+        phase += 2.0 * kPi;
+    }
+    return phase;
+}
+
+double radToDeg(const double rad)
+{
+    return rad * 180.0 / kPi;
+}
+
+config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig()
+{
+    config::DeviceManagerConfig config;
+    config.set_name("eyou_motor_device_manager_real_test");
+    config.set_version("test");
+    auto* motor_entry = config.add_devices();
+    motor_entry->set_id(kMotorManagerId);
+    motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
+    motor_entry->set_config_file(kMotorConfigFile);
+    motor_entry->set_enable(true);
+
+    return config;
+}
+
+void printMotorState(const int motor_id, const std::shared_ptr<AbstractMotor>& motor)
+{
+    ASSERT_NE(motor, nullptr);
+    std::cout << "motor_id=" << motor_id
+              << ", joint_name=" << motor->jointName()
+              << ", q=" << motor->getQ() << " rad"
+              << ", qd=" << motor->getQd() << " rad/s"
+              << std::endl;
+}
+
+} // namespace
+
+TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
+{
+    ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
+    DeviceManagerDestroyGuard guard;
+
+    auto& device_manager =
+        DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
+    auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
+    ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
+
+    for (const int motor_id : kFourMotorIds) {
+        auto motor = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
+        ASSERT_NE(motor, nullptr);
+        printMotorState(motor_id, motor);
+    }
+}
+
+TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory)
+{
+    ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
+    DeviceManagerDestroyGuard guard;
+
+    auto& device_manager =
+        DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
+    auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
+    ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
+
+    std::vector<std::shared_ptr<AbstractMotor>> motors(kFourMotorIds.size());
+    for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) {
+        const int motor_id = kFourMotorIds[i];
+        motors[i] = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
+        ASSERT_NE(motors[i], nullptr);
+        printMotorState(motor_id, motors[i]);
+    }
+    MultiMotorSafetyGuard safety_guard(motors);
+
+    std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
+    for (const auto& motor : motors) {
+        ASSERT_TRUE(motor->torqueOff());
+    }
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const int motor_id = kFourMotorIds[i];
+        std::cout << "before calibrateZeroQ: ";
+        printMotorState(motor_id, motors[i]);
+        ASSERT_TRUE(motors[i]->calibrateZeroQ());
+        std::cout << "after calibrateZeroQ: ";
+        printMotorState(motor_id, motors[i]);
+    }
+
+    for (const auto& motor : motors) {
+        ASSERT_TRUE(motor->torqueOn());
+    }
+    for (const auto& motor : motors) {
+        ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
+    }
+
+    std::vector<double> center_q;
+    std::vector<double> actual_qd;
+    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd));
+
+    const double omega = 2.0 * kPi / kFourMotorPeriodS;
+    std::cout << "command four motors in CSP, duration="
+              << kFourMotorTrajectoryDuration.count()
+              << " ms, command_period=" << kCyclicCommandPeriod.count()
+              << " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad
+              << " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad
+              << " rad, period=" << kFourMotorPeriodS
+              << " s" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto end_time = start_time + kFourMotorTrajectoryDuration;
+    auto next_command_time = start_time;
+    auto next_stats_time = start_time;
+    std::uint64_t missed_command_deadlines = 0;
+    std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
+    std::vector<double> target_q(motors.size(), 0.0);
+    std::vector<double> target_qd(motors.size(), 0.0);
+    std::vector<double> actual_q;
+    std::array<double, kFourMotorIds.size()> minimum_actual_q{};
+    std::array<double, kFourMotorIds.size()> maximum_actual_q{};
+    std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin());
+    std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin());
+    double max_error_spread_rad = 0.0;
+    double sum_error_spread_sq = 0.0;
+    std::int64_t error_spread_sample_count = 0;
+    while (true) {
+        std::this_thread::sleep_until(next_command_time);
+        const auto now = std::chrono::steady_clock::now();
+        if (now > end_time) {
+            break;
+        }
+        const double t_s = std::chrono::duration<double>(now - start_time).count();
+
+        for (std::size_t i = 0; i < motors.size(); ++i) {
+            const double theta = omega * t_s + kFourMotorPhaseRad[i];
+            target_q[i] =
+                center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta));
+            target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta);
+        }
+        ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
+
+        if (now >= next_stats_time) {
+            ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
+            double min_error = std::numeric_limits<double>::max();
+            double max_error = std::numeric_limits<double>::lowest();
+            for (std::size_t i = 0; i < motors.size(); ++i) {
+                const double theta = omega * t_s + kFourMotorPhaseRad[i];
+                const double error = actual_q[i] - target_q[i];
+                error_stats[i].add(error, theta);
+                minimum_actual_q[i] = std::min(minimum_actual_q[i], actual_q[i]);
+                maximum_actual_q[i] = std::max(maximum_actual_q[i], actual_q[i]);
+                min_error = std::min(min_error, error);
+                max_error = std::max(max_error, error);
+            }
+            const double error_spread = max_error - min_error;
+            max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
+            sum_error_spread_sq += error_spread * error_spread;
+            ++error_spread_sample_count;
+            next_stats_time = now + kStatsSamplePeriod;
+        }
+
+        next_command_time += kCyclicCommandPeriod;
+        const auto command_complete_time = std::chrono::steady_clock::now();
+        if (next_command_time <= command_complete_time) {
+            const auto skipped_periods =
+                (command_complete_time - next_command_time) / kCyclicCommandPeriod + 1;
+            missed_command_deadlines += static_cast<std::uint64_t>(skipped_periods);
+            next_command_time += skipped_periods * kCyclicCommandPeriod;
+        }
+    }
+
+    std::copy(center_q.begin(), center_q.end(), target_q.begin());
+    std::fill(target_qd.begin(), target_qd.end(), 0.0);
+    ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
+    std::this_thread::sleep_for(kHoldAfterTrajectoryDuration);
+    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
+
+    std::cout << "after four motor CSP trajectory" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        printMotorState(kFourMotorIds[i], motors[i]);
+    }
+
+    const double reference_phase = error_stats.front().phaseRad();
+    const double rms_error_spread =
+        error_spread_sample_count > 0
+            ? std::sqrt(sum_error_spread_sq / static_cast<double>(error_spread_sample_count))
+            : 0.0;
+    std::cout << "four motor CSP tracking error statistics, sample_period="
+              << kStatsSamplePeriod.count()
+              << " ms, samples=" << error_stats.front().sample_count
+              << ", missed_command_deadlines=" << missed_command_deadlines
+              << ", max_error_spread=" << max_error_spread_rad
+              << " rad, rms_error_spread=" << rms_error_spread
+              << " rad" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const double phase = error_stats[i].phaseRad();
+        const double relative_phase = normalizePhaseRad(phase - reference_phase);
+        const double relative_phase_ms = relative_phase / omega * 1000.0;
+        std::cout << "  motor_id=" << kFourMotorIds[i]
+                  << ", mean_error=" << error_stats[i].mean()
+                  << " rad, rms_error=" << error_stats[i].rms()
+                  << " rad, max_abs_error=" << error_stats[i].max_abs_error
+                  << " rad, error_fundamental_amp="
+                  << error_stats[i].fundamentalAmplitude()
+                  << " rad, error_phase=" << phase
+                  << " rad (" << radToDeg(phase)
+                  << " deg), relative_phase_to_motor1=" << relative_phase
+                  << " rad (" << radToDeg(relative_phase)
+                  << " deg, " << relative_phase_ms
+                  << " ms)" << std::endl;
+        EXPECT_GE(maximum_actual_q[i] - minimum_actual_q[i],
+                  kMinimumPositionExcursionRad)
+            << "motor_id=" << kFourMotorIds[i] << " did not complete enough motion";
+        EXPECT_LE(error_stats[i].max_abs_error,
+                  kMaximumAbsoluteTrackingErrorRad)
+            << "motor_id=" << kFourMotorIds[i] << " exceeded maximum tracking error";
+        EXPECT_LE(error_stats[i].rms(), kMaximumRmsTrackingErrorRad)
+            << "motor_id=" << kFourMotorIds[i] << " exceeded RMS tracking error";
+        EXPECT_NEAR(actual_q[i], center_q[i], kFinalPositionToleranceRad)
+            << "motor_id=" << kFourMotorIds[i] << " did not return to its start position";
+    }
+    EXPECT_LE(max_error_spread_rad, kMaximumErrorSpreadRad);
+    EXPECT_TRUE(safety_guard.stop());
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp

@@ -0,0 +1,569 @@
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <iomanip>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <thread>
+#include <utility>
+
+#include <gtest/gtest.h>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "devices/motor/abstract_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr int kMotorId = 1;
+constexpr std::chrono::milliseconds kCommandSamplePeriod{100};
+constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
+constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000};
+constexpr double kDefaultGearRatio = 101.0;
+constexpr double kEncoderCountsPerMotorRev = 65536.0;
+constexpr double kPi = 3.14159265358979323846;
+
+config::MotorGroupConfig createSingleSlaveGroup()
+{
+    config::MotorGroupConfig group;
+    group.set_id("eyou_motor_real_test");
+    group.set_bus_type(config::MOTOR_BUS_ETHERCAT);
+    group.set_vendor(config::MOTOR_VENDOR_EYOU);
+    group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402);
+
+    auto* ethercat = group.mutable_ethercat();
+    ethercat->set_master_index(0);
+    ethercat->set_cycle_us(1000);
+    ethercat->set_slave_op_timeout_ms(12000);
+    ethercat->set_slave_state_poll_period_ms(10);
+
+    auto* cia402 = ethercat->mutable_cia402();
+    cia402->set_state_transition_timeout_ms(1200);
+    cia402->set_velocity_stop_timeout_ms(2000);
+    cia402->set_status_poll_period_ms(10);
+    cia402->set_stopped_velocity_tolerance_rad_s(0.001);
+
+    auto* zero_calibration = ethercat->mutable_zero_calibration();
+    zero_calibration->set_timeout_ms(2000);
+    zero_calibration->set_poll_period_ms(10);
+    zero_calibration->set_stable_sample_count(5);
+    zero_calibration->set_position_tolerance_counts(10000);
+    zero_calibration->set_stable_delta_counts(1000);
+
+    auto* dc = ethercat->mutable_dc();
+    dc->set_enable(false);
+    dc->set_reference_motor_id(kMotorId);
+    dc->set_sync0_cycle_us(1000);
+    dc->set_sync0_shift_us(0);
+    dc->set_sync_reference_clock_period(1);
+    dc->set_assign_activate(768);
+    dc->set_sync_monitor_period_ms(1000);
+
+    auto* slave = ethercat->add_slaves();
+    slave->set_motor_id(kMotorId);
+    slave->set_alias(0);
+    slave->set_position(0);
+
+    return group;
+}
+
+class RuntimeStopGuard {
+public:
+    explicit RuntimeStopGuard(std::shared_ptr<EthercatMotorBusRuntime> runtime)
+        : runtime_(std::move(runtime))
+    {
+    }
+
+    ~RuntimeStopGuard()
+    {
+        if (runtime_) {
+            runtime_->stop();
+        }
+    }
+
+private:
+    std::shared_ptr<EthercatMotorBusRuntime> runtime_;
+};
+
+std::shared_ptr<EthercatMotorBusRuntime> startRuntime()
+{
+    auto runtime = std::make_shared<EthercatMotorBusRuntime>();
+    runtime->setPdoMapping(createEyouCia402PdoMapping());
+    if (!runtime->init(createSingleSlaveGroup())) {
+        return nullptr;
+    }
+    if (!runtime->start()) {
+        runtime->stop();
+        return nullptr;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    return runtime;
+}
+
+std::shared_ptr<Cia402Protocol> createProtocol(
+    const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
+{
+    return std::make_shared<Cia402Protocol>(runtime, runtime->config().cia402());
+}
+
+config::MotorConfigItem createMotorConfig()
+{
+    config::MotorConfigItem config;
+    config.set_id(kMotorId);
+    config.set_joint_name("ethercat_test_joint");
+    config.set_limit_q_lb(-36.14);
+    config.set_limit_q_ub(36.14);
+    config.set_limit_qd(10.0);
+    config.set_limit_qdd(100.0);
+    config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
+    config.set_gear_ratio(kDefaultGearRatio);
+    return config;
+}
+
+std::unique_ptr<AbstractMotor> createMotor(
+    const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
+{
+    auto motor = std::make_unique<EyouMotor>(
+        createMotorConfig(),
+        createProtocol(runtime),
+        std::make_unique<EyouMotorAdapter>(runtime));
+    if (!motor->init()) {
+        return nullptr;
+    }
+    return motor;
+}
+
+void printMotorState(const char* label, AbstractMotor& motor)
+{
+    std::cout << label
+              << ": motor_q=" << motor.getQ() << " rad"
+              << ", motor_qd=" << motor.getQd() << " rad/s"
+              << std::endl;
+}
+
+std::string hex16(const std::uint16_t value)
+{
+    std::ostringstream oss;
+    oss << "0x" << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
+        << value;
+    return oss.str();
+}
+
+void printRawEthercatFeedback(const char* label,
+                              const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
+{
+    std::uint16_t statusword = 0;
+    std::int8_t mode_display = 0;
+    std::int32_t actual_position = 0;
+    std::int32_t actual_velocity = 0;
+    std::int16_t actual_torque = 0;
+    std::uint16_t error_code = 0;
+
+    runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword);
+    runtime->readPdo<std::int8_t>(kMotorId, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display);
+    runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_POSITION_6064, 0x00,
+                                   actual_position);
+    runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00,
+                                   actual_velocity);
+    runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00,
+                                   actual_torque);
+    runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code);
+
+    std::cout << label
+              << ": statusword=" << hex16(statusword)
+              << ", mode_display=" << static_cast<int>(mode_display)
+              << ", actual_position=" << actual_position
+              << ", actual_velocity=" << actual_velocity
+              << ", actual_torque=" << actual_torque
+              << ", error_code=" << hex16(error_code)
+              << std::endl;
+}
+
+void sampleMotorState(AbstractMotor& motor,
+                      const std::chrono::milliseconds duration)
+{
+    for (auto elapsed = std::chrono::milliseconds{0};
+         elapsed < duration;
+         elapsed += kCommandSamplePeriod) {
+        std::this_thread::sleep_for(kCommandSamplePeriod);
+        std::cout << "t=" << (elapsed + kCommandSamplePeriod).count() << " ms";
+        printMotorState("", motor);
+    }
+}
+
+double nearbySafeTarget(const double current_q, const double delta_rad)
+{
+    return current_q + (current_q > 0.0 ? -std::abs(delta_rad) : std::abs(delta_rad));
+}
+
+} // namespace
+
+TEST(EyouMotorRealTest, ReadMotorStateOnly)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOn());
+    printMotorState("motor state", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    for (int i = 1; i <= 10; ++i) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        printMotorState("motor state", *motor);
+        printRawEthercatFeedback("raw feedback", runtime);
+    }
+}
+
+TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    printMotorState("before calibrateZeroQ", *motor);
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    printMotorState("after calibrateZeroQ", *motor);
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
+    ASSERT_TRUE(motor->commandProfilePosition(1.5,0.8,3.0));
+    sampleMotorState(*motor, kFeedbackSampleDuration);
+}
+
+TEST(EyouMotorRealTest, CommandProfilePosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
+
+    ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
+    sampleMotorState(*motor, kFeedbackSampleDuration);
+}
+
+TEST(EyouMotorRealTest, CommandProfileVelocity)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+
+
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
+
+    std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
+    ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
+    sampleMotorState(*motor, kFeedbackSampleDuration);
+
+    std::cout << "motor.commandProfileVelocity(0 rad/s, 1.0 rad/s^2)" << std::endl;
+    ASSERT_TRUE(motor->commandProfileVelocity(0.0, 1.0));
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+}
+
+TEST(EyouMotorRealTest, CommandCyclicPosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
+
+    const std::chrono::milliseconds trajectory_duration{12000};
+    const double period_s = 6.0;
+    const double excursion_rad = 3.0;
+    const double center_q = motor->getQ();
+    const double omega = 2.0 * kPi / period_s;
+
+    std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q
+              << " rad, period=" << period_s
+              << " s, excursion=" << excursion_rad
+              << " rad, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double theta = omega * t_s;
+        const double target_q =
+            center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta));
+        const double target_qd =
+            0.5 * excursion_rad * omega * std::sin(theta);
+
+        ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_q=" << target_q
+                      << " rad, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+}
+
+TEST(EyouMotorRealTest, CommandCyclicVelocity)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
+
+    const std::chrono::milliseconds trajectory_duration{12000};
+    const double period_s = 5.0;
+    const double excursion_rad = 5.0;
+    const double phase_rad = 0.0;
+    const double omega = 2.0 * kPi / period_s;
+    const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega;
+
+    std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
+              << " s, velocity_amplitude=" << velocity_amplitude_rad_s
+              << " rad/s, excursion=" << excursion_rad
+              << " rad, phase=" << phase_rad
+              << " rad, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double theta = omega * t_s + phase_rad;
+        const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
+
+        ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+            printRawEthercatFeedback("raw feedback", runtime);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl;
+    ASSERT_TRUE(motor->commandCyclicVelocity(0.0));
+    sampleMotorState(*motor, std::chrono::milliseconds{500});
+    printRawEthercatFeedback("raw feedback after stop", runtime);
+}
+
+TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
+
+    const std::chrono::milliseconds run_duration{2000};
+    const double period_s = 6.0;
+    const double velocity_amplitude_rad_s = 4.5;
+    const double phase_rad = 0.0;
+    const double omega = 2.0 * kPi / period_s;
+
+    std::cout << "motor.commandCyclicVelocity(sin), then quickStop at "
+              << run_duration.count()
+              << " ms, period=" << period_s
+              << " s, velocity_amplitude=" << velocity_amplitude_rad_s
+              << " rad/s, phase=" << phase_rad
+              << " rad, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = run_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double theta = omega * t_s + phase_rad;
+        const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
+
+        ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+            printRawEthercatFeedback("raw feedback", runtime);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+TEST(EyouMotorRealTest, QuickStopInProfilePosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
+
+    const std::chrono::milliseconds quick_stop_time{1000};
+    const double start_q = motor->getQ();
+    const double target_q = nearbySafeTarget(start_q, 4.0);
+    const double max_qd = 2.0;
+    const double max_qdd = 10.0;
+
+    std::cout << "motor.commandProfilePosition(" << target_q
+              << " rad, " << max_qd
+              << " rad/s, " << max_qdd
+              << " rad/s^2), then quickStop at "
+              << quick_stop_time.count() << " ms" << std::endl;
+    ASSERT_TRUE(motor->commandProfilePosition(target_q, max_qd, max_qdd));
+    std::cout << "wait " << quick_stop_time.count()
+              << " ms before quickStop" << std::endl;
+    sampleMotorState(*motor, quick_stop_time);
+    printRawEthercatFeedback("raw feedback before quickStop", runtime);
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
+
+    const std::chrono::milliseconds quick_stop_time{2000};
+    const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
+    const double max_qdd = 10.0;
+
+    std::cout << "motor.commandProfileVelocity(" << target_qd
+              << " rad/s, " << max_qdd
+              << " rad/s^2), then quickStop at "
+              << quick_stop_time.count() << " ms" << std::endl;
+    ASSERT_TRUE(motor->commandProfileVelocity(target_qd, max_qdd));
+    std::cout << "wait " << quick_stop_time.count()
+              << " ms before quickStop" << std::endl;
+    sampleMotorState(*motor, quick_stop_time);
+    printRawEthercatFeedback("raw feedback before quickStop", runtime);
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
+
+    const std::chrono::milliseconds run_duration{2000};
+    const double start_q = motor->getQ();
+    const double target_qd = start_q > 0.0 ? -2.0 : 2.0;
+
+    std::cout << "motor.commandCyclicPosition(linear), start_q=" << start_q
+              << " rad, target_qd=" << target_qd
+              << " rad/s, then quickStop at "
+              << run_duration.count()
+              << " ms, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = run_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double target_q = start_q + target_qd * t_s;
+
+        ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_q=" << target_q
+                      << " rad, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+            printRawEthercatFeedback("raw feedback", runtime);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h

@@ -21,29 +21,37 @@ public:
     std::string typeName() const override { return "MujocoMotor"; }
     bool init() override;
 
-    void setMode(msgs::RunMode mode) override;
+    bool setMode(msgs::RunMode mode) override;
     msgs::RunMode getMode() override;
-    void torqueOff() override;
+    bool torqueOn() override;
+    bool torqueOff() override;
+    bool brakeRelease() override;
+    bool quickStop() override;
 
     void setLimitQ(double ub, double lb) override;
     void setLimitQd(double qd) override;
     void setLimitQdd(double u_qdd, double l_qdd) override;
 
-    void brake() override;
-    void setQ(double q) override;
-    void setTarget(double q, double qd) override;
-    void setTarget(double qd) override;
     bool calibrateZeroQ() override;
     bool reachedTargetQ() override;
-    void setQd(double qd) override;
+    bool commandProfilePosition(double target_q,
+                                double max_qd = 0.0,
+                                double max_qdd = 0.0) override;
+    bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) override;
+    bool commandCyclicPosition(double target_q,
+                               double target_qd = 0.0) override;
+    bool commandCyclicVelocity(double target_qd) override;
+    bool commandCyclicTorque(double target_tau) override;
     double getQ() override;
     double getQd() override;
 
-    static bool setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
+    static bool commandCyclicPositionsAtomic(
-                                 const std::vector<double>& positions,
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
-                                 const std::vector<double>& velocities);
+        const std::vector<double>& positions,
+        const std::vector<double>& velocities);
 
 private:
+    bool holdPosition_();
     double clampQ_(double q) const;
     double clampQd_(double qd) const;
     std::shared_ptr<simulate::MujocoWorld> worldLocked_() const;

cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp

@@ -42,10 +42,11 @@ bool MujocoMotor::init()
     return true;
 }
 
-void MujocoMotor::setMode(const msgs::RunMode mode)
+bool MujocoMotor::setMode(const msgs::RunMode mode)
 {
     std::scoped_lock lock(mtx_);
     mode_ = mode;
+    return true;
 }
 
 msgs::RunMode MujocoMotor::getMode()
@@ -54,11 +55,27 @@ msgs::RunMode MujocoMotor::getMode()
     return mode_;
 }
 
-void MujocoMotor::torqueOff()
+bool MujocoMotor::torqueOn()
 {
-    brake();
+    return holdPosition_();
+}
+
+bool MujocoMotor::torqueOff()
+{
+    const bool ok = holdPosition_();
     std::scoped_lock lock(mtx_);
     mode_ = msgs::RUN_MODE_UNSPECIFIED;
+    return ok;
+}
+
+bool MujocoMotor::brakeRelease()
+{
+    return true;
+}
+
+bool MujocoMotor::quickStop()
+{
+    return holdPosition_();
 }
 
 void MujocoMotor::setLimitQ(const double ub, const double lb)
@@ -82,38 +99,78 @@ void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd)
     info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_));
 }
 
-void MujocoMotor::brake()
+bool MujocoMotor::holdPosition_()
 {
     const auto world = worldLocked_();
     double q = 0.0;
     if (!world || !world->getJointPosition(info_.joint_name, q)) {
-        return;
+        return false;
     }
     std::scoped_lock lock(mtx_);
     target_q_ = q;
     mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
     world->setJointTargetState(info_.joint_name, q, 0.0);
+    return true;
 }
 
-void MujocoMotor::setQ(const double q)
+bool MujocoMotor::commandProfilePosition(const double target_q,
+                                         const double max_qd,
+                                         const double max_qdd)
 {
-    setTarget(q, 0.0);
+    (void)max_qdd;
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return false;
+    }
+    mode_ = msgs::RUN_MODE_PROFILE_POSITION;
+    target_q_ = clampQ_(target_q);
+    const double profile_qd = max_qd > 0.0 ? max_qd : info_.limit_qd;
+    return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(profile_qd));
 }
 
-void MujocoMotor::setTarget(const double q, const double qd)
+bool MujocoMotor::commandProfileVelocity(const double target_qd, const double max_qdd)
+{
+    (void)max_qdd;
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return false;
+    }
+    mode_ = msgs::RUN_MODE_PROFILE_VELOCITY;
+    return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd));
+}
+
+bool MujocoMotor::commandCyclicPosition(const double target_q,
+                                        const double target_qd)
 {
     std::scoped_lock lock(mtx_);
     const auto world = worldLocked_();
     if (!world) {
-        return;
+        return false;
     }
-    target_q_ = clampQ_(q);
+    mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
-    world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd));
+    target_q_ = clampQ_(target_q);
+    return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(target_qd));
 }
 
-void MujocoMotor::setTarget(const double qd)
+bool MujocoMotor::commandCyclicVelocity(const double target_qd)
 {
-    setQd(qd);
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return false;
+    }
+    mode_ = msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
+    return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd));
+}
+
+bool MujocoMotor::commandCyclicTorque(const double target_tau)
+{
+    (void)target_tau;
+    CMVR_LOG(ERROR) << "[MujocoMotor] cyclic torque command is not implemented: "
+                    << info_.joint_name;
+    return false;
 }
 
 bool MujocoMotor::calibrateZeroQ()
@@ -140,16 +197,6 @@ bool MujocoMotor::reachedTargetQ()
     }
 }
 
-void MujocoMotor::setQd(const double qd)
-{
-    std::scoped_lock lock(mtx_);
-    const auto world = worldLocked_();
-    if (!world) {
-        return;
-    }
-    world->setJointTargetVelocity(info_.joint_name, clampQd_(qd));
-}
-
 double MujocoMotor::getQ()
 {
     const auto world = worldLocked_();
@@ -170,9 +217,10 @@ double MujocoMotor::getQd()
     return qd;
 }
 
-bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
+bool MujocoMotor::commandCyclicPositionsAtomic(
-                                   const std::vector<double>& positions,
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
-                                   const std::vector<double>& velocities)
+    const std::vector<double>& positions,
+    const std::vector<double>& velocities)
 {
     if (motors.size() != positions.size() || motors.size() != velocities.size() || motors.empty()) {
         return false;
@@ -187,7 +235,7 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
     clamped_velocities.reserve(motors.size());
 
     for (std::size_t i = 0; i < motors.size(); ++i) {
-        const auto& motor = motors[i];
+        const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
         if (!motor) {
             return false;
         }
@@ -217,9 +265,13 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
     }
 
     for (std::size_t i = 0; i < motors.size(); ++i) {
-        std::scoped_lock lock(motors[i]->mtx_);
+        const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
-        motors[i]->target_q_ = clamped_positions[i];
+        if (!motor) {
-        motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+            return false;
+        }
+        std::scoped_lock lock(motor->mtx_);
+        motor->target_q_ = clamped_positions[i];
+        motor->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
     }
     return true;
 }

cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h

@@ -22,6 +22,8 @@ namespace  cmvr {
                 info_.limit_q_ub = config.limit_q_ub();
                 info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5;
                 info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0;
+                encoder_counts_per_rev_ = config.encoder_counts_per_rev();
+                gear_ratio_ = config.gear_ratio();
                 node_id_ = info_.id;
             }
 
@@ -37,6 +39,19 @@ namespace  cmvr {
                 }
                 if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) {
                     auto canopen_protocol = std::dynamic_pointer_cast<Ti5MotorCanopenProtocol>(protocol_);
+                    if (!canopen_protocol) {
+                        CMVR_LOG(ERROR) << "[Ti5Motor] invalid CANopen protocol for motor: "
+                                        << info_.joint_name;
+                        return false;
+                    }
+                    if (encoder_counts_per_rev_ <= 0.0 || gear_ratio_ <= 0.0) {
+                        CMVR_LOG(ERROR) << "[Ti5Motor] missing encoder conversion config: "
+                                        << info_.joint_name
+                                        << ", encoder_counts_per_rev=" << encoder_counts_per_rev_
+                                        << ", gear_ratio=" << gear_ratio_;
+                        return false;
+                    }
+                    protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_);
                     // torqueOff(node_id_);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION);
                     // canopen_protocol->torqueOff(node_id_);
@@ -44,9 +59,14 @@ namespace  cmvr {
                     // canopen_protocol->configProfile(node_id_,4000,8000,8000);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE);
-                    canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+                    if (!canopen_protocol->setMode(
-                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
+                            node_id_, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
-                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
+                        CMVR_LOG(ERROR) << "[Ti5Motor] failed to initialize operation mode: "
+                                        << info_.joint_name;
+                        return false;
+                    }
+                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
+                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
                     canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
                     canopen_protocol->setLimitQd(node_id_, info_.limit_qd);
                     canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
@@ -55,6 +75,10 @@ namespace  cmvr {
                 }
                 return true;
             }
+
+        private:
+            double encoder_counts_per_rev_{0.0};
+            double gear_ratio_{0.0};
         };
 
 

cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h

@@ -18,6 +18,7 @@
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h"
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h"
 #include <cmath>
+#include <unordered_map>
 
 namespace cmvr {
     namespace device {
@@ -29,28 +30,40 @@ namespace cmvr {
 
             bool initNode(uint8_t node_id) override;
 
-            void setMode(uint8_t node_id, msgs::RunMode mode);
+            bool setMode(uint8_t node_id, msgs::RunMode mode) override;
-            void setTarget(uint8_t node_id, double angle_rad, double vel) override;
-            void setTarget(uint8_t node_id, double vel) override;
-            void setQ(uint8_t node_id, double angle_rad) override;
             void setLimitQ(uint8_t node_id, double ub, double lb) override;
             void setLimitQd(uint8_t node_id, double qd) override;
             void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;
             bool calibrateZeroQ(uint8_t node_id) override;
-            void brake(uint8_t node_id) override;
+            bool torqueOn(uint8_t node_id) override;
+            bool torqueOff(uint8_t node_id) override;
+            bool brakeRelease(uint8_t node_id) override;
+            bool quickStop(uint8_t node_id) override;
             bool reachedTargetQ(uint8_t node_id) override;
 
             double getQ(uint8_t node_id) override;
             double getQd(uint8_t node_id) override;
 
-            void setQd(uint8_t node_id, double qd) override;
+            bool commandProfilePosition(uint8_t node_id,
-            void setQdd(uint8_t node_id, double qdd) override;
+                                        double target_q,
-
+                                        double max_qd,
-            void torqueOff(uint8_t node_id) override;
+                                        double max_qdd) override;
+            bool commandProfileVelocity(uint8_t node_id,
+                                        double target_qd,
+                                        double max_qdd) override;
+            bool commandCyclicPosition(uint8_t node_id,
+                                       double target_q,
+                                       double target_qd) override;
+            bool commandCyclicVelocity(uint8_t node_id,
+                                       double target_qd) override;
+            bool commandCyclicTorque(uint8_t node_id, double target_tau) override;
+            void setMotorConversion(uint8_t node_id,
+                                    double encoder_counts_per_rev,
+                                    double gear_ratio) override;
 
             void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10);
-            void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index,
+            void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, uint32_t index,
-                          msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10);
+                          uint32_t sub_index, uint32_t data, uint32_t delay_ms = 10);
 
             void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
             void configPdo(uint8_t node_id);
@@ -61,23 +74,24 @@ namespace cmvr {
                 return data_ptr;
             }
 
-            msgs::RunMode getMode(uint8_t node_id) override {
+            msgs::RunMode getMode(uint8_t node_id) override;
-                return  GetRobotDetail()->motors().at(node_id).run_mode();
-                // cur_mode_[node_id] = feed_mode;
-                // return feed_mode;
-                // return cur_mode_[node_id];
-            }
 
 
         private:
-            static constexpr double GearRatio = 101.0; // 电机减速比
             static constexpr double RADTODEG = 180.0 / M_PI;
+            static constexpr double Ti5VelocityUnitScale = 100.0;
+            static constexpr double Ti5AccelerationTimeScale = 1000.0;
+
+            struct MotorConversion {
+                double encoder_counts_per_rev{0.0};
+                double gear_ratio{0.0};
+            };
+
             std::shared_ptr<AbstractCanbus> can_client_{nullptr};
 
             // key node_id
             // std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
-            std::unordered_map<uint8_t,uint32_t> last_Qd_{};
+            std::unordered_map<uint8_t, MotorConversion> motor_conversions_{};
-            std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
             std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
             std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
 
@@ -94,11 +108,8 @@ namespace cmvr {
             std::map<uint8_t, motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
             std::map<uint8_t, motor::Ti5MotorRPDO2 *> rpdo2_commands_{};
 
-            void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
+            bool getMotorStatus(uint8_t node_id, msgs::MotorStatus* status) const;
-
+            void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos);
-            void setPPTargetPosByPdo(uint8_t node_id, int32_t pos);
-
-            void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos);
 
 
             void configTPDO1(uint8_t node_id);
@@ -107,6 +118,14 @@ namespace cmvr {
             void configRPDO1(uint8_t node_id, bool enable);
             void configRPDO2(uint8_t node_id, bool enable);
 
+            const MotorConversion* conversionForNode(uint8_t node_id) const;
+            double radToCounts(double angle_rad, const MotorConversion& conversion) const;
+            double countsToRad(int32_t counts, const MotorConversion& conversion) const;
+            double radPerSecToVelocityRaw(double velocity_rad_s, const MotorConversion& conversion) const;
+            uint32_t radPerSec2ToAccelerationRaw(double acceleration_rad_s2,
+                                                 const MotorConversion& conversion) const;
+            double velocityRawToRadPerSec(int32_t velocity_raw, const MotorConversion& conversion) const;
+
 
             bool waitUntil(std::function<bool()> condition, int timeout_ms) {
                 auto start = std::chrono::steady_clock::now();

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp

@@ -3,6 +3,7 @@
 // Created by lgv on 2025/7/24.
 //
 
+#include "cmvr/msgs/cia402.pb.h"
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h"
 
 using namespace cmvr::device::motor;
@@ -19,17 +20,17 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
 
 
     switch (sdo_response.index()) {
-        case msgs::CONTROL_WORD_6040:
+        case msgs::CIA402_CONTROL_WORD_6040:
             motor_status->set_ctrl_word(sdo_response.data());
             break;
-        case msgs::STATUS_WORD_6041:
+        case msgs::CIA402_STATUS_WORD_6041:
             motor_status->set_status_word(sdo_response.data());
             break;
-        case msgs::ACTUAL_POSITION_6064:
+        case msgs::CIA402_ACTUAL_POSITION_6064:
             motor_status->set_position(static_cast<int32_t>(sdo_response.data()));
             CMVR_LOG(INFO) << "pos = " << motor_status->position();
             break;
-        case msgs::POSITION_OFFSET_2008:
+        case msgs::CANOPEN_POSITION_OFFSET_2008:
             motor_status->set_position_offset(sdo_response.data());
     }
         //

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp

@@ -19,16 +19,4 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot
 
     motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]);
     motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]);
-
-
-    double gearRatio = 101.0;
-    double radToDeg = 180.0 / M_PI;
-    auto speed =   (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0);
-
-    auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg);
-
-    // CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s";
-
-
-
 }

cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp

@@ -3,6 +3,7 @@
 // Created by lgv on 2025/8/1.
 //
 
+#include "cmvr/msgs/cia402.pb.h"
 #include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 #include "canbus/canopen/register.h"
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h"
@@ -66,7 +67,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
 
     if (sdo_commands_[node_id] == nullptr) {
         CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
-        return ErrorCode::CANBUS_ERROR;
+        return false;
     }
     can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true);
 
@@ -77,7 +78,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
 
     if (rpdo1_commands_[node_id] == nullptr) {
         CMVR_LOG(ERROR) << "Ti5 Motor RPDO1  Protocol does not exist in the MessageManager!";
-        return ErrorCode::CANBUS_ERROR;
+        return false;
     }
     can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
 
@@ -87,52 +88,187 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
 
     if (rpdo2_commands_[node_id] == nullptr) {
         CMVR_LOG(ERROR) << "Ti5 Motor RPDO2  Protocol does not exist in the MessageManager!";
-        return ErrorCode::CANBUS_ERROR;
+        return false;
     }
     can_sender_->AddMessage(rpdo2_commands_[node_id]->ID(), rpdo2_commands_[node_id], true);
 
-    return ErrorCode::OK;
+    return true;
+}
+
+bool Ti5MotorCanopenProtocol::getMotorStatus(
+    const uint8_t node_id,
+    msgs::MotorStatus* const status) const {
+    if (!message_manager_ || !status) {
+        return false;
+    }
+
+    msgs::RobotDetail robot_detail;
+    if (message_manager_->GetSensorData(&robot_detail) != ErrorCode::OK) {
+        return false;
+    }
+    const auto it = robot_detail.motors().find(node_id);
+    if (it == robot_detail.motors().end()) {
+        return false;
+    }
+    status->CopyFrom(it->second);
+    return true;
+}
+
+cmvr::msgs::RunMode Ti5MotorCanopenProtocol::getMode(const uint8_t node_id) {
+    msgs::MotorStatus status;
+    if (!getMotorStatus(node_id, &status)) {
+        return msgs::RUN_MODE_UNSPECIFIED;
+    }
+    return status.run_mode();
+}
+
+void Ti5MotorCanopenProtocol::setMotorConversion(
+    const uint8_t node_id,
+    const double encoder_counts_per_rev,
+    const double gear_ratio) {
+    motor_conversions_[node_id] = {encoder_counts_per_rev, gear_ratio};
+}
+
+const Ti5MotorCanopenProtocol::MotorConversion*
+Ti5MotorCanopenProtocol::conversionForNode(const uint8_t node_id) const {
+    const auto it = motor_conversions_.find(node_id);
+    if (it != motor_conversions_.end() &&
+        it->second.encoder_counts_per_rev > 0.0 &&
+        it->second.gear_ratio > 0.0) {
+        return &it->second;
+    }
+    CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] missing conversion config for node "
+                    << static_cast<int>(node_id);
+    return nullptr;
+}
+
+double Ti5MotorCanopenProtocol::radToCounts(
+    const double angle_rad,
+    const MotorConversion& conversion) const {
+    return (angle_rad * RADTODEG) / 360.0 *
+           conversion.gear_ratio * conversion.encoder_counts_per_rev;
+}
+
+double Ti5MotorCanopenProtocol::countsToRad(
+    const int32_t counts,
+    const MotorConversion& conversion) const {
+    return (counts * 360.0) /
+           (conversion.gear_ratio * conversion.encoder_counts_per_rev * RADTODEG);
+}
+
+double Ti5MotorCanopenProtocol::radPerSecToVelocityRaw(
+    const double velocity_rad_s,
+    const MotorConversion& conversion) const {
+    return ((velocity_rad_s * RADTODEG) * conversion.gear_ratio * Ti5VelocityUnitScale) /
+           360.0;
+}
+
+uint32_t Ti5MotorCanopenProtocol::radPerSec2ToAccelerationRaw(
+    const double acceleration_rad_s2,
+    const MotorConversion& conversion) const {
+    const auto raw = ((std::abs(acceleration_rad_s2) * RADTODEG) *
+                      conversion.gear_ratio * Ti5VelocityUnitScale) /
+                     360.0 / Ti5AccelerationTimeScale;
+    return static_cast<uint32_t>(std::abs(raw));
+}
+
+double Ti5MotorCanopenProtocol::velocityRawToRadPerSec(
+    const int32_t velocity_raw,
+    const MotorConversion& conversion) const {
+    return (velocity_raw * 360.0) /
+           (conversion.gear_ratio * Ti5VelocityUnitScale * RADTODEG);
 }
 
 
-void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index,
+void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, uint32_t index, uint32_t sub_index,
                                              uint32_t data, uint32_t delay_ms) {
     sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data);
     can_sender_->Update(sdo_commands_[node_id]->ID());
     std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
 }
 
-void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) {
+bool Ti5MotorCanopenProtocol::commandProfilePosition(uint8_t node_id,
-    auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
+                                                     double target_q,
-
+                                                     double max_qd,
-    switch (getMode(node_id)) {
+                                                     double max_qdd) {
-        // case RUN_MODE_CYCLIC_SYNC_POSITION:
+    const auto* conversion = conversionForNode(node_id);
-        //     setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
+    if (!conversion) {
-        //     break;
+        return false;
-        case RUN_MODE_PROFILE_POSITION:
-            // setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
-            setPPTargetPosBySdo(node_id, static_cast<int32_t>(cmd));
-            break;
     }
+    if (max_qd > 0.0) {
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081,
+                       SUB_INDEX_0,
+                       static_cast<uint32_t>(std::abs(radPerSecToVelocityRaw(max_qd, *conversion))),
+                       0);
+    }
+    if (max_qdd > 0.0) {
+        const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083,
+                       SUB_INDEX_0, accel, 0);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084,
+                       SUB_INDEX_0, accel, 0);
+    }
+    writeProfilePositionTargetBySdo(node_id, static_cast<int32_t>(radToCounts(target_q, *conversion)));
+    return true;
 }
 
-void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) {
+bool Ti5MotorCanopenProtocol::commandProfileVelocity(uint8_t node_id,
-    auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
+                                                     double target_qd,
-    auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
+                                                     double max_qdd) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return false;
+    }
+    if (max_qdd > 0.0) {
+        const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083,
+                       SUB_INDEX_0, accel, 0);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084,
+                       SUB_INDEX_0, accel, 0);
+    }
+    const auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_VELOCITY_60FF,
+                   SUB_INDEX_0,
+                   static_cast<uint32_t>(static_cast<int32_t>(std::llround(speed))),
+                   0);
+    return true;
+}
+
+bool Ti5MotorCanopenProtocol::commandCyclicPosition(uint8_t node_id,
+                                                    double target_q,
+                                                    double target_qd) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return false;
+    }
+    auto pos_cmd = radToCounts(target_q, *conversion);
+    auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
     rpdo1_commands_[node_id]->SetTargetPos(pos_cmd);
     rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed)));
     can_sender_->Update(rpdo1_commands_[node_id]->ID());
+    return true;
 }
 
-
+bool Ti5MotorCanopenProtocol::commandCyclicVelocity(uint8_t node_id,
-void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) {
+                                                    double target_qd) {
-    auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return false;
+    }
+    auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
     rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed));
     can_sender_->Update(rpdo2_commands_[node_id]->ID());
+    return true;
 }
 
+bool Ti5MotorCanopenProtocol::commandCyclicTorque(uint8_t node_id, double target_tau) {
+    (void)target_tau;
+    CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] cyclic torque command is not implemented, node="
+                    << static_cast<int>(node_id);
+    return false;
+}
 
-void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
+void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos) {
     controlword_t cw = {};
     cw.switch_on = 1;
     cw.enable_voltage = 1;
@@ -141,45 +277,20 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos)
     cw.change_set_immediately = 1;
 
     // 1. 设置目标位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, pos);
 
 
     // 2. 设置触发位（bit4 = 1）
     cw.new_set_point = 1;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
 
 
     // 3. 清除触发位（bit4 = 0），准备下一次触发
     cw.new_set_point = 0;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
 }
 
-void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
+bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
-    // 触发目标位置运动
-    controlword_t cw;
-    cw.value = 0x0F;
-    cw.new_set_point = 1;
-    cw.change_set_immediately = 1;
-
-    rpdo1_commands_[node_id]->SetTargetPos(pos);
-    rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
-    can_sender_->Update(rpdo1_commands_[node_id]->ID());
-
-    std::this_thread::sleep_for(std::chrono::milliseconds(10));
-
-    cw.new_set_point = 0;
-    rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
-    can_sender_->Update(rpdo1_commands_[node_id]->ID());
-}
-
-void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
-    rpdo1_commands_[node_id]->SetTargetPos(pos);
-    rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
-    can_sender_->Update(rpdo1_commands_[node_id]->ID());
-}
-
-
-void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
     // cur_mode_[node_id] = mode;
 
 
@@ -188,56 +299,72 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
     controlword_t cw = {};
     cw.quick_stop = 1;
     cw.enable_voltage = 1;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
     // configRPDO1(node_id, false);
     // configRPDO2(node_id, false);
 
     // 1 : 先设置模式
     auto data = static_cast<uint32_t>(mode);
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CIA402_OPERATION_MODE_6060, SUB_INDEX_0, data);
 
 
     // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
     cw.switch_on = 1;
     cw.enable_operation = 1;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
+
+    int32_t current_position = 0;
+    if (mode == RUN_MODE_PROFILE_POSITION || mode == RUN_MODE_CYCLIC_SYNC_POSITION) {
+        seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
+        msgs::MotorStatus status;
+        if (!waitUntil([&]() {
+                return getMotorStatus(node_id, &status) &&
+                       status.has_sdo_response() &&
+                       status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064;
+            }, 500)) {
+            CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
+                            << ": actual-position feedback timed out";
+            return false;
+        }
+        current_position = status.position();
+    }
 
 
     switch (mode) {
         case RUN_MODE_PROFILE_POSITION: {
             // 4 : 设置目标位置（为当前位置）
-            auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
+            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A,
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
+                           SUB_INDEX_0, current_position);
 
 
             // 5 : 触发位置运动（new_set_point 翻转）
             cw.new_set_point = 1;
             cw.change_set_immediately = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
 
             // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
             cw.new_set_point = 0;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
 
         case RUN_MODE_CYCLIC_SYNC_POSITION: {
             // configRPDO1(node_id, true);
             // 设置目标位置为当前位置
-            auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
+            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A,
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
+                           SUB_INDEX_0, current_position);
 
             //3 : 使能   15
             cw.enable_operation = 1;
             cw.switch_on = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
 
         case RUN_MODE_PROFILE_VELOCITY: {
             cw.enable_operation = 1;
             cw.switch_on = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
 
@@ -245,13 +372,21 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
             // configRPDO2(node_id, true);
             cw.enable_operation = 1;
             cw.switch_on = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
         default:
             // TODO: Handle unspecified or unknown mode
-            break;
+            return false;
     }
+
+    if (!waitUntil([&]() { return getMode(node_id) == mode; }, 500)) {
+        CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
+                        << ": operation mode switch failed, target_mode="
+                        << static_cast<int>(mode);
+        return false;
+    }
+    return true;
 }
 
 void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
@@ -262,9 +397,9 @@ void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand c
 
 
 void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, SUB_INDEX_0, speed);
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
 }
 
 
@@ -272,128 +407,128 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) {
     //TDPO1 配置   状态字 和 控制字
     // 1: 失能 pdo
     uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 0);
 
     // 2: 配置为异步
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
     // 3：配置约束时间 unit:0.1ms
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_3, 10);
 
     // 4 : 配置周期发送时间  unit : ms   0 为 数据改变时发送
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_5, 0);
 
     // 5 ：映射控制字
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_1,
-                   CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
+                   CIA402_CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
 
     //6 : 映射状态字
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_2,
-                   STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
+                   CIA402_STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
 
     //7 : 映射模式
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_3,
-                   MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
+                   CIA402_MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
 
     //8  映射错误码
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_4,
-                   ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
+                   CIA402_ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
 
     //9  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 4);
 
     //10 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 
 void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) {
     // 1: 失能 pdo
     uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 0);
 
     // 2: 配置为异步
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
     // 3：配置约束时间 unit:0.1ms
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_3, 100);
 
     // 4 : 配置周期发送时间  unit : ms
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_5, 0);
 
     // 5 ：映射当前位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_1,
-                   ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
+                   CIA402_ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
 
     //6 : 映射当前速度
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_2,
-                   ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
+                   CIA402_ACTUAL_VELOCITY_606C << 16 | SUB_INDEX_0 << 8 | 32);
 
     //9  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 2);
 
     //10 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool enable) {
     // 1: 失能 pdo
     uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 0);
 
     // 2: 配置为
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
     // // 3：配置约束时间 unit:0.1ms
-    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
+    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_3,10);
     //
     // // 4 : 配置周期发送时间  unit : ms   0 为 数据改变时发送
-    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
+    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_5,0);
 
     // 5 ：映射位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_1,
-                   TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
+                   CIA402_TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
 
     //6 : 映射控制字
 
 
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_2,
-                   PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32);
+                   CIA402_PROFILE_VELOCITY_6081 << 16 | SUB_INDEX_0 << 8 | 32);
 
 
     //7  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 2);
 
     //8 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 
 void Ti5MotorCanopenProtocol::configRPDO2(uint8_t node_id, bool enable) {
     // 1: 失能 pdo
     uint32_t cob_id = RPDO2_BASE_ID_300 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 0);
 
     if (!enable) return;
 
     // 2: 配置为
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
 
     // 5 ：映射位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_MAP_1601, SUB_INDEX_1,
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_1,
-                   TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32);
+                   CIA402_TARGET_VELOCITY_60FF << 16 | SUB_INDEX_0 << 8 | 32);
 
 
     //7  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 1);
 
     //8 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 
@@ -405,140 +540,166 @@ void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) {
 }
 
 void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) {
-    auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
+    const auto* conversion = conversionForNode(node_id);
-    auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
+    if (!conversion) {
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
+        return;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
+    }
+    auto accel = ((u_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) /
+                 360.0 / Ti5AccelerationTimeScale;
+    auto decel = ((l_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) /
+                 360.0 / Ti5AccelerationTimeScale;
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
 }
 
 void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) {
-    auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
+    const auto* conversion = conversionForNode(node_id);
-    // seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed);
+    if (!conversion) {
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
+        return;
+    }
+    auto speed = radPerSecToVelocityRaw(qd, *conversion);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_MAX_PROFILE_VELOCITY_607F, SUB_INDEX_0, speed);
 }
 
 void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) {
-    ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
+    const auto* conversion = conversionForNode(node_id);
-    lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0;
+    if (!conversion) {
+        return;
+    }
+    ub = radToCounts(ub, *conversion);
+    lb = radToCounts(lb, *conversion);
 
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
 }
 
 
 bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
     // 0: 设置控制字为 0x06，确保停机状态
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
 
     // 1: 清除偏置值 0x2008 ← 0
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0);
 
     // 2: 等待确认清除成功
-    seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0);
     if (!waitUntil([&]() {
-        return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
+        msgs::MotorStatus status;
+        return getMotorStatus(node_id, &status) &&
+               status.has_sdo_response() &&
+               status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 &&
+               status.position_offset() == 0;
     }, 1000)) {
         CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed";
         return false;
     }
 
     // 3: 读取当前位置 0x6064
-    seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
+    seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
-    auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
+    msgs::MotorStatus status;
+    if (!waitUntil([&]() {
+            return getMotorStatus(node_id, &status) &&
+                   status.has_sdo_response() &&
+                   status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064;
+        }, 500)) {
+        CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
+                        << ": actual-position feedback timed out during calibration";
+        return false;
+    }
+    const auto cur_pos = status.position();
 
     // 4: 将当前位置写入偏置寄存器
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
 
     // 5: 保存参数到永久区（0x2000 ← 1）
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
 
 
     // 6: 确认写入成功
-    seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
+    seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
     if (!waitUntil([&]() {
-        return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
+        msgs::MotorStatus latest_status;
+        return getMotorStatus(node_id, &latest_status) &&
+               latest_status.has_sdo_response() &&
+               latest_status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 &&
+               latest_status.position_offset() == cur_pos;
     }, 500)) {
-        return false;
         CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed";
+        return false;
     }
 
     return true;
 }
 
-void Ti5MotorCanopenProtocol::brake(uint8_t node_id) {
+bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) {
+    return setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+}
+
+bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) {
+    // (void)node_id;
+    // CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented";
+    torqueOff(node_id);
+    return true;
+}
+
+bool Ti5MotorCanopenProtocol::quickStop(uint8_t node_id) {
     // // 开机未使能电机时调用
-    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
     // 6 抱闸   0 ： 立即停机 自由
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_QUICK_STOP_DECELERATION_6085, SUB_INDEX_0, 0XFFFFFFF0);
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
 
     // 必须要发送 0xf 才能按照6085中设定的减速度减速
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    return true;
 }
 
 bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
+    msgs::MotorStatus motor_status;
+    if (!getMotorStatus(node_id, &motor_status)) {
+        return false;
+    }
     statusword_t st{};
-    st.value = GetRobotDetail()->motors().at(node_id).status_word();
+    st.value = motor_status.status_word();
     return st.target_reached == 1;
 }
 
-void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) {
+bool Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
-    auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
-    switch (getMode(node_id)) {
-        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
-        case msgs::RUN_MODE_PROFILE_POSITION: {
-            auto it = last_Qd_.find(node_id);
-            if (it == last_Qd_.end() || it->second != speed) {
-                seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),
-                               0);
-                last_Qd_[node_id] = speed;
-            }
-            break;
-        }
-        case msgs::RUN_MODE_PROFILE_VELOCITY:
-        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: {
-            // 在速度模式下，直接设置目标速度
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
-            break;
-        }
-        default:
-            break;
-    }
-}
-
-void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) {
-    uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0);
-    auto it = last_Qdd_.find(node_id);
-    if (it == last_Qdd_.end() || it->second != accel) {
-        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
-        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel);
-        last_Qdd_[node_id] = accel;
-    }
-}
-
-void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
     // 0 ： 立即停机 自由
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
     // 必须要发送 0xf 才能按照6085中设定的减速度减速
-    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
 
     // 停机之后，要重新使能？
     // cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED;
+    return true;
 }
 
 
 double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) {
-    auto data_ptr = std::make_unique<msgs::RobotDetail>();
+    const auto* conversion = conversionForNode(node_id);
-    message_manager_->GetSensorData(data_ptr.get());
+    if (!conversion) {
-    auto cnt = data_ptr->motors().at(node_id).position();
+        return 0.0;
-    return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
+    }
+    msgs::MotorStatus motor_status;
+    if (!getMotorStatus(node_id, &motor_status)) {
+        return 0.0;
+    }
+    const auto cnt = motor_status.position();
+    return countsToRad(cnt, *conversion);
 }
 
 double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) {
-    auto data_ptr = std::make_unique<msgs::RobotDetail>();
+    const auto* conversion = conversionForNode(node_id);
-    message_manager_->GetSensorData(data_ptr.get());
+    if (!conversion) {
-    auto cnt = data_ptr->motors().at(node_id).speed();
+        return 0.0;
-    return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
+    }
+    msgs::MotorStatus motor_status;
+    if (!getMotorStatus(node_id, &motor_status)) {
+        return 0.0;
+    }
+    const auto cnt = motor_status.speed();
+    return velocityRawToRadPerSec(cnt, *conversion);
 }

cmvr-es/devices/motor/manager/CMakeLists.txt

@@ -12,6 +12,7 @@ target_link_libraries(motor_manager
         PRIVATE
         cmvr_es::device::ti5_canopen_motor_driver
         cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device::ethercat_motor_driver
         cmvr_es::ik_solver
         glog
 )

cmvr-es/devices/motor/manager/include/motor_manager.h

@@ -5,7 +5,6 @@
 #include <memory>
 #include <mutex>
 #include <string>
-#include <unordered_set>
 #include <unordered_map>
 #include <vector>
 
@@ -32,7 +31,9 @@ class AbstractMotorBusRuntime;
 class MotorManager final : public AbstractDevice,
                            public std::enable_shared_from_this<MotorManager> {
 public:
-    MotorManager(std::string id, const config::MotorConfig& cfg);
+    MotorManager(std::string id,
+                 const config::MotorConfig& cfg,
+                 std::string selected_group_id);
     ~MotorManager() override;
 
     DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; }
@@ -45,19 +46,18 @@ public:
     std::shared_ptr<AbstractMotor> getMotor(std::uint8_t node_id) const;
     std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const;
     const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const;
+    bool commandCyclicPositionsAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        const std::vector<double>& positions,
+        const std::vector<double>& velocities) const;
+    bool readFeedbacksAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        std::vector<double>& positions,
+        std::vector<double>& velocities) const;
 
     static std::shared_ptr<MotorManager> managerFor(const std::string& id);
     static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id);
-    static void setActiveJoints(const std::string& motor_manager_id,
-                                std::unordered_map<std::string, std::unordered_set<std::string>> group_joints);
-    static void clearActiveJoints();
-
 private:
-    using ActiveJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
-
-    bool selectActiveMotors_(const std::string& group_name,
-                             const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
-                             std::vector<config::MotorConfigItem>& selected) const;
     bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
                                      std::vector<config::MotorConfigItem>& selected) const;
     std::shared_ptr<AbstractMotorBusRuntime> createBusRuntime_(
@@ -81,6 +81,7 @@ private:
 
 private:
     config::MotorConfig cfg_;
+    std::string selected_group_id_;
     std::vector<std::shared_ptr<AbstractMotorBusRuntime>> bus_runtimes_;
     mutable std::mutex motors_mutex_;
     std::unordered_map<std::uint8_t, std::shared_ptr<AbstractMotor>> motors_by_id_;
@@ -90,7 +91,6 @@ private:
     static std::mutex registry_mutex_;
     static std::unordered_map<std::string, std::weak_ptr<MotorManager>> managers_;
     static std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> mujoco_world_registry_;
-    static std::unordered_map<std::string, ActiveJointSelection> active_joints_;
 };
 
 } // namespace cmvr::device

cmvr-es/devices/motor/manager/src/motor_manager.cpp

@@ -1,38 +1,41 @@
-#include "motor/manager/include/motor_manager.h"
+#include "devices/motor/manager/include/motor_manager.h"
 
+#include <chrono>
 #include <cmath>
 #include <cstddef>
 #include <cstdint>
+#include <thread>
 #include <unordered_map>
 #include <utility>
 
 #include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
 #include "common/base/logging/logger.h"
 #include "common/config/config_files.h"
-#include "../../bus_runtime/abstract_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h"
-#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
-#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
-#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
-#include "motor/drivers/mujoco/include/mujoco_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
-#include "motor/drivers/ti5_canopen/include/ti5_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
-#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
+#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
+#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 
 namespace cmvr::device {
 
 std::mutex MotorManager::registry_mutex_;
 std::unordered_map<std::string, std::weak_ptr<MotorManager>> MotorManager::managers_;
 std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> MotorManager::mujoco_world_registry_;
-std::unordered_map<std::string, MotorManager::ActiveJointSelection> MotorManager::active_joints_;
 
-MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg)
+MotorManager::MotorManager(std::string id,
-    : cfg_(cfg)
+                           const config::MotorConfig& cfg,
+                           std::string selected_group_id)
+    : cfg_(cfg),
+      selected_group_id_(std::move(selected_group_id))
 {
     id_ = std::move(id);
-    if (!cfg_.id().empty() && cfg_.id() != id_) {
-        CMVR_LOG(ERROR) << "[MotorManager] config id '" << cfg_.id()
-                        << "' does not match device id '" << id_ << "'";
-        id_.clear();
-    }
 }
 
 MotorManager::~MotorManager() = default;
@@ -46,6 +49,14 @@ bool MotorManager::init()
         CMVR_LOG(ERROR) << "[MotorManager] id is empty";
         return false;
     }
+    if (selected_group_id_.empty()) {
+        CMVR_LOG(ERROR) << "[MotorManager] selected motor group id is empty: " << id_;
+        return false;
+    }
+    if (cfg_.motor_groups_size() == 0) {
+        CMVR_LOG(ERROR) << "[MotorManager] no motor groups configured: " << id_;
+        return false;
+    }
 
     bus_runtimes_.clear();
     bus_runtimes_.reserve(static_cast<std::size_t>(cfg_.motor_groups_size()));
@@ -56,25 +67,27 @@ bool MotorManager::init()
     }
 
     bool all_ok = true;
+    std::size_t selected_group_count = 0;
     for (const auto& motor_group_cfg : cfg_.motor_groups()) {
         const auto& group_name = motor_group_cfg.id();
-        if (group_name.empty()) {
+        if (group_name != selected_group_id_) {
-            CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_;
-            all_ok = false;
             continue;
         }
+        ++selected_group_count;
         if (!motor_group_cfg.has_motors()) {
             CMVR_LOG(ERROR) << "[MotorManager] motor group missing motors: " << group_name;
             all_ok = false;
             continue;
         }
 
-        std::vector<config::MotorConfigItem> selected_motor_cfgs;
+        if (motor_group_cfg.motors().motors_size() == 0) {
-        const bool selected_active_group = selectActiveMotors_(
+            CMVR_LOG(ERROR) << "[MotorManager] motor group has no motors: " << group_name;
-            group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs);
+            all_ok = false;
-        if (!selected_active_group) {
             continue;
         }
+        std::vector<config::MotorConfigItem> selected_motor_cfgs(
+            motor_group_cfg.motors().motors().begin(),
+            motor_group_cfg.motors().motors().end());
 
         if (!applyConfiguredJointLimits_(motor_group_cfg, selected_motor_cfgs)) {
             all_ok = false;
@@ -91,11 +104,17 @@ bool MotorManager::init()
             all_ok = false;
             continue;
         }
-        if (!bus_runtime->start()) {
+
+        const bool start_before_motor_creation =
+            motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT;
+        if (start_before_motor_creation && !bus_runtime->start()) {
             bus_runtime->stop();
             all_ok = false;
             continue;
         }
+        if (start_before_motor_creation) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        }
 
         auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime);
         if (motors.empty()) {
@@ -104,7 +123,29 @@ bool MotorManager::init()
             all_ok = false;
             continue;
         }
+        if (!start_before_motor_creation && !bus_runtime->start()) {
+            bus_runtime->stop();
+            all_ok = false;
+            continue;
+        }
+
         bool group_ok = true;
+        if (motor_group_cfg.bus_type() == config::MOTOR_BUS_CAN) {
+            for (auto& motor : motors) {
+                if (!motor->init()) {
+                    CMVR_LOG(ERROR) << "[MotorManager] failed to initialize CAN motor: "
+                                    << motor->jointName();
+                    group_ok = false;
+                    break;
+                }
+            }
+        }
+        if (!group_ok) {
+            bus_runtime->stop();
+            all_ok = false;
+            continue;
+        }
+
         for (auto& motor : motors) {
             if (!addMotor(motor)) {
                 group_ok = false;
@@ -120,6 +161,12 @@ bool MotorManager::init()
         bus_runtimes_.push_back(std::move(bus_runtime));
     }
 
+    if (selected_group_count != 1) {
+        CMVR_LOG(ERROR) << "[MotorManager] selected motor group '" << selected_group_id_
+                        << "' must occur exactly once in config: " << id_;
+        all_ok = false;
+    }
+
     if (!all_ok) {
         for (auto& bus_runtime : bus_runtimes_) {
             if (bus_runtime) {
@@ -222,6 +269,66 @@ const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& MotorMana
     return motors_by_joint_;
 }
 
+bool MotorManager::commandCyclicPositionsAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    const std::vector<double>& positions,
+    const std::vector<double>& velocities) const
+{
+    if (motors.empty() || motors.size() != positions.size() ||
+        motors.size() != velocities.size()) {
+        return false;
+    }
+
+    if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
+        return EyouMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
+    }
+    if (std::dynamic_pointer_cast<MujocoMotor>(motors.front())) {
+        return MujocoMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
+    }
+    if (std::dynamic_pointer_cast<Ti5Motor>(motors.front())) {
+        // TI5 currently exposes only a per-motor RPDO command. Keep the
+        // fallback here so callers retain one manager-level entry point.
+        static std::once_flag ti5_non_atomic_warning;
+        std::call_once(ti5_non_atomic_warning, []() {
+            CMVR_LOG(WARNING)
+                << "[MotorManager] TI5 motors use non-atomic cyclic position commands";
+        });
+        for (std::size_t i = 0; i < motors.size(); ++i) {
+            if (!std::dynamic_pointer_cast<Ti5Motor>(motors[i])) {
+                CMVR_LOG(ERROR) << "[MotorManager] mixed motor types in cyclic position command";
+                return false;
+            }
+            if (!motors[i]->commandCyclicPosition(positions[i], velocities[i])) {
+                CMVR_LOG(ERROR) << "[MotorManager] failed to command TI5 motor at index " << i;
+                return false;
+            }
+        }
+        return true;
+    }
+
+    CMVR_LOG(ERROR) << "[MotorManager] atomic cyclic position is unsupported for motor type: "
+                    << motors.front()->typeName();
+    return false;
+}
+
+bool MotorManager::readFeedbacksAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    std::vector<double>& positions,
+    std::vector<double>& velocities) const
+{
+    if (motors.empty()) {
+        return false;
+    }
+
+    if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
+        return EyouMotor::readFeedbacksAtomic(motors, positions, velocities);
+    }
+
+    CMVR_LOG(ERROR) << "[MotorManager] atomic feedback is unsupported for motor type: "
+                    << motors.front()->typeName();
+    return false;
+}
+
 std::shared_ptr<MotorManager> MotorManager::managerFor(const std::string& id)
 {
     std::lock_guard<std::mutex> lock(registry_mutex_);
@@ -242,68 +349,6 @@ std::shared_ptr<simulate::MujocoWorld> MotorManager::mujocoWorldFor(const std::s
     return it->second.lock();
 }
 
-void MotorManager::setActiveJoints(const std::string& motor_manager_id,
-                                  ActiveJointSelection group_joints)
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_[motor_manager_id] = std::move(group_joints);
-}
-
-void MotorManager::clearActiveJoints()
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_.clear();
-}
-
-bool MotorManager::selectActiveMotors_(
-    const std::string& group_name,
-    const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
-    std::vector<config::MotorConfigItem>& selected) const
-{
-    selected.clear();
-
-    ActiveJointSelection selection;
-    {
-        std::lock_guard<std::mutex> lock(registry_mutex_);
-        const auto it = active_joints_.find(id_);
-        if (it != active_joints_.end()) {
-            selection = it->second;
-        }
-    }
-
-    if (selection.empty()) {
-        CMVR_LOG(WARNING) << "[MotorManager] No active joints selected for motor manager " << id_
-                          << ", motor group " << group_name << " will not initialize motors.";
-        return false;
-    }
-
-    const auto group_it = selection.find(group_name);
-    if (group_it == selection.end() || group_it->second.empty()) {
-        return false;
-    }
-
-    for (const auto& motor_cfg : source) {
-        if (group_it->second.count(motor_cfg.joint_name()) > 0) {
-            selected.push_back(motor_cfg);
-        }
-    }
-
-    if (selected.size() != group_it->second.size()) {
-        std::unordered_set<std::string> found;
-        for (const auto& motor_cfg : selected) {
-            found.insert(motor_cfg.joint_name());
-        }
-        for (const auto& joint_name : group_it->second) {
-            if (found.count(joint_name) == 0) {
-                CMVR_LOG(ERROR) << "[MotorManager] active joint '" << joint_name
-                                << "' not found in motor group '" << group_name << "'";
-                return false;
-            }
-        }
-    }
-    return !selected.empty();
-}
-
 bool MotorManager::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
                                               std::vector<config::MotorConfigItem>& selected) const
 {
@@ -398,8 +443,19 @@ std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
             return std::make_shared<CanMotorBusRuntime>();
         case config::MOTOR_BUS_MUJOCO:
             return std::make_shared<MujocoMotorBusRuntime>();
-        case config::MOTOR_BUS_ETHERCAT:
+        case config::MOTOR_BUS_ETHERCAT: {
-            return std::make_shared<EthercatMotorBusRuntime>();
+            auto runtime = std::make_shared<EthercatMotorBusRuntime>();
+            if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
+                group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
+                runtime->setPdoMapping(createEyouCia402PdoMapping());
+                return runtime;
+            }
+            CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
+                            << config::MotorVendor_Name(group_cfg.vendor())
+                            << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+                            << ", group=" << group_cfg.id();
+            return nullptr;
+        }
         default:
             CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
                             << config::MotorBusType_Name(group_cfg.bus_type())
@@ -464,9 +520,8 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createCanMotors_(
     motors.reserve(motor_cfgs.size());
     for (const auto& cfg : motor_cfgs) {
         auto motor = std::make_shared<Ti5Motor>(cfg);
-        motor->setProtocol(protocol);
+        if (!motor->setProtocol(protocol)) {
-        if (!motor->init()) {
+            CMVR_LOG(ERROR) << "[MotorManager] failed to register TI5 motor protocols: "
-            CMVR_LOG(ERROR) << "[MotorManager] failed to init TI5 motor: "
                             << cfg.joint_name();
             return {};
         }
@@ -534,6 +589,14 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
         CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << group_cfg.id();
         return {};
     }
+    if (group_cfg.vendor() != config::MOTOR_VENDOR_EYOU ||
+        group_cfg.protocol() != config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
+        CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
+                        << config::MotorVendor_Name(group_cfg.vendor())
+                        << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+                        << ", group=" << group_cfg.id();
+        return {};
+    }
     for (const auto& motor_cfg : motor_cfgs) {
         if (!ethercat_bus_runtime->slaveForMotor(motor_cfg.id())) {
             CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT slave config for motor id "
@@ -542,11 +605,22 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
         }
     }
 
-    CMVR_LOG(ERROR) << "[MotorManager] EtherCAT motor creation is not implemented: vendor="
+    auto protocol = std::make_shared<Cia402Protocol>(
-                    << config::MotorVendor_Name(group_cfg.vendor())
+        ethercat_bus_runtime, group_cfg.ethercat().cia402());
-                    << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+
-                    << ", group=" << group_cfg.id();
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
-    return {};
+    motors.reserve(motor_cfgs.size());
+    for (const auto& cfg : motor_cfgs) {
+        auto motor = std::make_shared<EyouMotor>(
+            cfg, protocol, std::make_unique<EyouMotorAdapter>(ethercat_bus_runtime));
+        if (!motor->init()) {
+            CMVR_LOG(ERROR) << "[MotorManager] failed to init EYOU EtherCAT motor: "
+                            << cfg.joint_name();
+            return {};
+        }
+        motors.push_back(std::move(motor));
+    }
+    return motors;
 }
 
 } // namespace cmvr::device

cmvr-es/devices/motor/motor_protocol_interface.h

@@ -15,7 +15,8 @@ namespace cmvr {
         public:
             enum class CommProto : uint8_t {
                 CANOPEN = 1,
-                CUSTOM = 2
+                ETHERCAT = 2,
+                CUSTOM = 3
             };
             virtual ~MotorProtocolInterface() = default;
 
@@ -26,22 +27,42 @@ namespace cmvr {
              */
             virtual bool initNode(uint8_t node_id) = 0;
 
-            virtual void setQ(uint8_t node_id, double angle_rad) = 0;
+            virtual bool setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
-            virtual void setTarget(uint8_t node_id, double angle_rad,double vel) = 0;
-            virtual void setTarget(uint8_t node_id,double vel) = 0;
-            virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
             virtual msgs::RunMode getMode(uint8_t node_id) = 0;
             virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
             virtual void setLimitQd(uint8_t node_id,double qd) = 0;
             virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0;
             virtual bool calibrateZeroQ(uint8_t node_id) = 0;
             virtual bool reachedTargetQ(uint8_t node_id) = 0;
-            virtual void setQd(uint8_t node_id, double qd) = 0;
+            // target_q: rad, max_qd: rad/s, max_qdd: rad/s^2.
-            virtual void setQdd(uint8_t node_id,double qdd) = 0;
+            // Profile Position 写入目标位置和轮廓速度/加速度，并触发一次新目标。
-            // virtual void setVelocity(uint8_t node_id, double velocity) = 0;
+            virtual bool commandProfilePosition(uint8_t node_id,
-            // virtual void clearError(uint8_t node_id) = 0;
+                                                double target_q,
-            virtual void brake(uint8_t node_id) = 0;
+                                                double max_qd,
-            virtual void torqueOff(uint8_t node_id) = 0;
+                                                double max_qdd) = 0;
+            // target_qd: rad/s, max_qdd: rad/s^2.
+            // Profile Velocity 写入目标速度和轮廓加速度。
+            virtual bool commandProfileVelocity(uint8_t node_id,
+                                                double target_qd,
+                                                double max_qdd) = 0;
+            // target_q: rad, target_qd: rad/s.
+            // Cyclic Position 周期写入目标位置和目标速度。
+            virtual bool commandCyclicPosition(uint8_t node_id,
+                                                double target_q,
+                                               double target_qd) = 0;
+            // target_qd: rad/s.
+            // Cyclic Velocity 周期写入目标速度。
+            virtual bool commandCyclicVelocity(uint8_t node_id,
+                                               double target_qd) = 0;
+            // target_tau: N*m.
+            virtual bool commandCyclicTorque(uint8_t node_id, double target_tau) = 0;
+            virtual void setMotorConversion(uint8_t node_id,
+                                            double encoder_counts_per_rev,
+                                            double gear_ratio) = 0;
+            virtual bool torqueOn(uint8_t node_id) = 0;
+            virtual bool torqueOff(uint8_t node_id) = 0;
+            virtual bool brakeRelease(uint8_t node_id) = 0;
+            virtual bool quickStop(uint8_t node_id) = 0;
 
             virtual double getQ(uint8_t node_id) = 0;
             virtual double getQd(uint8_t node_id) = 0;

cmvr-es/manager/device_manager/include/device_manager.h

@@ -8,7 +8,6 @@
 #include <list>
 #include <mutex>
 #include <string>
-#include <unordered_set>
 #include <unordered_map>
 #include "device_factory.h"
 #include "cmvr/config/device_manager_config/device_manager_config.pb.h"
@@ -49,7 +48,6 @@ namespace cmvr::device {
 
         explicit DeviceManager(const config::DeviceManagerConfig &cfg);
         void log_device_plan_() const;
-        void pre_scan_robot_arm_dependencies_() const;
         void init_devices_();
         void configure_mujoco_viewer_pip_();
     };

cmvr-es/manager/device_manager/src/device_factory.cpp

@@ -226,22 +226,41 @@ DeviceFactory::DeviceFactory()
                     });
 
     registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
-	                    [](const auto& entry) {
+                    [](const auto& entry) {
-	                        if (entry.id().empty()) {
+                        if (entry.id().empty()) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required";
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id is required";
-	                            return DeviceRecord{};
+                            return DeviceRecord{};
-	                        }
+                        }
-	                        if (entry.config_file().empty()) {
+                        if (entry.config_file().empty()) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id();
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for motor group ID: " << entry.id();
-	                            return DeviceRecord{};
+                            return DeviceRecord{};
-	                        }
+                        }
-	                        config::MotorRootConfig root_cfg;
+                        config::MotorRootConfig root_cfg;
-	                        if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
+                        if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail";
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail";
-	                            return DeviceRecord{};
+                            return DeviceRecord{};
-	                        }
+                        }
+
+                        const config::MotorGroupConfig* selected_group = nullptr;
+                        for (const auto& group_cfg : root_cfg.motor().motor_groups()) {
+                            if (group_cfg.id() != entry.id()) {
+                                continue;
+                            }
+                            if (selected_group != nullptr) {
+                                CMVR_LOG(ERROR) << "[DeviceFactory]: Duplicate motor group id '"
+                                                << entry.id() << "' in config: " << entry.config_file();
+                                return DeviceRecord{};
+                            }
+                            selected_group = &group_cfg;
+                        }
+                        if (selected_group == nullptr) {
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id '" << entry.id()
+                                            << "' not found in config: " << entry.config_file();
+                            return DeviceRecord{};
+                        }
                         CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success";
-                        auto device = std::make_shared<MotorManager>(entry.id(), root_cfg.motor());
+                        auto device = std::make_shared<MotorManager>(
+                            entry.id(), root_cfg.motor(), entry.id());
                         DeviceRecord record;
                         record.id = entry.id();
                         record.kind = device->kind();

cmvr-es/manager/device_manager/src/device_manager.cpp

@@ -18,8 +18,6 @@
 #include "devices/speaker/abstract_speaker.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 #include "common/config/config_files.h"
-#include "cmvr/config/arm_config/arm_config.pb.h"
-#include "cmvr/config/motor_config/motor_config.pb.h"
 
 using namespace std;
 using namespace cmvr::device;
@@ -60,17 +58,6 @@ const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType
     }
 }
 
-bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group,
-                        const std::string& joint_name)
-{
-    for (const auto& motor : motor_group.motors().motors()) {
-        if (motor.joint_name() == joint_name) {
-            return true;
-        }
-    }
-    return false;
-}
-
 } // namespace
 
 template std::shared_ptr<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
@@ -96,7 +83,6 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
     dev_factory_ = std::make_unique<DeviceFactory>();
     logSection("Device Plan");
     log_device_plan_();
-    pre_scan_robot_arm_dependencies_();
     logSection("Initialize Devices");
     init_devices_();
     configure_mujoco_viewer_pip_();
@@ -121,7 +107,6 @@ DeviceManager& DeviceManager::getInstance() {
 void DeviceManager::destroyInstance() {
     std::lock_guard lock(init_mutex_);
     instance_.reset();
-    MotorManager::clearActiveJoints();
 }
 
 void DeviceManager::start(){
@@ -254,145 +239,6 @@ void DeviceManager::log_device_plan_() const
     CMVR_LOG(INFO) << "[DeviceManager]: Device plan end";
 }
 
-void DeviceManager::pre_scan_robot_arm_dependencies_() const
-{
-    using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
-    std::unordered_map<std::string, GroupJointSelection> selections;
-    std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
-
-    for (const auto& entry : cfg_.devices()) {
-        if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) {
-            continue;
-        }
-        if (entry.id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
-            return;
-        }
-        if (entry.config_file().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
-            return;
-        }
-
-        config::MotorRootConfig root_cfg;
-        if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file();
-            return;
-        }
-        if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
-                            << "' does not match config id '" << root_cfg.motor().id() << "'";
-            return;
-        }
-        motor_roots.emplace(entry.id(), std::move(root_cfg));
-    }
-
-    for (const auto& entry : cfg_.devices()) {
-        if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM) {
-            continue;
-        }
-        if (entry.id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty";
-            return;
-        }
-        if (entry.config_file().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id();
-            return;
-        }
-
-        config::ArmRootConfig root_cfg;
-        if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file();
-            return;
-        }
-
-        const config::RobotArmConfig* arm_cfg = nullptr;
-        for (const auto& candidate : root_cfg.arm().robot_arms()) {
-            if (candidate.id() == entry.id()) {
-                arm_cfg = &candidate;
-                break;
-            }
-        }
-        if (!arm_cfg) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id()
-                            << "' not found in config: " << entry.config_file();
-            return;
-        }
-
-        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) {
-            continue;
-        }
-        if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id();
-            return;
-        }
-
-        const auto& motor_config = arm_cfg->motor();
-        if (motor_config.motor_system_id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id();
-            return;
-        }
-        if (motor_config.motor_group_ids_size() == 0) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id();
-            return;
-        }
-        if (motor_config.joint_names_size() == 0) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id();
-            return;
-        }
-
-        const auto motor_root_it = motor_roots.find(motor_config.motor_system_id());
-        if (motor_root_it == motor_roots.end()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
-                            << "' depends on disabled or missing MotorManager: "
-                            << motor_config.motor_system_id();
-            return;
-        }
-
-        std::unordered_set<std::string> allowed_groups;
-        allowed_groups.reserve(static_cast<size_t>(motor_config.motor_group_ids_size()));
-        for (const auto& group_id : motor_config.motor_group_ids()) {
-            if (group_id.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id();
-                return;
-            }
-            allowed_groups.insert(group_id);
-        }
-
-        auto& group_selection = selections[motor_config.motor_system_id()];
-        for (const auto& joint_name : motor_config.joint_names()) {
-            if (joint_name.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id();
-                return;
-            }
-
-            std::string matched_group;
-            for (const auto& motor_group : motor_root_it->second.motor().motor_groups()) {
-                const auto& group_id = motor_group.id();
-                if (allowed_groups.count(group_id) == 0) {
-                    continue;
-                }
-                if (motorGroupHasJoint(motor_group, joint_name)) {
-                    matched_group = group_id;
-                    break;
-                }
-            }
-
-            if (matched_group.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
-                                << "' joint '" << joint_name
-                                << "' not found in configured motor_group_ids";
-                return;
-            }
-            group_selection[matched_group].insert(joint_name);
-        }
-    }
-
-    MotorManager::clearActiveJoints();
-    for (auto& [motor_system_id, group_selection] : selections) {
-        MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
-    }
-}
-
 void DeviceManager::init_devices_() {
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable()) {
@@ -462,12 +308,14 @@ void DeviceManager::configure_mujoco_viewer_pip_()
 
             auto viewer = getDevice<cmvr::MujocoViewerDevice>(viewer_id);
             if (viewer && viewer->setPiPCameraConfig(camera_config)) {
-                camera->setFetchRgbdFn([viewer](std::vector<unsigned char>& rgb,
+                const std::string camera_name = camera_config.camera_name();
-                                                std::vector<float>& depth,
+                camera->setFetchRgbdFn([viewer, camera_name](std::vector<unsigned char>& rgb,
-                                                int& width,
+                                                             std::vector<float>& depth,
-                                                int& height,
+                                                             int& width,
-                                                uint64_t& frame_id) {
+                                                             int& height,
-                    return viewer->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
+                                                             uint64_t& frame_id) {
+                    return viewer->getPiPCameraRGBD(
+                        camera_name, rgb, depth, width, height, frame_id);
                 });
                 break;
             }

cmvr-es/manager/task_manager/src/task_manager.cpp

@@ -36,6 +36,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type)
             return "TASK_TYPE_TOUCH_SCREEN";
         case config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER:
             return "TASK_TYPE_GRPC_SERVER";
+        case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION:
+            return "TASK_TYPE_SELF_COLLISION";
         case config::TaskConfigEntry::TASK_TYPE_UNKNOWN:
         default:
             return "TASK_TYPE_UNKNOWN";

cmvr-es/service/grpc/src/grpc_camera_service.cpp

@@ -6,6 +6,7 @@
 #include "../include/grpc_camera_service.h"
 
 #include <limits>
+#include <thread>
 
 using namespace std;
 using namespace cmvr::service;
@@ -259,7 +260,8 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
 
-        // color image
+        // color image is serialized in the existing OpenCV BGR byte order;
+        // clients convert it once when constructing an RGB image.
         if (color_image.type() == CV_8UC3) {
             response->mutable_color_frame()->set_type(api::FrameData::U8C3);
         }
@@ -392,11 +394,13 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
             dev->getEncodedFrame(frame_data,index);
             if (!frame_data.rgbFrame.empty()) {
                 response.mutable_header()->set_success(true);
+                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
                 response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
                 response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
-                response.mutable_depth_frame()->set_codec(frame_data.codec);
+                response.mutable_depth_frame()->set_codec("none");
-                response.mutable_depth_frame()->set_width(frame_data.width);
+                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
-                response.mutable_depth_frame()->set_height(frame_data.height);
+                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
 
                 response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
                 response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
@@ -468,11 +472,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
                 response.mutable_color_frame()->set_width(frame_data.width);
                 response.mutable_color_frame()->set_height(frame_data.height);
 
+                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
                 response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
                 response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
-                response.mutable_depth_frame()->set_codec(frame_data.codec);
+                response.mutable_depth_frame()->set_codec("none");
-                response.mutable_depth_frame()->set_width(frame_data.width);
+                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
-                response.mutable_depth_frame()->set_height(frame_data.height);
+                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
 
                 response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
                 response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);

cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt

@@ -35,4 +35,9 @@ target_link_libraries(mujoco_viewer_test
         gtest_main
         cmvr_es::mujoco_viewer
         cmvr_es::mujoco_world
+        cmvr_es::device_manager
+        cmvr_es::device::camera
+        cmvr_es::device::motor_manager
+        cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device::motor_robot_arm
 )

cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h

@@ -5,8 +5,11 @@
 #pragma once
 
 #include <atomic>
+#include <cstdint>
+#include <deque>
 #include <memory>
 #include <mutex>
+#include <string>
 #include <thread>
 #include <vector>
 
@@ -52,6 +55,14 @@ namespace cmvr {
                              int display_height,
                              int render_width,
                              int render_height);
+        // Append another fixed camera view to the same viewer window.
+        void addPiPCamera(const char *camera_name,
+                          int left,
+                          int bottom,
+                          int display_width,
+                          int display_height,
+                          int render_width,
+                          int render_height);
         void disablePiPCamera();
         // 获取 PiP 相机 RGB+Depth（Depth 已线性化为米）
         // depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口）
@@ -60,9 +71,16 @@ namespace cmvr {
                               int &width,
                               int &height,
                               uint64_t &frame_id) const;
+        bool getPiPCameraRGBD(const std::string &camera_name,
+                              std::vector<unsigned char> &rgb,
+                              std::vector<float> &depth,
+                              int &width,
+                              int &height,
+                              uint64_t &frame_id) const;
 
         // 只拿 frame_id，便于 physics 线程判断是否新帧
         uint64_t getPiPCameraFrameId() const;
+        uint64_t getPiPCameraFrameId(const std::string &camera_name) const;
 
     public:
         void setupCamera(double distance = 3.0,
@@ -79,9 +97,34 @@ namespace cmvr {
         void printCameraState() const;
 
     private:
+        struct PiPCameraState {
+            std::string name;
+            int camera_id = -1;
+            int width = 320;
+            int height = 240;
+            int render_width = 320;
+            int render_height = 240;
+            bool render_size_warning_logged = false;
+            int margin = 10;
+            bool custom_pos = false;
+            int left = 0;
+            int bottom = 0;
+            mjvCamera camera{};
+            mjvScene scene{};
+            bool scene_inited = false;
+            mjModel *scene_model = nullptr;
+            std::vector<unsigned char> rgb;
+            std::vector<float> depth;
+            int rgb_width = 0;
+            int rgb_height = 0;
+            bool rgb_valid = false;
+            uint64_t frame_id = 0;
+        };
+
         void renderPiP();
         void initSim();
         void syncThreadFunc();
+        void clearPiPCameras();
 
     private:
         std::shared_ptr<simulate::MujocoWorld> world_;
@@ -93,29 +136,10 @@ namespace cmvr {
         std::unique_ptr<mujoco::Simulate> sim_;
         std::thread sync_thread_;
 
-        bool pip_enabled_ = false;
+        std::deque<PiPCameraState> pip_cameras_;
-        std::string pip_camera_name_;
+        mjData *pip_render_data_ = nullptr;
-        int pip_camera_id_ = -1;
+        mjModel *pip_render_data_model_ = nullptr;
-        int pip_width_ = 320;
-        int pip_height_ = 240;
-        int pip_render_width_ = 320;
-        int pip_render_height_ = 240;
-        bool pip_render_size_warning_logged_ = false;
-        int pip_margin_ = 10;
-        bool pip_custom_pos_ = false;
-        int pip_left_ = 0;
-        int pip_bottom_ = 0;
-        mjvCamera pip_cam_;
-        mjvScene pip_scene_;
-        bool pip_scene_inited_ = false;
-        mjModel *pip_scene_model_ = nullptr;
         mutable std::mutex pip_rgb_mtx_;
-        std::vector<unsigned char> pip_rgb_;
-        std::vector<float> pip_depth_;     // 新增：z-buffer
-        int pip_rgb_width_ = 0;
-        int pip_rgb_height_ = 0;
-        bool pip_rgb_valid_ = false;
-        uint64_t pip_frame_id_ = 0;        // 新增：帧序号
 
     };
 
@@ -137,15 +161,20 @@ namespace cmvr {
                               int& width,
                               int& height,
                               uint64_t& frame_id) const;
+        bool getPiPCameraRGBD(const std::string& camera_name,
+                              std::vector<unsigned char>& rgb,
+                              std::vector<float>& depth,
+                              int& width,
+                              int& height,
+                              uint64_t& frame_id) const;
 
     private:
         config::MujocoViewerConfig config_;
-        config::MujocoCameraConfig pip_camera_config_;
+        std::vector<config::MujocoCameraConfig> pip_camera_configs_;
         std::shared_ptr<simulate::MujocoWorld> world_;
         std::unique_ptr<MuJocoViewer> viewer_;
         std::thread viewer_thread_;
         mutable std::mutex mtx_;
-        bool has_pip_camera_config_ = false;
         bool running_ = false;
         bool stop_requested_ = false;
     };

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp

@@ -53,8 +53,6 @@ namespace cmvr {
         mjv_defaultCamera(&cam_);
         mjv_defaultOption(&opt_);
         mjv_defaultPerturb(&pert_);
-        mjv_defaultCamera(&pip_cam_);
-        mjv_defaultScene(&pip_scene_);
 
         auto platform_ui = std::make_unique<PiPGlfwAdapter>(this);
         sim_ = std::make_unique<mj::Simulate>(
@@ -72,9 +70,11 @@ namespace cmvr {
             sync_thread_.join();
         }
 
-        if (pip_scene_inited_) {
+        clearPiPCameras();
-            mjv_freeScene(&pip_scene_);
+        if (pip_render_data_ != nullptr) {
-            pip_scene_inited_ = false;
+            mj_deleteData(pip_render_data_);
+            pip_render_data_ = nullptr;
+            pip_render_data_model_ = nullptr;
         }
     }
 
@@ -115,14 +115,12 @@ namespace cmvr {
     }
 
     void MuJocoViewer::enablePiPCamera(const char *camera_name) {
-        pip_enabled_ = true;
+        clearPiPCameras();
-        pip_camera_name_ = camera_name ? camera_name : "";
+        PiPCameraState state;
-        pip_camera_id_ = -1;
+        state.name = camera_name ? camera_name : "";
-        pip_width_ = 320;
+        mjv_defaultCamera(&state.camera);
-        pip_height_ = 240;
+        mjv_defaultScene(&state.scene);
-        pip_render_width_ = pip_width_;
+        pip_cameras_.push_back(std::move(state));
-        pip_render_height_ = pip_height_;
-        pip_custom_pos_ = false;
     }
 
     void MuJocoViewer::enablePiPCamera(const char *camera_name,
@@ -140,183 +138,241 @@ namespace cmvr {
                                        int display_height,
                                        int render_width,
                                        int render_height) {
-        pip_enabled_ = true;
+        clearPiPCameras();
-        pip_camera_name_ = camera_name ? camera_name : "";
+        addPiPCamera(camera_name,
-        pip_camera_id_ = -1;
+                     left,
-        pip_left_ = left;
+                     bottom,
-        pip_bottom_ = bottom;
+                     display_width,
-        pip_width_ = display_width > 0 ? display_width : 320;
+                     display_height,
-        pip_height_ = display_height > 0 ? display_height : 240;
+                     render_width,
-        pip_render_width_ = render_width > 0 ? render_width : pip_width_;
+                     render_height);
-        pip_render_height_ = render_height > 0 ? render_height : pip_height_;
+    }
-        pip_custom_pos_ = true;
+
+    void MuJocoViewer::addPiPCamera(const char *camera_name,
+                                    int left,
+                                    int bottom,
+                                    int display_width,
+                                    int display_height,
+                                    int render_width,
+                                    int render_height) {
+        PiPCameraState state;
+        state.name = camera_name ? camera_name : "";
+        state.left = left;
+        state.bottom = bottom;
+        state.width = display_width > 0 ? display_width : 320;
+        state.height = display_height > 0 ? display_height : 240;
+        state.render_width = render_width > 0 ? render_width : state.width;
+        state.render_height = render_height > 0 ? render_height : state.height;
+        state.custom_pos = true;
+        mjv_defaultCamera(&state.camera);
+        mjv_defaultScene(&state.scene);
+        pip_cameras_.push_back(std::move(state));
     }
 
     void MuJocoViewer::disablePiPCamera() {
-        pip_enabled_ = false;
+        clearPiPCameras();
+    }
+
+    void MuJocoViewer::clearPiPCameras() {
+        for (auto &pip : pip_cameras_) {
+            if (pip.scene_inited) {
+                mjv_freeScene(&pip.scene);
+                pip.scene_inited = false;
+                pip.scene_model = nullptr;
+            }
+        }
+        pip_cameras_.clear();
     }
 
 
 
     void MuJocoViewer::renderPiP() {
-        if (!pip_enabled_ || !sim_) return;
+        if (!sim_ || pip_cameras_.empty()) return;
-        if (pip_camera_name_.empty()) return;
 
         mjModel* render_model = sim_->m_passive_ ? sim_->m_passive_ : sim_->m_;
         mjData* render_data = sim_->d_passive_ ? sim_->d_passive_ : sim_->d_;
         if (!render_model || !render_data) return;
 
-        if (pip_camera_id_ < 0) {
-            pip_camera_id_ = mj_name2id(render_model, mjOBJ_CAMERA, pip_camera_name_.c_str());
-            if (pip_camera_id_ < 0) {
-                return;
-            }
-        }
-
         auto [fb_width, fb_height] = sim_->platform_ui->GetFramebufferSize();
         if (fb_width <= 0 || fb_height <= 0) return;
 
-        int left = 0;
+        // Copy one visualization snapshot for all PiP cameras. Keep the
-        int bottom = 0;
+        // simulation lock out of scene updates, GPU rendering, and readback.
-        int width = 0;
+        {
-        int height = 0;
+            std::unique_lock<std::recursive_mutex> lock(sim_->mtx, std::try_to_lock);
-
+            if (lock.owns_lock()) {
-        if (pip_custom_pos_) {
+                if (pip_render_data_model_ != render_model) {
-            width = std::min(pip_width_, fb_width);
+                    if (pip_render_data_ != nullptr) {
-            height = std::min(pip_height_, fb_height);
+                        mj_deleteData(pip_render_data_);
-            const int max_left = fb_width - width;
+                        pip_render_data_ = nullptr;
-            const int max_bottom = fb_height - height;
+                    }
-            left = pip_left_ >= 0
+                    pip_render_data_ = mj_makeData(render_model);
-                       ? std::max(0, std::min(pip_left_, max_left))
+                    pip_render_data_model_ = render_model;
-                       : std::max(0, std::min(fb_width - width + pip_left_, max_left));
+                }
-            bottom = pip_bottom_ >= 0
+                if (pip_render_data_ != nullptr) {
-                         ? std::max(0, std::min(pip_bottom_, max_bottom))
+                    mjv_copyData(pip_render_data_, render_model, render_data);
-                         : std::max(0, std::min(fb_height - height + pip_bottom_, max_bottom));
+                }
-        } else {
-            width = std::min(pip_width_, fb_width - 2 * pip_margin_);
-            height = std::min(pip_height_, fb_height - 2 * pip_margin_);
-            left = fb_width - pip_margin_ - width;
-            bottom = pip_margin_;
-        }
-
-        if (width <= 0 || height <= 0) return;
-
-        mjrRect display_rect;
-        display_rect.width = width;
-        display_rect.height = height;
-        display_rect.left = left;
-        display_rect.bottom = bottom;
-
-        const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
-
-        if (!pip_scene_inited_ || pip_scene_model_ != render_model) {
-            if (pip_scene_inited_) {
-                mjv_freeScene(&pip_scene_);
             }
-            mjv_makeScene(render_model, &pip_scene_, kPiPMaxGeom);
-            pip_scene_inited_ = true;
-            pip_scene_model_ = render_model;
         }
 
-        pip_cam_.type = mjCAMERA_FIXED;
+        // If the sync thread owns the lock, use the last complete snapshot.
-        pip_cam_.fixedcamid = pip_camera_id_;
+        if (pip_render_data_ == nullptr || pip_render_data_model_ != render_model) {
-        pip_cam_.trackbodyid = -1;
+            return;
-
+        }
-        mjv_updateScene(render_model, render_data, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
 
         auto& context = sim_->platform_ui->mjr_context();
-        const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
+        for (size_t pip_index = 0; pip_index < pip_cameras_.size(); ++pip_index) {
-        const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
+            auto& pip = pip_cameras_[pip_index];
-        const int render_width = std::min(std::max(1, pip_render_width_), offscreen_width);
+            if (pip.name.empty()) continue;
-        const int render_height = std::min(std::max(1, pip_render_height_), offscreen_height);
-        if (!pip_render_size_warning_logged_ &&
-            (render_width != pip_render_width_ || render_height != pip_render_height_)) {
-            CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
-                              << ", requested=" << pip_render_width_ << "x" << pip_render_height_
-                              << ", actual=" << render_width << "x" << render_height
-                              << ", offscreen=" << offscreen_width << "x" << offscreen_height;
-            pip_render_size_warning_logged_ = true;
-        }
-        const bool use_offscreen = render_width != display_rect.width || render_height != display_rect.height;
 
-        mjrRect render_rect;
+            if (pip.camera_id < 0) {
-        render_rect.left = 0;
+                pip.camera_id = mj_name2id(render_model, mjOBJ_CAMERA, pip.name.c_str());
-        render_rect.bottom = 0;
+                if (pip.camera_id < 0) {
-        render_rect.width = render_width;
+                    CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera not found: " << pip.name;
-        render_rect.height = render_height;
+                    continue;
+                }
+            }
 
-        bool rendered_offscreen = false;
+            int left = 0;
-        if (use_offscreen) {
+            int bottom = 0;
-            mjr_setBuffer(mjFB_OFFSCREEN, &context);
+            int width = 0;
-            if (context.currentBuffer == mjFB_OFFSCREEN) {
+            int height = 0;
-                mjr_render(render_rect, &pip_scene_, &context);
+            if (pip.custom_pos) {
-                rendered_offscreen = true;
+                width = std::min(pip.width, fb_width);
+                height = std::min(pip.height, fb_height);
+                const int max_left = fb_width - width;
+                const int max_bottom = fb_height - height;
+                left = pip.left >= 0
+                           ? std::max(0, std::min(pip.left, max_left))
+                           : std::max(0, std::min(fb_width - width + pip.left, max_left));
+                bottom = pip.bottom >= 0
+                             ? std::max(0, std::min(pip.bottom, max_bottom))
+                             : std::max(0, std::min(fb_height - height + pip.bottom, max_bottom));
             } else {
-                mjr_render(display_rect, &pip_scene_, &context);
+                width = std::min(pip.width, fb_width - 2 * pip.margin);
+                height = std::min(pip.height, fb_height - 2 * pip.margin);
+                left = fb_width - pip.margin - width;
+                bottom = pip.margin;
+            }
+            if (width <= 0 || height <= 0) continue;
+
+            mjrRect display_rect;
+            display_rect.width = width;
+            display_rect.height = height;
+            display_rect.left = left;
+            display_rect.bottom = bottom;
+
+            if (!pip.scene_inited || pip.scene_model != render_model) {
+                if (pip.scene_inited) {
+                    mjv_freeScene(&pip.scene);
+                }
+                mjv_makeScene(render_model, &pip.scene, kPiPMaxGeom);
+                pip.scene_inited = true;
+                pip.scene_model = render_model;
+            }
+
+            pip.camera.type = mjCAMERA_FIXED;
+            pip.camera.fixedcamid = pip.camera_id;
+            pip.camera.trackbodyid = -1;
+            mjv_updateScene(render_model,
+                            pip_render_data_,
+                            &opt_,
+                            &pert_,
+                            &pip.camera,
+                            mjCAT_ALL,
+                            &pip.scene);
+
+            const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
+            const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
+            const int render_width = std::min(std::max(1, pip.render_width), offscreen_width);
+            const int render_height = std::min(std::max(1, pip.render_height), offscreen_height);
+            if (!pip.render_size_warning_logged &&
+                (render_width != pip.render_width || render_height != pip.render_height)) {
+                CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
+                                  << ", camera=" << pip.name
+                                  << ", requested=" << pip.render_width << "x" << pip.render_height
+                                  << ", actual=" << render_width << "x" << render_height
+                                  << ", offscreen=" << offscreen_width << "x" << offscreen_height;
+                pip.render_size_warning_logged = true;
+            }
+            const bool use_offscreen =
+                render_width != display_rect.width || render_height != display_rect.height;
+
+            mjrRect render_rect;
+            render_rect.left = 0;
+            render_rect.bottom = 0;
+            render_rect.width = render_width;
+            render_rect.height = render_height;
+
+            bool rendered_offscreen = false;
+            if (use_offscreen) {
+                mjr_setBuffer(mjFB_OFFSCREEN, &context);
+                if (context.currentBuffer == mjFB_OFFSCREEN) {
+                    mjr_render(render_rect, &pip.scene, &context);
+                    rendered_offscreen = true;
+                } else {
+                    mjr_render(display_rect, &pip.scene, &context);
+                    render_rect = display_rect;
+                }
+            } else {
+                mjr_render(display_rect, &pip.scene, &context);
                 render_rect = display_rect;
             }
-        } else {
-            mjr_render(display_rect, &pip_scene_, &context);
-            render_rect = display_rect;
-        }
 
-        {
+            {
-            std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+                std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
-            const int w = render_rect.width;
+                const int w = render_rect.width;
-            const int h = render_rect.height;
+                const int h = render_rect.height;
-            if (w > 0 && h > 0) {
+                if (w > 0 && h > 0) {
-                pip_rgb_.resize(static_cast<size_t>(3 * w * h));
+                    pip.rgb.resize(static_cast<size_t>(3 * w * h));
-                pip_depth_.resize(static_cast<size_t>(w * h));
+                    pip.depth.resize(static_cast<size_t>(w * h));
+                    mjr_readPixels(pip.rgb.data(), pip.depth.data(), render_rect, &context);
 
-                // 同时读 RGB 和 depth(z-buffer 0..1)
+                    // OpenGL's pixel origin is bottom-left; expose top-left images.
-                mjr_readPixels(pip_rgb_.data(), pip_depth_.data(),
+                    for (int r = 0; r < h / 2; ++r) {
-                               render_rect, &context);
+                        unsigned char *top_row = pip.rgb.data() + 3 * w * r;
-                if (rendered_offscreen) {
+                        unsigned char *bottom_row = pip.rgb.data() + 3 * w * (h - 1 - r);
-                    mjr_setBuffer(mjFB_WINDOW, &context);
+                        std::swap_ranges(top_row, top_row + 3 * w, bottom_row);
-                    mjr_render(display_rect, &pip_scene_, &context);
-                }
 
-                // OpenGL 像素原点在左下，需要竖直翻转 RGB 和 depth
+                        float *top_d = pip.depth.data() + w * r;
-                for (int r = 0; r < h / 2; ++r) {
+                        float *bot_d = pip.depth.data() + w * (h - 1 - r);
-                    // flip rgb row
+                        std::swap_ranges(top_d, top_d + w, bot_d);
-                    unsigned char *top_row = pip_rgb_.data() + 3 * w * r;
-                    unsigned char *bottom_row = pip_rgb_.data() + 3 * w * (h - 1 - r);
-                    std::swap_ranges(top_row, top_row + 3 * w, bottom_row);
-
-                    // flip depth row
-                    float *top_d = pip_depth_.data() + w * r;
-                    float *bot_d = pip_depth_.data() + w * (h - 1 - r);
-                    std::swap_ranges(top_d, top_d + w, bot_d);
-                }
-
-                // 将 OpenGL depth buffer(0..1) 线性化为相机前向距离（米）。
-                const double znear = static_cast<double>(render_model->vis.map.znear) *
-                                     static_cast<double>(render_model->stat.extent);
-                const double zfar  = static_cast<double>(render_model->vis.map.zfar) *
-                                     static_cast<double>(render_model->stat.extent);
-                if (znear > 0.0 && zfar > znear) {
-                    const double two_nf = 2.0 * znear * zfar;
-                    const double f_plus_n = zfar + znear;
-                    const double f_minus_n = zfar - znear;
-                    for (float &d : pip_depth_) {
-                        if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) {
-                            d = std::numeric_limits<float>::infinity();
-                            continue;
-                        }
-                        const double z_ndc = 2.0 * static_cast<double>(d) - 1.0; // [-1,1]
-                        const double denom = f_plus_n - z_ndc * f_minus_n;
-                        if (denom <= 1e-12) {
-                            d = std::numeric_limits<float>::infinity();
-                            continue;
-                        }
-                        d = static_cast<float>(two_nf / denom);
                     }
-                }
 
-                pip_rgb_width_ = w;
+                    // Linearize the OpenGL depth buffer to camera-forward meters.
-                pip_rgb_height_ = h;
+                    const double znear = static_cast<double>(render_model->vis.map.znear) *
-                pip_rgb_valid_ = true;
+                                         static_cast<double>(render_model->stat.extent);
-                ++pip_frame_id_;   // 新帧
+                    const double zfar = static_cast<double>(render_model->vis.map.zfar) *
+                                        static_cast<double>(render_model->stat.extent);
+                    if (znear > 0.0 && zfar > znear) {
+                        const double two_nf = 2.0 * znear * zfar;
+                        const double f_plus_n = zfar + znear;
+                        const double f_minus_n = zfar - znear;
+                        for (float &d : pip.depth) {
+                            if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) {
+                                d = std::numeric_limits<float>::infinity();
+                                continue;
+                            }
+                            const double z_ndc = 2.0 * static_cast<double>(d) - 1.0;
+                            const double denom = f_plus_n - z_ndc * f_minus_n;
+                            if (denom <= 1e-12) {
+                                d = std::numeric_limits<float>::infinity();
+                                continue;
+                            }
+                            d = static_cast<float>(two_nf / denom);
+                        }
+                    }
+
+                    pip.rgb_width = w;
+                    pip.rgb_height = h;
+                    pip.rgb_valid = true;
+                    ++pip.frame_id;
+                }
+            }
+
+            if (rendered_offscreen) {
+                mjr_setBuffer(mjFB_WINDOW, &context);
+                mjr_render(display_rect, &pip.scene, &context);
             }
         }
     }
@@ -331,12 +387,18 @@ namespace cmvr {
             if (!world_->model() || !world_->data()) {
                 mju_error("MuJocoViewer world has null model/data");
             }
-            if (pip_enabled_ && pip_render_width_ > 0 && pip_render_height_ > 0) {
+            int max_pip_render_width = 0;
+            int max_pip_render_height = 0;
+            for (const auto &pip : pip_cameras_) {
+                max_pip_render_width = std::max(max_pip_render_width, pip.render_width);
+                max_pip_render_height = std::max(max_pip_render_height, pip.render_height);
+            }
+            if (!pip_cameras_.empty() && max_pip_render_width > 0 && max_pip_render_height > 0) {
                 mjModel* model = world_->model();
                 const int old_width = model->vis.global.offwidth;
                 const int old_height = model->vis.global.offheight;
-                model->vis.global.offwidth = std::max(model->vis.global.offwidth, pip_render_width_);
+                model->vis.global.offwidth = std::max(model->vis.global.offwidth, max_pip_render_width);
-                model->vis.global.offheight = std::max(model->vis.global.offheight, pip_render_height_);
+                model->vis.global.offheight = std::max(model->vis.global.offheight, max_pip_render_height);
                 if (model->vis.global.offwidth != old_width || model->vis.global.offheight != old_height) {
                     CMVR_LOG(INFO) << "[MuJocoViewer] resize offscreen buffer before context creation"
                                    << ", old=" << old_width << "x" << old_height
@@ -386,7 +448,15 @@ namespace cmvr {
 
     uint64_t MuJocoViewer::getPiPCameraFrameId() const {
         std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
-        return pip_frame_id_;
+        return pip_cameras_.empty() ? 0 : pip_cameras_.front().frame_id;
+    }
+
+    uint64_t MuJocoViewer::getPiPCameraFrameId(const std::string &camera_name) const {
+        std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+        const auto it = std::find_if(
+            pip_cameras_.begin(), pip_cameras_.end(),
+            [&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; });
+        return it == pip_cameras_.end() ? 0 : it->frame_id;
     }
 
     bool MuJocoViewer::getPiPCameraRGBD(std::vector<unsigned char> &rgb,
@@ -395,13 +465,35 @@ namespace cmvr {
                                         int &height,
                                         uint64_t &frame_id) const {
         std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
-        if (!pip_rgb_valid_ || pip_rgb_.empty()) return false;
+        if (pip_cameras_.empty()) return false;
+        const auto &pip = pip_cameras_.front();
+        if (!pip.rgb_valid || pip.rgb.empty()) return false;
 
-        rgb = pip_rgb_;
+        rgb = pip.rgb;
-        depth = pip_depth_;
+        depth = pip.depth;
-        width = pip_rgb_width_;
+        width = pip.rgb_width;
-        height = pip_rgb_height_;
+        height = pip.rgb_height;
-        frame_id = pip_frame_id_;
+        frame_id = pip.frame_id;
+        return true;
+    }
+
+    bool MuJocoViewer::getPiPCameraRGBD(const std::string &camera_name,
+                                        std::vector<unsigned char> &rgb,
+                                        std::vector<float> &depth,
+                                        int &width,
+                                        int &height,
+                                        uint64_t &frame_id) const {
+        std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+        const auto it = std::find_if(
+            pip_cameras_.begin(), pip_cameras_.end(),
+            [&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; });
+        if (it == pip_cameras_.end() || !it->rgb_valid || it->rgb.empty()) return false;
+
+        rgb = it->rgb;
+        depth = it->depth;
+        width = it->rgb_width;
+        height = it->rgb_height;
+        frame_id = it->frame_id;
         return true;
     }
 
@@ -456,6 +548,7 @@ namespace cmvr {
         }
 
         std::shared_ptr<simulate::MujocoWorld> world;
+        std::vector<config::MujocoCameraConfig> pip_camera_configs;
         {
             std::lock_guard<std::mutex> lock(mtx_);
             if (running_) {
@@ -466,6 +559,7 @@ namespace cmvr {
                 return false;
             }
             world = world_;
+            pip_camera_configs = pip_camera_configs_;
             stop_requested_ = false;
             running_ = true;
         }
@@ -486,19 +580,30 @@ namespace cmvr {
                             config_.camera_azimuth(),
                             config_.camera_elevation());
 
-        if (has_pip_camera_config_ && !pip_camera_config_.camera_name().empty()) {
+        bool first_pip_camera = true;
-            const auto& pip = pip_camera_config_.viewer_pip();
+        for (const auto& camera_config : pip_camera_configs) {
-            const auto& render = pip_camera_config_.render();
+            if (camera_config.camera_name().empty()) {
-            if (pip.width() > 0 && pip.height() > 0) {
+                continue;
-                viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str(),
+            }
+            const auto& pip = camera_config.viewer_pip();
+            const auto& render = camera_config.render();
+            if (first_pip_camera) {
+                viewer->enablePiPCamera(camera_config.camera_name().c_str(),
                                         pip.left(),
                                         pip.bottom(),
                                         pip.width(),
                                         pip.height(),
                                         render.width(),
                                         render.height());
+                first_pip_camera = false;
             } else {
-                viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str());
+                viewer->addPiPCamera(camera_config.camera_name().c_str(),
+                                     pip.left(),
+                                     pip.bottom(),
+                                     pip.width(),
+                                     pip.height(),
+                                     render.width(),
+                                     render.height());
             }
         }
 
@@ -525,19 +630,31 @@ namespace cmvr {
         if (camera_config.world_id() != config_.world_id()) {
             return false;
         }
-
+        if (camera_config.camera_name().empty()) {
-        std::lock_guard<std::mutex> lock(mtx_);
-        if (has_pip_camera_config_) {
-            CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured, keep first"
-                              << ", viewer_id=" << id_
-                              << ", current_camera=" << pip_camera_config_.camera_name()
-                              << ", ignored_camera=" << camera_config.camera_name();
             return false;
         }
 
-        pip_camera_config_ = camera_config;
+        std::lock_guard<std::mutex> lock(mtx_);
-        has_pip_camera_config_ = true;
+        if (running_) {
-        CMVR_LOG(INFO) << "[MujocoViewerDevice] set PiP camera"
+            CMVR_LOG(WARNING) << "[MujocoViewerDevice] cannot add PiP camera while viewer is running"
+                              << ", viewer_id=" << id_
+                              << ", camera=" << camera_config.camera_name();
+            return false;
+        }
+        const auto duplicate = std::find_if(
+            pip_camera_configs_.begin(), pip_camera_configs_.end(),
+            [&camera_config](const config::MujocoCameraConfig& configured) {
+                return configured.camera_name() == camera_config.camera_name();
+            });
+        if (duplicate != pip_camera_configs_.end()) {
+            CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured"
+                              << ", viewer_id=" << id_
+                              << ", camera=" << camera_config.camera_name();
+            return false;
+        }
+
+        pip_camera_configs_.push_back(camera_config);
+        CMVR_LOG(INFO) << "[MujocoViewerDevice] add PiP camera"
                        << ", viewer_id=" << id_
                        << ", camera=" << camera_config.camera_name()
                        << ", world_id=" << camera_config.world_id();
@@ -556,6 +673,20 @@ namespace cmvr {
         return viewer_->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
     }
 
+    bool MujocoViewerDevice::getPiPCameraRGBD(const std::string& camera_name,
+                                              std::vector<unsigned char>& rgb,
+                                              std::vector<float>& depth,
+                                              int& width,
+                                              int& height,
+                                              uint64_t& frame_id) const {
+        std::lock_guard<std::mutex> lock(mtx_);
+        if (!viewer_) {
+            return false;
+        }
+        return viewer_->getPiPCameraRGBD(
+            camera_name, rgb, depth, width, height, frame_id);
+    }
+
     bool MujocoViewerDevice::stop() {
         std::thread thread_to_join;
         {

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp

@@ -1,11 +1,21 @@
+#include <algorithm>
+#include <array>
 #include <cstdlib>
 #include <filesystem>
 #include <iostream>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include <gtest/gtest.h>
 
+#include "common/config/config_files.h"
+#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "devices/arm/robot_arm.h"
+#include "devices/camera/abstract_camera.h"
+#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
+#include "devices/motor/manager/include/motor_manager.h"
+#include "manager/device_manager/include/device_manager.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 #include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
@@ -37,6 +47,63 @@ std::string defaultModelPath()
     return (root / "model/xiaoyan_description/dual_arm.xml").string();
 }
 
+cmvr::device::DeviceManager& createEyeToHandDeviceManager(
+    const std::filesystem::path& project_root)
+{
+    cmvr::device::DeviceManager::destroyInstance();
+    cmvr::ConfigHelper::setConfigRootFromFile(
+        (project_root / "cmvr-es/config/cmvr_es.pb.txt").string());
+
+    cmvr::config::DeviceManagerConfig config;
+    config.set_name("mujoco_viewer_eye_to_hand_test");
+    config.set_version("test");
+
+    auto* world = config.add_devices();
+    world->set_id("mujoco_world");
+    world->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD);
+    world->set_config_file("devices/mujoco/right_arm_eye_to_hand_world.pb.txt");
+    world->set_enable(true);
+
+    auto* motors = config.add_devices();
+    motors->set_id("right_arm_mujoco_motors");
+    motors->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
+    motors->set_config_file("devices/motor/mujoco_motors.pb.txt");
+    motors->set_enable(true);
+
+    auto* arm = config.add_devices();
+    arm->set_id("mujoco_right_arm");
+    arm->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM);
+    arm->set_config_file("devices/arm/arm_mujoco.pb.txt");
+    arm->set_enable(true);
+
+    for (const auto* camera_id : {"mujoco_hand_cam", "mujoco_external_touch_cam"}) {
+        auto* camera = config.add_devices();
+        camera->set_id(camera_id);
+        camera->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA);
+        camera->set_config_file("devices/camera/camera.pb.txt");
+        camera->set_enable(true);
+    }
+
+    return cmvr::device::DeviceManager::getInstance(config);
+}
+
+cmvr::device::Result moveArmToInitialization(cmvr::device::RobotArm& arm)
+{
+    const std::vector<double> positions = {
+        -0.2423,
+        1.2929,
+        1.61,
+        1.58,
+        -2.8792,
+        0.1150,
+        -0.08,
+    };
+    cmvr::device::MotionOptions options;
+    options.velocity = 2.8;
+    options.acceleration = 20.0;
+    return arm.moveJ(cmvr::device::JointPositionCommand{positions}, options);
+}
+
 } // namespace
 
 TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
@@ -63,3 +130,67 @@ TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
 
     world->stop();
 }
+
+TEST(MujocoViewerTest, ShowsRightArmEyeToHandCamera)
+{
+    const auto project_root = findProjectRoot();
+    ASSERT_FALSE(project_root.empty());
+    const auto model_path = project_root / "model/xiaoyan_description/right_arm_eye_to_hand.xml";
+
+    auto& device_manager = createEyeToHandDeviceManager(project_root);
+    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
+    ASSERT_NE(arm, nullptr);
+
+    auto hand_camera_base =
+        device_manager.getDevice<cmvr::device::AbstractCamera>("mujoco_hand_cam");
+    auto external_camera_base =
+        device_manager.getDevice<cmvr::device::AbstractCamera>("mujoco_external_touch_cam");
+    auto hand_camera = std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(hand_camera_base);
+    auto external_camera =
+        std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(external_camera_base);
+    ASSERT_NE(hand_camera, nullptr);
+    ASSERT_NE(external_camera, nullptr);
+
+    auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
+    ASSERT_NE(world, nullptr);
+    ASSERT_TRUE(world->isLoaded());
+    ASSERT_TRUE(world->isRunning());
+
+    const auto move_result = moveArmToInitialization(*arm);
+    ASSERT_TRUE(move_result.ok()) << move_result.message;
+
+    cmvr::MuJocoViewer viewer(world);
+    ASSERT_NE(viewer.model(), nullptr);
+    ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "hand_cam"), 0)
+        << "right_arm_eye_to_hand.xml does not contain hand_cam";
+    ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "external_touch_cam"), 0)
+        << "right_arm_eye_to_hand.xml does not contain external_touch_cam";
+
+    viewer.setupCamera(2.5, -160.0, -25.0);
+    const auto& external_config = external_camera->config();
+    const auto& hand_config = hand_camera->config();
+    ASSERT_TRUE(external_config.viewer_pip().enable());
+    ASSERT_TRUE(hand_config.viewer_pip().enable());
+    viewer.enablePiPCamera(external_config.camera_name().c_str(),
+                           external_config.viewer_pip().left(),
+                           external_config.viewer_pip().bottom(),
+                           external_config.viewer_pip().width(),
+                           external_config.viewer_pip().height(),
+                           external_config.encoder().width(),
+                           external_config.encoder().height());
+    viewer.addPiPCamera(hand_config.camera_name().c_str(),
+                        hand_config.viewer_pip().left(),
+                        hand_config.viewer_pip().bottom(),
+                        hand_config.viewer_pip().width(),
+                        hand_config.viewer_pip().height(),
+                        hand_config.encoder().width(),
+                        hand_config.encoder().height());
+
+    std::cout << "MujocoWorld loaded: " << model_path.string() << std::endl;
+    std::cout << "Showing hand_cam and external_touch_cam in PiP. "
+                 "Close the MuJoCo window to exit."
+              << std::endl;
+    viewer.run();
+
+    world->stop();
+}