merge agv msquic huayan_arm aubo_arm

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,38 +28,6 @@ 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}")
@@ -140,7 +107,7 @@ target_link_libraries(cmvr_es PRIVATE
         cmvr_es::runtime
         cmvr_es::proto
         cmvr_es::logging
-        service
+        cmvr_es::quic_edge_task
         ${GLOG_LIBRARIES}
         jsoncpp
         cmvr_es::service

cmvr-es/CMakeLists.txt

@@ -8,7 +8,9 @@ add_subdirectory(simulate)
 add_subdirectory(devices)
 add_subdirectory(manager/device_manager)
 add_subdirectory(manager/media_source_hub)
+add_subdirectory(service/quic_edge)
 add_subdirectory(task)
+add_subdirectory(task/quic_edge_task)
 add_subdirectory(manager/task_manager)
 add_subdirectory(service)
 add_subdirectory(runtime)

cmvr-es/algorithms/CMakeLists.txt

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

cmvr-es/algorithms/README.md

@@ -0,0 +1,125 @@
+# Algorithms 模块开发指南
+
+`algorithms/` 保存与具体厂商协议无关的运动学、规划、控制和感知算法。算法接受通用类型或显式接口输入，不应直接解析设备报文，也不应承担 gRPC/QUIC 传输职责。
+
+返回[项目总览](../../README.md)。
+
+## 当前结构
+
+| 目录 | 主要能力 | 主要 CMake target |
+| --- | --- | --- |
+| `kinematics/ik_solver/` | Pinocchio DLS/QP、SRS、LAWBA 逆运动学 | `cmvr_es::ik_solver` |
+| `motion_planner/base_motion/` | TOPPRA、S 曲线、笛卡尔速度限制 | `cmvr_es::base_motion` |
+| `motion_planner/arm_motion/` | MoveJ、MoveL、SpeedL 机械臂规划 | `cmvr_es::algorithms::arm_motion` |
+| `controllers/` | PID、IBVS、笛卡尔速度控制 | `cmvr_es::algorithms::controller`、`cmvr_es::algorithms::arm_control` |
+| `perception/` | AprilTag 和视觉定位 | `cmvr_es::perception` |
+
+顶层入口是 [`CMakeLists.txt`](CMakeLists.txt)。
+
+## 依赖边界
+
+- 算法层可以依赖 `common/`、Eigen、Pinocchio、OSQP、TOPPRA、OpenCV、ViSP 等；
+- 不包含串口、CAN、HTTP 或厂商 SDK 协议处理；
+- 不启动 gRPC/QUIC 服务或管理设备生命周期；
+- 不从算法内部读取全局配置文件，构造或 `configure` 时显式传入配置；
+- 可复用算法不应主动取得 `DeviceManager` 单例。
+
+当前部分 controller target 仍链接 `device_manager`，这是现有耦合。新增算法应优先通过参数、回调或窄接口注入设备状态，避免继续扩大该依赖。
+
+## 扩展已有算法类别
+
+### 1. 定义或复用抽象接口
+
+常用接口：
+
+- [`IKSolver`](kinematics/ik_solver/common/include/ik_solver.h)
+- [`JointMotionPlanner`](motion_planner/arm_motion/joint_motion/joint_motion_planner.h)
+- [`CartesianMotionPlanner`](motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h)
+
+接口应明确：
+
+- 输入输出单位和坐标系；
+- 是否修改内部状态；
+- 是否线程安全；
+- 失败时输出是否保持不变；
+- 是否支持实时循环，以及最大允许耗时。
+
+### 2. 增加配置
+
+在 [`../../protos/README.md`](../../protos/README.md) 指导下：
+
+1. 为算法增加独立配置 message；
+2. 在所属 `oneof algorithm` 中增加新字段和新 tag；
+3. 不复用已发布 tag；
+4. 为迭代次数、容差、速度和加速度设置有效范围；
+5. 在默认设备配置中给出显式参数。
+
+### 3. 实现与工厂注册
+
+将实现放在对应类别子目录，并修改实际工厂：
+
+- IK：[`ik_solver_factory.h`](kinematics/ik_solver/ik_solver_factory.h)
+- MoveJ：[`joint_motion_planner_factory.h`](motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h)
+- MoveL / SpeedL：[`cartesian_motion_planner_factory.h`](motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h)
+
+工厂失败应返回 `nullptr` 并记录清晰原因，不能静默回退到另一个算法。MoveL 和 SpeedL 的实现必须保持配置组合一致。
+
+### 4. 更新 CMake
+
+- 将实现 `.cpp` 加入对应 library；
+- 使用项目已有 alias target；
+- 通过 `target_include_directories` 暴露公共头；
+- 将依赖放入使用它的最小 target；
+- 测试源文件不能加入生产共享库；
+- 新增三方依赖时同步根依赖发现逻辑和 `request.txt`。
+
+## 数值与机器人语义
+
+算法扩展至少需要明确：
+
+- 关节位置单位为 rad，速度为 rad/s；
+- 笛卡尔平移为 m，旋转和角速度为 rad；
+- base、tool、world、user frame 的转换方向；
+- URDF base frame、tip frame 和关节顺序；
+- 位置、速度、加速度和 jerk 限制；
+- 奇异点、不可达目标和求解超时行为；
+- measured state 与算法内部 seed 的更新时机。
+
+IK 在求解前应使用真实关节角更新 seed。MoveL 连续求解时，应使用上一步解更新下一步状态，不能一直使用初始状态。
+
+## 测试要求
+
+每个新算法至少覆盖：
+
+1. 正常输入；
+2. 空输入、自由度不匹配和 NaN/Inf；
+3. 关节限位与速度限制；
+4. 不可达目标和不收敛；
+5. 坐标系转换；
+6. 确定性和重复调用；
+7. 若用于实时控制，统计最坏执行时间；
+8. 与一个已知模型或离线参考结果对比。
+
+当前不少算法测试只通过 `add_executable()` 构建，没有登记到 CTest。新增无设备测试应放在 `BUILD_TESTING` 条件内，并使用 `add_test()`；需要图形界面、RealSense 或 MuJoCo 的测试应明确标为集成测试，不得阻塞默认无设备测试。
+
+## 新增算法类别
+
+如果现有类别无法承载：
+
+1. 在 `algorithms/<category>/` 新建目录；
+2. 定义协议无关抽象接口；
+3. 定义配置 Proto 和工厂；
+4. 提供单独 CMake library 与 `cmvr_es::...` alias；
+5. 在 [`algorithms/CMakeLists.txt`](CMakeLists.txt) 添加子目录；
+6. 由设备或任务层注入使用，不让算法反向控制服务层；
+7. 添加无设备单元测试和真实设备/仿真集成测试。
+
+## 提交检查
+
+- [ ] 厂商协议没有进入算法接口
+- [ ] 单位、坐标系和关节顺序明确
+- [ ] 工厂已注册且配置组合经过校验
+- [ ] 不可达、超时和数值异常可观测
+- [ ] 测试没有被编入生产共享库
+- [ ] 无设备测试已登记到 CTest
+- [ ] 实时路径没有日志洪泛和无界内存分配

cmvr-es/algorithms/collision_detection/CMakeLists.txt

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -14,3 +14,14 @@ target_link_libraries(arm_motion
 add_library(cmvr_es::arm_motion ALIAS arm_motion)
 add_library(cmvr_es::algorithms::arm_motion ALIAS arm_motion)
 install(TARGETS arm_motion LIBRARY DESTINATION lib)
+
+add_executable(toppra_joint_motion_planner_test
+        joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp
+)
+
+target_link_libraries(toppra_joint_motion_planner_test
+        PRIVATE
+        cmvr_es::algorithms::arm_motion
+        gtest
+        gtest_main
+)

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h

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

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

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

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

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

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

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

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

@@ -21,3 +21,14 @@ target_link_libraries(base_motion PUBLIC
 
 add_library(cmvr_es::base_motion ALIAS base_motion)
 install(TARGETS base_motion LIBRARY DESTINATION lib)
+
+add_executable(toppra_multi_waypoint_test
+        joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp
+)
+
+target_link_libraries(toppra_multi_waypoint_test
+        PRIVATE
+        cmvr_es::base_motion
+        gtest
+        gtest_main
+)

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

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

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

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

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

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

cmvr-es/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/README.md

@@ -0,0 +1,111 @@
+# Common 模块开发指南
+
+`common/` 保存可被设备、算法、管理器和协议层复用的基础能力。这里适合放稳定、协议无关、厂商无关的类型与工具，不适合放设备连接、业务服务或任务调度逻辑。
+
+返回[项目总览](../../README.md)。
+
+## 目录职责
+
+| 目录 | 职责 |
+| --- | --- |
+| `base/` | 日志、基础常量、gRPC 辅助函数和线程安全缓冲区 |
+| `config/` | 配置根目录解析和 Proto Text 配置加载 |
+| `io/` | Protobuf 二进制与 TextFormat 文件读写 |
+| `math/` | 坐标变换、关节限制、QP 和运动数学 |
+| `media/` | 协议无关媒体模型以及 FFmpeg 采集、编码、写文件能力 |
+| `types/` | 跨后端共享的领域类型，例如 AGV、机械臂和几何类型 |
+| `vision/` | 图像显示、投影等视觉辅助代码 |
+
+## 依赖边界
+
+新增公共组件时应遵守：
+
+- 不依赖 `service/`、`task/` 或具体厂商设备实现；
+- 不保存 gRPC/QUIC 连接、session 或客户端状态；
+- 通用类型不包含厂商报文字段、端口号和私有错误码；
+- 需要调用设备的逻辑应放在 manager adapter、service 或 task；
+- 需要第三方库的 `.cpp` 组件应通过明确的 CMake target 暴露依赖；
+- 避免在公共头文件中使用全局 `using namespace` 或引入大体量实现头。
+
+当前 `common` 共享库目标是 `cmvr_es::common`，日志是独立目标 `cmvr_es::logging`。新增 `.cpp` 文件时，需要更新 [`CMakeLists.txt`](CMakeLists.txt) 或对应子目录 CMake；纯头文件不需要加入 `add_library` 源文件列表。
+
+## 新增共享类型
+
+1. 选择 `types/<domain>/` 或已有领域文件；
+2. 类型使用明确单位，例如米、弧度、秒、纳秒；
+3. 为容器长度、自由度和数值范围提供校验函数；
+4. 保持控制器无关，将厂商字段转换为通用枚举或结果；
+5. 确认不会迫使所有调用方引入设备 SDK；
+6. 增加边界值和错误输入测试。
+
+AGV 通用类型应参考 [`types/agv/agv_types.h`](types/agv/agv_types.h)，机械臂通用类型应参考 [`types/arm/arm_types.h`](types/arm/arm_types.h)。不要为了一个具体控制器把协议结构塞回 `abstract_*.h`。
+
+## 媒体模型
+
+[`media/media_frame.h`](media/media_frame.h) 中的 `TrackDescriptor`、`MediaFrame` 及 payload 在构造后不可变，可被多个协议消费者共享。
+
+扩展媒体字段时需要保持：
+
+- `TrackDescriptor::generation` 非零，编码参数变化时创建新 descriptor；
+- PTS、DTS 和 duration 使用 descriptor 的 `time_base`；
+- `capture_time_ns` 使用单调时钟，供节奏控制和延迟统计；
+- `capture_utc_ns` 只作为可选墙上时间，不能用于计算持续时间；
+- H.264/H.265 明确 `ANNEX_B` 或 `AVCC`；
+- AAC、Opus、PCM 明确 payload format、采样率和声道数；
+- 不把 QUIC、gRPC 或浏览器专有字段加入通用帧。
+
+设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。
+
+## 环形队列选择
+
+[`base/ring_buffer.h`](base/ring_buffer.h) 当前包含三类缓冲区：
+
+| 类型 | 使用场景 | 重要约束 |
+| --- | --- | --- |
+| `RingBuffer<T>` | 只需要保存最近 N 项并批量读取 | 覆盖最旧项，没有阻塞读取 |
+| `SPMCRingBuffer<T>` | 历史单生产者场景 | 独立 `reader_tail` 只能由一个线程拥有 |
+| `BroadcastFrameRing<T>` | 新的媒体或广播式多消费者场景 | 每个消费者使用独立 Cursor，保存不可变共享对象 |
+
+新的实时多消费者模块优先使用 `BroadcastFrameRing<T>`：
+
+- capacity 必须大于零；
+- 同一 Cursor 不得被多个线程同时读取或移动；
+- 慢消费者落后时会跳到最旧保留项，并得到精确 dropped count；
+- `reset()` 开启新 generation，旧 Cursor 在下一次成功读取时看到变化；
+- `close()` 唤醒等待者，关闭后不能继续发布；
+- 不要先读取 head 再无锁读取槽位，应使用队列提供的原子读取接口。
+
+## 配置和文件路径
+
+[`config/config_files.h`](config/config_files.h) 提供：
+
+- `resolveConfigFile()`：相对根配置目录解析业务配置；
+- `resolveResourceFile()`：在配置根及其父目录中查找模型等资源；
+- `loadConfigFile()` / `saveConfigFile()`：读写 Proto Text 配置。
+
+进程启动后配置根由 `main.cpp` 设置。公共组件不应自行使用当前工作目录拼接配置路径。
+
+[`io/proto_file_io.h`](io/proto_file_io.h) 写出的 TextFormat 文件权限为 `0600`。保存运行时配置前，应确认目标目录存在，并避免把生产密钥写入仓库。
+
+## 新增公共组件
+
+1. 确认能力确实会被两个及以上模块复用；
+2. 定义最小 API 和所有权、线程安全、错误语义；
+3. 将头文件放入合适子目录，将实现放入相邻 `.cpp`；
+4. 更新 CMake target 和 `target_link_libraries`；
+5. 不使用未声明的传递依赖；
+6. 增加无设备单元测试；
+7. 对并发组件增加关闭、超时、覆盖、取消和析构测试；
+8. 使用 ASan/TSan 时检查生命周期和数据竞争。
+
+推荐测试目标放在组件相邻的 `tests/`，并在 `BUILD_TESTING` 下通过 `add_test()` 登记。仅创建 `_test` 可执行文件不会自动进入 CTest。
+
+## 提交检查
+
+- [ ] API 不依赖具体设备或传输协议
+- [ ] 公共类型有明确单位和有效性规则
+- [ ] 所有权及线程安全写入注释
+- [ ] 新增 `.cpp` 和依赖已经加入 CMake
+- [ ] 缓冲区关闭能够唤醒等待线程
+- [ ] 不记录密码、私钥或大块媒体 payload
+- [ ] 无设备测试可以在开发主机运行

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/media/ffmpeg/camera_capture.cpp

@@ -62,14 +62,14 @@ int CameraCapture::initialize(const Config& config) {
 }
 
 int CameraCapture::init_device() {
-    // AVInputFormat* input_fmt = nullptr;
+    AVInputFormat* input_fmt = nullptr;
     std::string device_path;
 
 #ifdef _WIN32
-    auto input_fmt = av_find_input_format("dshow");
+    input_fmt = av_find_input_format("dshow");
     device_path = "video=" + config_.device_name;
 #else
-    auto input_fmt = av_find_input_format("v4l2");
+    input_fmt = av_find_input_format("v4l2");
     device_path = config_.device_name;
 #endif
 

cmvr-es/common/types/agv/agv_types.h

@@ -0,0 +1,420 @@
+#ifndef CMVR_ES_AGV_TYPES_H
+#define CMVR_ES_AGV_TYPES_H
+
+#include <cstdint>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "common/types/geometry_types.h"
+
+namespace cmvr::device {
+
+/**
+ * @brief AGV 通用命令/结果错误类别。
+ *
+ * 这些枚举描述框架层面的通用结果。厂商或控制器特有错误码应由具体
+ * AGV 实现转换，或保存在该实现私有的适配参数/细节中。
+ */
+enum class AgvErrorCode {
+    OK = 0,
+    NotConnected,
+    AlreadyConnected,
+    ConnectionFailed,
+    Timeout,
+    InvalidArgument,
+    LocalizationLost,
+    MapNotLoaded,
+    TaskRejected,
+    TaskFailed,
+    TaskCanceled,
+    CommandFailed,
+    EmergencyStopped,
+    Fault,
+    UnsupportedCommand,
+    UnknownError
+};
+
+/**
+ * @brief AGV 命令的标准返回值。
+ */
+struct AgvResult {
+    AgvErrorCode code{AgvErrorCode::OK};
+    std::string message{"OK"};
+
+    bool ok() const { return code == AgvErrorCode::OK; }
+    static AgvResult success() { return {AgvErrorCode::OK, "OK"}; }
+    static AgvResult failure(AgvErrorCode c, const std::string& msg) { return {c, msg}; }
+};
+
+/**
+ * @brief AGV 粗粒度运行模式。
+ */
+enum class AgvMode {
+    Unknown = 0,
+    Disconnected,
+    Idle,
+    Manual,
+    Auto,
+    Charging,
+    Paused,
+    Stopped,
+    Fault,
+    EmergencyStop
+};
+
+/**
+ * @brief 当前跟踪的导航任务状态。
+ */
+enum class AgvTaskState {
+    None = 0,
+    Waiting,
+    Running,
+    Paused,
+    Completed,
+    Failed,
+    Canceled
+};
+
+/**
+ * @brief 当前跟踪的导航任务类型。
+ */
+enum class AgvTaskType {
+    None = 0,
+    NavigateToPose,
+    NavigateToStation,
+    FollowPath,
+    Dock,
+    Charge,
+    Custom
+};
+
+/**
+ * @brief AGV 车体坐标系下的平面速度。
+ *
+ * 线速度单位为米/秒，角速度单位为弧度/秒。
+ */
+struct AgvVelocity {
+    double vx{0.0};
+    double vy{0.0};
+    double wz{0.0};
+};
+
+/**
+ * @brief 导航通用运动约束和执行选项。
+ *
+ * 除非具体实现另有说明，数值限制为 0 表示使用设备或控制器默认值。
+ */
+struct AgvMotionOptions {
+    double max_speed{0.0};
+    double max_angular_speed{0.0};
+    double max_acceleration{0.0};
+    double max_angular_acceleration{0.0};
+    double reach_distance{0.0};
+    double reach_angle{0.0};
+    double speed_ratio{1.0};
+    bool asynchronous{true};
+};
+
+/**
+ * @brief AGV 适配器可选的实现特定参数。
+ *
+ * 该结构用于避免抽象接口绑定某一个控制器协议。具体 AGV 驱动可以按需
+ * 解释操作名、特殊运动模式、设备特定标志等键值。
+ */
+struct AgvAdapterParams {
+    std::unordered_map<std::string, std::string> values;
+
+    bool empty() const { return values.empty(); }
+
+    std::optional<std::string> getString(const std::string& key) const
+    {
+        const auto it = values.find(key);
+        if (it == values.end()) {
+            return std::nullopt;
+        }
+        return it->second;
+    }
+
+    std::optional<double> getDouble(const std::string& key) const
+    {
+        const auto value = getString(key);
+        if (!value) {
+            return std::nullopt;
+        }
+        try {
+            return std::stod(*value);
+        } catch (...) {
+            return std::nullopt;
+        }
+    }
+
+    std::optional<bool> getBool(const std::string& key) const
+    {
+        const auto value = getString(key);
+        if (!value) {
+            return std::nullopt;
+        }
+        if (*value == "1" || *value == "true" || *value == "yes" || *value == "on") {
+            return true;
+        }
+        if (*value == "0" || *value == "false" || *value == "no" || *value == "off") {
+            return false;
+        }
+        return std::nullopt;
+    }
+};
+
+/**
+ * @brief 作为 AgvRuntimeState 一部分暴露的电池信息。
+ */
+struct AgvBatteryState {
+    double percentage{0.0};
+    double voltage{0.0};
+    double current{0.0};
+    double temperature{0.0};
+    bool charging{false};
+};
+
+/**
+ * @brief AGV 当前运行状态快照。
+ *
+ * 这是 AGV 设备的主要状态查询对象。抽象接口中应避免派生出的便利
+ * getter；调用方可直接从该快照读取字段。
+ */
+struct AgvRuntimeState {
+    double timestamp{0.0};
+    AgvMode mode{AgvMode::Unknown};
+    bool connected{false};
+    bool localized{false};
+    bool moving{false};
+    bool fault{false};
+    bool emergency_stopped{false};
+    math::Pose2d pose{};
+    AgvVelocity velocity{};
+    AgvBatteryState battery{};
+    std::string current_map;
+    std::string current_station;
+    std::string last_error;
+};
+
+/**
+ * @brief AGV 抽象层可见的地图站点/路径点。
+ */
+struct AgvStation {
+    std::string id;
+    std::string type;
+    math::Pose2d pose{};
+    std::string description;
+};
+
+/**
+ * @brief 显式导航路径中的一段站点到站点路径。
+ */
+struct AgvPathSegment {
+    std::string source_station;
+    std::string target_station;
+};
+
+/**
+ * @brief AGV 扫图过程中产生的数据文件。
+ *
+ * content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。
+ */
+struct AgvMappingDataFile {
+    std::string name;
+    std::string content;
+};
+
+/**
+ * @brief 从控制器增量获取的扫图数据批次。
+ */
+struct AgvMappingData {
+    int start_index{0};
+    int next_index{0};
+    std::vector<AgvMappingDataFile> files;
+};
+
+/**
+ * @brief 上位机请求的统一地图维度。
+ *
+ * 该枚举只表示上位机希望得到 2D、3D 或两者都要；不表示厂商文件格式。
+ * 厂商原始地图必须由具体 AGV 驱动转换为下面的统一地图结构。
+ */
+enum class AgvMapDimension {
+    Unspecified = 0,
+    Map2D,
+    Map3D,
+    Map2DAnd3D
+};
+
+/**
+ * @brief 地图流中的更新类型。
+ */
+enum class AgvMapUpdateType {
+    Unspecified = 0,
+    Snapshot,
+    Incremental,
+    Reset
+};
+
+/**
+ * @brief 统一语义地图对象类型。
+ */
+enum class AgvMapObjectType {
+    Unspecified = 0,
+    Station,
+    Line,
+    Area,
+    QrTag,
+    Reflector,
+    BinLocation,
+    ExternalDevice
+};
+
+/**
+ * @brief 地图坐标系下的三维点，单位：米。
+ */
+struct AgvMapPoint3D {
+    double x{0.0};
+    double y{0.0};
+    double z{0.0};
+};
+
+/**
+ * @brief 统一语义对象。几何点均使用地图坐标系，单位：米。
+ */
+struct AgvMapObject {
+    std::string id;
+    AgvMapObjectType type{AgvMapObjectType::Unspecified};
+    std::vector<AgvMapPoint3D> points;
+    double heading{0.0};
+    std::unordered_map<std::string, std::string> properties;
+};
+
+/**
+ * @brief 统一 2D 地图。
+ *
+ * data 采用行优先顺序，取值约定为 -1 未知、0 空闲、100 占据。
+ * 当厂商地图只提供矢量/语义元素时，data 可以为空，objects 仍然有效。
+ */
+struct AgvUnifiedMap2D {
+    std::string frame_id{"map"};
+    double timestamp{0.0};
+    double resolution{0.0};
+    std::uint32_t width{0};
+    std::uint32_t height{0};
+    math::Pose2d origin{};
+    std::vector<std::int32_t> data;
+    std::vector<AgvMapObject> objects;
+};
+
+/**
+ * @brief 统一 3D 点样本，坐标单位：米。
+ */
+struct AgvMapPointSample3D {
+    double x{0.0};
+    double y{0.0};
+    double z{0.0};
+    float intensity{0.0F};
+    std::uint32_t ring{0};
+    double time_offset{0.0};
+};
+
+/**
+ * @brief 统一 3D 占据体素。
+ */
+struct AgvMapVoxel3D {
+    std::int32_t x{0};
+    std::int32_t y{0};
+    std::int32_t z{0};
+    float probability{-1.0F};
+};
+
+/**
+ * @brief 统一 3D 平面特征。
+ */
+struct AgvMapPlane3D {
+    AgvMapPoint3D center;
+    AgvMapPoint3D normal;
+    double d{0.0};
+    double radius{0.0};
+};
+
+/**
+ * @brief 统一 3D 地图。
+ */
+struct AgvUnifiedMap3D {
+    std::string frame_id{"map"};
+    double timestamp{0.0};
+    double voxel_resolution{0.0};
+    std::vector<AgvMapPointSample3D> points;
+    std::vector<AgvMapVoxel3D> voxels;
+    std::vector<AgvMapPlane3D> planes;
+    std::vector<AgvMapObject> objects;
+};
+
+/**
+ * @brief 地图流读取参数。
+ */
+struct AgvMapStreamOptions {
+    AgvMapDimension dimension{AgvMapDimension::Unspecified};
+    std::string map_name;
+    std::string resume_token;
+    bool snapshot{true};
+    bool incremental{false};
+    int max_chunk_bytes{0};
+    int wait_timeout_ms{1000};
+};
+
+/**
+ * @brief 建图/扫图启动参数。
+ */
+struct AgvMappingOptions {
+    AgvMapDimension dimension{AgvMapDimension::Unspecified};
+    std::string map_name;
+    bool real_time{false};
+};
+
+/**
+ * @brief 统一地图流中的单条更新。
+ */
+struct AgvUnifiedMapUpdate {
+    std::string map_id;
+    std::string session_id;
+    std::uint64_t sequence{0};
+    std::string resume_token;
+    AgvMapDimension dimension{AgvMapDimension::Unspecified};
+    AgvMapUpdateType update_type{AgvMapUpdateType::Unspecified};
+    std::string frame_id{"map"};
+    double timestamp{0.0};
+    bool snapshot_begin{false};
+    bool snapshot_end{false};
+    std::uint32_t chunk_index{0};
+    std::uint32_t chunk_count{0};
+    std::optional<AgvUnifiedMap2D> map_2d;
+    std::optional<AgvUnifiedMap3D> map_3d;
+};
+
+/**
+ * @brief 当前导航任务状态。
+ *
+ * 该状态独立于 AgvRuntimeState，因为导航任务可能处于排队、暂停、
+ * 完成或失败状态，而车辆本体仍然保持连接并处于正常状态。
+ */
+struct AgvNavigationStatus {
+    AgvTaskState state{AgvTaskState::None};
+    AgvTaskType type{AgvTaskType::None};
+    double progress{0.0};
+    std::string message;
+};
+
+/**
+ * @brief 兼容旧 AGV 状态 API 命名的别名。
+ */
+using AGVState = AgvRuntimeState;
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_AGV_TYPES_H

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/common/types/geometry_types.h

@@ -49,7 +49,7 @@ namespace cmvr::math {
     typedef struct {
         double x;        //* unit: m
         double y;
-        double theta;
+        double theta;    //* unit: rad
     } Pose2d;
 }
 

cmvr-es/config/README.md

@@ -0,0 +1,130 @@
+# Config 模块开发指南
+
+`config/` 保存 CMVR-ES 的默认运行配置。配置格式是 Protobuf TextFormat，Schema 位于 [`../../protos/cmvr/config/`](../../protos/cmvr/config/)。
+
+返回[项目总览](../../README.md)。
+
+## 配置树
+
+```text
+cmvr_es.pb.txt
+├── logger/logger.pb.txt
+├── manager/device_manager.pb.txt
+│   └── devices/<category>/*.pb.txt
+└── manager/task_manager.pb.txt
+    └── tasks/<task>/*.pb.txt
+```
+
+入口文件：
+
+- [`cmvr_es.pb.txt`](cmvr_es.pb.txt)
+- [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt)
+- [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt)
+
+## 路径规则
+
+无参数运行时，程序读取：
+
+```text
+<cmvr_es 可执行文件所在目录>/config/cmvr_es.pb.txt
+```
+
+安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时：
+
+```bash
+./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt
+```
+
+设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。
+
+日志配置中的相对 `directory` 以可执行文件目录解析，不以配置根解析。
+
+## 新增设备配置
+
+增加同类设备后端时：
+
+1. 在 `protos/cmvr/config/<category>_config/` 增加后端 message；
+2. 在类别设备 message 的 `oneof backend` 中增加字段；
+3. 在 `devices/<category>/` 的 `.pb.txt` 中增加实例；
+4. 实例外层 `id` 必须唯一；
+5. 在 [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) 增加相同 `id`、正确 `type` 和配置路径；
+6. 开发默认保持 `enable: false`；
+7. 同步类别 factory 和 CMake；
+8. 在无硬件环境验证关闭状态，在真机环境单独开启。
+
+设备集合中的 ID 与 DeviceManager 条目 ID 不一致时，工厂会拒绝创建。
+
+## 新增任务配置
+
+1. 在 `protos/cmvr/config/` 增加任务配置和 root message；
+2. 在 `tasks/<task_name>/` 增加默认 `.pb.txt`；
+3. 在 `task_manager.pb.txt` 增加唯一任务 ID；
+4. 配置正确的 `TaskType` 和 `TaskRunMode`；
+5. 周期任务设置大于零的 `control_period_s`；
+6. 服务任务使用 `TASK_RUN_MODE_BLOCKING_SERVICE`；
+7. 默认关闭依赖网络、证书或硬件的新任务。
+
+任务实现流程见 [`../task/README.md`](../task/README.md)。
+
+## 默认值与校验
+
+- 不依赖 proto3 数值零值表达危险的生产默认值；
+- timeout、队列大小、帧大小和周期应在代码中校验；
+- 新增 loader 对不认识的 enum 和未设置的 oneof 必须明确失败；当前个别历史路径仍有退化默认行为，不应复制；
+- 设备端口、坐标系、速度和单位写入注释；
+- `enable` 应由 manager 层控制，后端内部的 enable 字段不能替代 manager 开关；
+- QUIC 需要 TaskManager 与 `QuicEdgeConfig.enable` 同时开启；
+- QUIC 零媒体轨道是合法配置。
+
+### gRPC 相机实时流
+
+[`tasks/grpc_server_task/grpc_server_task.pb.txt`](tasks/grpc_server_task/grpc_server_task.pb.txt)
+中的两个低延迟参数仅作用于 gRPC RGB 编码流，不改变机械臂、AGV 等控制 RPC：
+
+- `camera_stream_max_pending_frames`：单个客户端允许的待发送帧数，超过后清空该
+  客户端积压；默认 2；
+- `camera_stream_max_frame_age_ms`：从设备回调进入边缘系统起计算的最大帧龄，
+  超过后不再发送；默认 250 ms。
+
+两个字段填 0 或旧配置未包含字段时使用默认值。丢弃 H.264/H.265 帧后服务会请求
+IDR 并等待关键帧恢复。如果现场采集、编码本身稳定超过 250 ms，应根据日志中的
+`age_ms` 调高帧龄阈值，而不是增大环形队列。
+
+新二进制可以读取未包含这两个字段的旧配置；旧二进制不能解析包含新字段的
+TextFormat。部署时必须同步更新程序与配置，不能只把新版
+`grpc_server_task.pb.txt` 复制给旧的 `output/bin/cmvr_es`。
+
+## 配置验证
+
+构建后可以用 `protoc --encode` 对单个 TextFormat 文件做语法和字段验证。例如：
+
+```bash
+output/bin/protoc \
+  -I protos \
+  --encode=cmvr.config.QuicEdgeRootConfig \
+  protos/cmvr/config/quic_edge_config/quic_edge_config.proto \
+  < cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt \
+  > /tmp/quic_edge_config.pb
+```
+
+该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。
+
+## 生产配置
+
+`cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。
+
+- 不提交真实设备密码、token、私钥和生产地址；
+- 证书与私钥放在独立 `certs/`，使用最小读取权限；
+- 为不同站点维护独立配置根，不在运行时修改仓库样例；
+- 发布前检查所有 `enable`、IP、端口和设备 ID；
+- 变更配置 Schema 时同步 Proto 兼容性文档和平台生成代码。
+
+## 提交检查
+
+- [ ] TextFormat 可以被对应 root message 解析
+- [ ] ID、类别和引用路径完全一致
+- [ ] 新硬件和新网络任务默认关闭
+- [ ] 参数单位、范围和安全默认值明确
+- [ ] 没有生产凭据
+- [ ] 安装覆盖不会丢失现场配置
+- [ ] 无设备启动仍然成功

cmvr-es/config/certs/cmvr-quic-ca.crt

@@ -0,0 +1,25 @@
+-----BEGIN CERTIFICATE-----
+MIIEKzCCApOgAwIBAgIUTtZCyKM8INYxZKkfwbKdpHKLjn4wDQYJKoZIhvcNAQEL
+BQAwHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENBMB4XDTI2MDcyNDA2NDQy
+MFoXDTM2MDcyMTA2NDQyMFowHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENB
+MIIBojANBgkqhkiG9w0BAQEFAAOCAY8AMIIBigKCAYEAmq2rHldOobaemqNfWggS
+OVj3inKy6AYjfgtcXUfKs48DDbpZ9gyEd/YPJXA8C2jGPXpxzgnc7a4UCUVQZ8ah
+ddoJtFcC+Q6BgjeMVqUdUubu5Y9HpkfU3lvnp4KhzvOeFnkKtrCzYIPa2nK3zLc7
+uCiuLlB+91KQSRXPFbc6N7H/EAfGmUHIwlZGysAkRN7b2TAoR4C7E96JLVtuUQsS
+VtlEGpunSfuefFzeeZCMS6avLbB+a8Q6yUzLt6pqnheNsDB+jCCXodlJs5XS1AOB
+W2GOpGFMj7dLoTD+eBIlAlrhWFcwKjzFmtp6LGl/Jy0O+E99X4TL72oNSOdTrO2O
+iavXABz9IvWR2BrAyo5AKlTJqO6tmZw77iVti8jYi+HsXIVGQKMYwvv5k0jdHeIZ
+FaDToUbFPP/zj0m8ZraMy+8eNAhScnx6Zs56fcncBuDti6pT+zKisjV1rH/sFvZY
+wO1UvJlOEbvXrfYLPp58Aqe/toG2nV45a2Q4+x6AETV1AgMBAAGjYzBhMB0GA1Ud
+DgQWBBRdHz+g2vVmqghDZi2kEHtZfoyHZzAfBgNVHSMEGDAWgBRdHz+g2vVmqghD
+Zi2kEHtZfoyHZzAPBgNVHRMBAf8EBTADAQH/MA4GA1UdDwEB/wQEAwIBBjANBgkq
+hkiG9w0BAQsFAAOCAYEASuEoMFiVcg4iTMxO2kshFTJ6LIrqqGXBn+1j+yQ1ennG
+mqPMo5fBOe/Kp3YWCnREQWu0+EEPEC9qWgIDOIm3v7ch4mZiW31GUOqae6bjprBe
+er7ySElKGZ5GefKAq++we19A6WHnNxtNAT9BE1VSKUmkxEsnIkuwd+QMmQ9eaIRm
+8RvzshWdUcyiBJg07sI3rPzPD/YpnfcYlAa2a0+oXJ3o3zlhdbs2S+9fIl+Y/zFN
+wBaUV6ZNk0RFzOCA+jUq2jU5Y1pODcot3Mp2jCHzp4uTFyVqsoqc5USRB2TUILCP
+acSbEjD9GnKaNU4miTcftnC85fnBAiZ5btZkXIs72ij+J3VW7QnbcFQG8bElngoR
+mWizh6ByPQrsvyT0evJjdjpuOzILsVTAn64m0cYpnlR4g5orcEkIJHv4DzMhpXc2
+CvpcWwk2vJrUuws/LGxDATTAcqtIUIxAkxffqUsdB/6j8fdykNCvXFwNh/uPcawd
+4kzh6dr+RFzHSMQ6oN16
+-----END CERTIFICATE-----

cmvr-es/config/certs/cmvr-quic-ca.key

@@ -0,0 +1,40 @@
+-----BEGIN PRIVATE KEY-----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+-----END PRIVATE KEY-----

cmvr-es/config/certs/cmvr-quic-ca.srl

@@ -0,0 +1 @@
+2C945D70B02014891B6E09D57E377CEFB6D18498

cmvr-es/config/certs/quic-gateway.crt

@@ -0,0 +1,22 @@
+-----BEGIN CERTIFICATE-----
+MIIDuzCCAiOgAwIBAgIULJRdcLAgFIkbbgnVfjd877bRhJgwDQYJKoZIhvcNAQEL
+BQAwHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENBMB4XDTI2MDcyNDA2NDQz
+NloXDTI4MTAyNjA2NDQzNlowGDEWMBQGA1UEAwwNMTkyLjE2OC4wLjIyMjCCASIw
+DQYJKoZIhvcNAQEBBQADggEPADCCAQoCggEBAKdM3i1FYFKqNWJzhfhsD9nRUAuK
+pzilz5uqCKAt8lKYYC9WnLHOYdiEjcHGnGr02yd6sWFH/LBbxNhzx8M7h4S4izuO
+bhSlG1EIhkMiojzVD1e3P7YzXdEoVxTCfmMgBZQJG63GNOfzRawFYtEeGv7ndFVw
+kitCYlyTza5KlBNlWpiNOPmmx4dLTdGLUk8a5TUm0zJ+b/LyzVsWUtr9sxKmKeG8
+0/77AeiL0hQE3xUt5QROTZjRTVhNHowv410dFMJyIfY4sab8ndc4SIwE9PCKg068
+4705vbFInBS3eTvQur5VLSZLPatnXGKzCXjci1lIQX2p/QIqADMGFWIN9OECAwEA
+AaN4MHYwDwYDVR0RBAgwBocEwKgA3jAOBgNVHQ8BAf8EBAMCBaAwEwYDVR0lBAww
+CgYIKwYBBQUHAwEwHQYDVR0OBBYEFLmoEtsglxm3mXh4l8OF31bt2ZhnMB8GA1Ud
+IwQYMBaAFF0fP6Da9WaqCENmLaQQe1l+jIdnMA0GCSqGSIb3DQEBCwUAA4IBgQAR
+eK1mD9rJkzHe4OusimQfcuDQW+0J32e4T/34RHlW+lIj7botFaElXIzO9S80tDwq
+4d4ozNPKysqgJN9hv/BBMzJpZLwP2XozPaGLTNl1jRTCc9UhFUPrUeu0LbpQGBfC
+6Ghq42V94zPAw4lnMujnkq8botk21hclbJORQ9kblXP31IdWCgiKSFLy1NTBmQmc
+IxmR+SldMWYrWGWv/0I85AeMu6HR3+NKHmzDblm1HUHFekyC1f7sypNG+D1r8ab2
+GSijoCMKHSEOm81Vl/j5bgWQygnnIOhsLOUf2DZO6jC+VZmTKpMEJNHgwk23WBlu
+FYz/X9p5z9ZucL8aBxegj7G1fI5Ik2O05+LLeqJMfspAm6ZcnRDxCVZWQ+K/dAle
+fz+gTASzhHTsjEBeiX46LP0L2PVyBiPvNtD66e52LoM5ZigechZq91niLO+ipWdh
+xq7ryJzmaAEQKPuYaoswriWzJam07ywX9yupGHzBA6vVgGy0jPAsnGCuZXTfDyA=
+-----END CERTIFICATE-----

cmvr-es/config/certs/quic-gateway.csr

@@ -0,0 +1,17 @@
+-----BEGIN CERTIFICATE REQUEST-----
+MIICpDCCAYwCAQAwGDEWMBQGA1UEAwwNMTkyLjE2OC4wLjIyMjCCASIwDQYJKoZI
+hvcNAQEBBQADggEPADCCAQoCggEBAKdM3i1FYFKqNWJzhfhsD9nRUAuKpzilz5uq
+CKAt8lKYYC9WnLHOYdiEjcHGnGr02yd6sWFH/LBbxNhzx8M7h4S4izuObhSlG1EI
+hkMiojzVD1e3P7YzXdEoVxTCfmMgBZQJG63GNOfzRawFYtEeGv7ndFVwkitCYlyT
+za5KlBNlWpiNOPmmx4dLTdGLUk8a5TUm0zJ+b/LyzVsWUtr9sxKmKeG80/77AeiL
+0hQE3xUt5QROTZjRTVhNHowv410dFMJyIfY4sab8ndc4SIwE9PCKg0684705vbFI
+nBS3eTvQur5VLSZLPatnXGKzCXjci1lIQX2p/QIqADMGFWIN9OECAwEAAaBHMEUG
+CSqGSIb3DQEJDjE4MDYwDwYDVR0RBAgwBocEwKgA3jAOBgNVHQ8BAf8EBAMCBaAw
+EwYDVR0lBAwwCgYIKwYBBQUHAwEwDQYJKoZIhvcNAQELBQADggEBAGIcLeCE344z
+PENI1/oONVHBzMMt5VN0P8jbkJOFgZ3a6AUhfqAmDNBr+8SBym+cX2Y9Q2BsaWAu
+TKOBN+fs+fh5/NqF0hTNvmXzp89NFK5SlsTjoC21HJvK1HTNuNW8drOxNfWgFW3/
+gSCutcsWS9hVtYrV2FHQzOvVXvXKfmuTkZE8g92P3BCkvRm+ORxI0QfVS81/Ibnv
+Yl+t/o2QCphdP+1OWzU6+Pccqi2xCyIc6jqCmh0b01zg265PTTOfICO6WQNtYcOn
+308avDDpTneGl5vWRVdmmiXPqkc+dCesoguIEDUlerFpDKKY81P2GTpfjki93K7Z
+2DJPs4P1cU0=
+-----END CERTIFICATE REQUEST-----

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

@@ -1,9 +0,0 @@
-agv {
-  agvs {
-    id: "agv_1"
-    my_agv {
-      ip: "127.0.0.1"
-      port: 8080
-    }
-  }
-}

cmvr-es/config/devices/agv/src1100.pb.txt

@@ -0,0 +1,45 @@
+agv {
+  agvs {
+    id: "agv_1"
+    my_agv {
+      ip: "127.0.0.1"
+      port: 8080
+    }
+  }
+
+  agvs {
+    id: "src1100"
+    src1100_agv {
+      ip: "192.168.192.5"
+      port_status: 19204
+      port_control: 19205
+      port_nav: 19206
+      port_config: 19207
+      port_other: 19210
+      port_push: 19301
+      recv_timeout_ms: 1000
+      enable_state_push: true
+      state_push_interval_ms: 200
+      state_push_included_fields: "x"
+      state_push_included_fields: "y"
+      state_push_included_fields: "angle"
+      state_push_included_fields: "vx"
+      state_push_included_fields: "vy"
+      state_push_included_fields: "w"
+      state_push_included_fields: "battery_level"
+      state_push_included_fields: "battery_temp"
+      state_push_included_fields: "charging"
+      state_push_included_fields: "voltage"
+      state_push_included_fields: "current"
+      state_push_included_fields: "current_map"
+      state_push_included_fields: "current_station"
+      state_push_included_fields: "confidence"
+      state_push_included_fields: "emergency"
+      state_push_included_fields: "fatals"
+      state_push_included_fields: "errors"
+      enable_map_update: true
+      map_update_interval_ms: 1000
+      map_update_history_size: 8
+    }
+  }
+}

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

@@ -0,0 +1,151 @@
+arm {
+  robot_arms {
+    id: "mujoco_right_arm"
+
+    motor {
+      motor_system_id: "mujoco_motors"
+      motor_group_ids: "mujoco_right_arm"
+      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
+            weight: 2.0
+          }
+        }
+      }
+    }
+
+    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.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
+          }
+        }
+      }
+    }
+  }
+}

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

@@ -4,7 +4,7 @@ arm {
 
     vendor {
       brand: VENDOR_ROBOT_ARM_BRAND_AUBO_ARM
-      ip: "192.168.1.100"
+      ip: "192.168.192.18"
       port: 30004
       dof: 6
       joint_names: "joint_1"

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

@@ -101,7 +101,27 @@ camera {
     }
   }
 
-
+  cameras {
+    id: "left_eye_cam"
+    uvc {
+      usb: "/dev/uvc_left_camera"
+      camera_mode: CAMERA_MODE_VIDEO
+      capture {
+        width: 640
+        height: 480
+        fps: 30
+        stream_mode: STREAM_MODE_RGB
+      }
+      encoder {
+        width: 640
+        height: 480
+        fps: 30
+        codec: "H265"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
+    }
+  }
 
   cameras {
     id: "cam5"
@@ -165,50 +185,4 @@ camera {
           buffer_size: 30
         }
       }
-
-   cameras {
-       id: "real_cam1"
-       realsense {
-         serialNumber: "332522076896"
-         camera_mode: CAMERA_MODE_VIDEO
-         capture {
-           width: 640
-           height: 480
-           fps: 30
-           stream_mode: STREAM_MODE_RGBD
-         }
-         encoder {
-           width: 640
-           height: 480
-           fps: 30
-           codec: "h265_qsv"
-           enable_stream_timestamp: true
-           buffer_size: 30
-         }
-         align_mode: ALIGN_MODE_COLOR
-         sync: false
-       }
-     }
-
-     cameras {
-         id: "usb_cam1"
-         uvc {
-           usb: "/dev/video0"
-           camera_mode: CAMERA_MODE_VIDEO
-           capture {
-             width: 640
-             height: 480
-             fps: 30
-             stream_mode: STREAM_MODE_RGB
-           }
-           encoder {
-             width: 640
-             height: 480
-             fps: 30
-             codec: "h265_qsv"
-             enable_stream_timestamp: true
-             buffer_size: 30
-           }
-         }
-       }
 }

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

@@ -5,7 +5,7 @@ microphone {
       channels: 2
       sampleRate: 48000
       volume: 100
-      input_device: "plughw:CARD=XFMDPV0018,DEV=0"
+      input_device: "default"
     }
   }
 }

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

@@ -0,0 +1,72 @@
+motor {
+  id: "ethercat_motors"
+
+  motor_groups {
+    id: "right_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: 0 }
+      slaves { motor_id: 2 alias: 0 position: 1 }
+      slaves { motor_id: 3 alias: 0 position: 2 }
+      slaves { motor_id: 4 alias: 0 position: 3 }
+      slaves { motor_id: 5 alias: 0 position: 4 }
+      slaves { motor_id: 6 alias: 0 position: 5 }
+      slaves { motor_id: 7 alias: 0 position: 6 }
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+    }
+  }
+}

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

@@ -0,0 +1,63 @@
+motor {
+  id: "ethercat_motors"
+
+  motor_groups {
+    id: "right_arm_ethercat"
+    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

@@ -6,7 +6,6 @@ motor {
     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: "mujoco_right_arm"
+    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

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

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

@@ -89,20 +89,6 @@ 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
@@ -161,20 +147,4 @@ device_manager {
       # 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

@@ -14,4 +14,20 @@ 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
+  }
+  tasks {
+    id: "quic_edge"
+    type: TASK_TYPE_QUIC_EDGE
+    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
+    config_file: "tasks/quic_edge_task/quic_edge_task.pb.txt"
+    # Host-development default: no QUIC Gateway or physical media devices.
+    enable: true
+  }
 }

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

@@ -0,0 +1,65 @@
+quic_edge {
+  id: "quic_edge"
+
+  # Task enablement is controlled by manager/task_manager.pb.txt. Configure a
+  # reachable QUIC Gateway and TLS policy before enabling the task there.
+  server_host: "quic-gateway.example.com"
+  server_port: 4433
+  alpn: "cmvr-quic-edge/1"
+  node_id: "cmvr-edge"
+  software_version: "0.1"
+
+  # The existing cmvr-es gRPC server remains the robot-control endpoint. "auto"
+  # selects a usable address from the interface snapshot sent at registration
+  # and on every heartbeat.
+  grpc_endpoint_host: "auto"
+  grpc_endpoint_port: 50052
+  grpc_endpoint_tls: false
+  include_loopback_interfaces: false
+
+  # Local heartbeat period. The Gateway keeps this value when its registration
+  # response returns heartbeat_interval_ms=0; a non-zero response overrides it.
+  heartbeat_interval_ms: 5000
+  control_response_timeout_ms: 1000
+
+  tls {
+    ca_file: "certs/quic_gateway_ca.pem"
+    certificate_file: "certs/cmvr_edge_cert.pem"
+    private_key_file: "certs/cmvr_edge_key.pem"
+    server_name: "quic-gateway.example.com"
+    allow_insecure: false
+  }
+
+  reconnect {
+    initial_delay_ms: 500
+    maximum_delay_ms: 30000
+    multiplier: 2.0
+    jitter_percent: 20
+    connect_timeout_ms: 5000
+  }
+
+  maximum_datagram_bytes: 1200
+  maximum_control_frame_bytes: 1048576
+  # With 1200-byte DATAGRAMs and a 512-entry queue, 524288 stays below
+  # the atomic batch capacity while reserving slots for control messages.
+  maximum_frame_bytes: 524288
+  datagram_send_queue_depth: 512
+  media_poll_interval_ms: 2
+
+  # Zero media tracks is valid and keeps registration, IP reporting and
+  # heartbeat active. Add tracks only for devices enabled in DeviceManager.
+  # tracks {
+  #   track_id: 1
+  #   source_kind: SOURCE_KIND_CAMERA
+  #   device_id: "right_hand_cam"
+  #   source_track_id: "right_hand_cam/video/color"
+  #   enable: true
+  # }
+  # tracks {
+  #   track_id: 2
+  #   source_kind: SOURCE_KIND_MICROPHONE
+  #   device_id: "mic1"
+  #   source_track_id: "mic1/audio/main"
+  #   enable: true
+  # }
+}

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/devices/abstract_device.h

@@ -38,6 +38,20 @@ namespace cmvr::device {
             response_json = R"({"success":false,"error_message":"JSON command unsupported"})";
             return false;
         }
+
+        // This hook is sampled by DeviceManager while building heartbeats. It
+        // must be thread-safe and complete in bounded time while only copying
+        // in-memory state through atomics or a dedicated short-held state
+        // lock. Implementations must not perform device I/O, network requests,
+        // or wait on a lifecycle lock held across such I/O.
+        //
+        // The method is intentionally non-const because several legacy device
+        // categories expose non-const state getters. The returned object is a
+        // value and does not expose the device lifetime to callers.
+        virtual DeviceHealthSnapshot healthSnapshot() {
+            return {};
+        }
+
     protected:
         std::string id_;   // 设备名称
     };

cmvr-es/devices/agv/CMakeLists.txt

@@ -1,4 +1,5 @@
 add_subdirectory(my_agv)
+add_subdirectory(src1100)
 
 add_library(agv INTERFACE)
 
@@ -7,6 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(agv
         INTERFACE
         cmvr_es::device::my_agv
+        cmvr_es::device::src1100_agv
         cmvr_es::proto
 )
 

cmvr-es/devices/agv/abstract_agv.h

@@ -6,32 +6,235 @@
 #define CMVR_ES_ABSTRACT_AGV_H
 #pragma once
 
+#include <cstdint>
+#include <string>
+#include <vector>
+
+#include "common/types/agv/agv_types.h"
 #include "devices/abstract_device.h"
 
-namespace cmvr::device{
-    class AbstractAGV: public AbstractDevice {
-    public:
+namespace cmvr::device {
+
+/**
+ * @brief AGV/移动底盘设备抽象基类。
+ *
+ * 该接口只描述通用 AGV 能力。控制器特有的请求/响应字段应放在具体
+ * 驱动类中；当公共 API 需要扩展点时，可通过 AgvAdapterParams 传递。
+ */
+class AbstractAGV : public AbstractDevice {
+public:
     AbstractAGV() = default;
-        ~AbstractAGV() override=default;
+    ~AbstractAGV() override = default;
 
     DeviceKind kind() const noexcept override { return DeviceKind::AGV; }
-        virtual bool getState(AGVState &state) { return true; }
 
-        // navigation
-        virtual bool eStop() { return true; }
-        virtual bool goHome() { return true; }
-        virtual bool moveto(math::Pose2d &location, double speed_ratio) { return true; }
-        virtual bool setVelocity(math::Vec3 linear, math::Vec3 angular) { return true; }
+    /**
+     * @brief 获取 AGV 运行状态快照。
+     */
+    virtual AgvRuntimeState runtimeState() const { return {}; }
 
-        // map
-        virtual bool initMap(float resolution, int width, int height) { return true; }
-        virtual bool updateMap() { return true; }
-        virtual bool saveMap(const std::string& file_path) { return true; }
-        virtual bool loadMap(const std::string& file_path) { return true; }
+    /**
+     * @brief 获取当前导航任务状态。
+     */
+    virtual AgvNavigationStatus navigationStatus() const { return {}; }
 
-    protected:
-        AGVState state_;
-    };
-}
+    /**
+     * @brief 触发 AGV 急停行为。
+     */
+    virtual AgvResult emergencyStop()
+    {
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "emergencyStop not implemented");
+    }
     
-#endif //CMVR_ES_ABSTRACT_AGV_H
+    /**
+     * @brief 清除可恢复的 AGV 故障或告警。
+     */
+    virtual AgvResult clearFault()
+    {
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "clearFault not implemented");
+    }
+
+    /**
+     * @brief 发起到世界/地图位姿的导航任务。
+     */
+    virtual AgvResult navigateToPose(
+        const math::Pose2d& pose,
+        const AgvMotionOptions& options = {},
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{})
+    {
+        (void)pose;
+        (void)options;
+        (void)adapter_params;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToPose not implemented");
+    }
+    
+    /**
+     * @brief 发起到指定地图站点的导航任务。
+     */
+    virtual AgvResult navigateToStation(
+        const std::string& station_id,
+        const AgvMotionOptions& options = {},
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{})
+    {
+        (void)station_id;
+        (void)options;
+        (void)adapter_params;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToStation not implemented");
+    }
+    
+    /**
+     * @brief 发起显式站点到站点路径导航任务。
+     */
+    virtual AgvResult followPath(const std::vector<AgvPathSegment>& path)
+    {
+        (void)path;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented");
+    }
+    
+    /**
+     * @brief 暂停当前导航任务，如果设备支持。
+     */
+    virtual AgvResult pauseNavigation()
+    {
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "pauseNavigation not implemented");
+    }
+    
+    /**
+     * @brief 恢复已暂停的导航任务，如果设备支持。
+     */
+    virtual AgvResult resumeNavigation()
+    {
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "resumeNavigation not implemented");
+    }
+    
+    /**
+     * @brief 取消当前导航任务，如果设备支持。
+     */
+    virtual AgvResult cancelNavigation()
+    {
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "cancelNavigation not implemented");
+    }
+
+    /**
+     * @brief 向 AGV 下发低层速度控制指令。
+     *
+     * 该接口不同于导航命令。具体实现应明确速度控制在导航过程中是中断
+     * 导航、与导航共存，还是被拒绝执行。
+     */
+    virtual AgvResult setVelocity(const AgvVelocity& velocity)
+    {
+        (void)velocity;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "setVelocity not implemented");
+    }
+    
+    /**
+     * @brief 通过下发零速度停止低层速度控制。
+     *
+     * 该接口不表示取消正在执行的导航任务；取消导航请使用
+     * cancelNavigation()。
+     */
+    virtual AgvResult stopVelocityControl()
+    {
+        return setVelocity(AgvVelocity{});
+    }
+
+    /**
+     * @brief 查询 AGV 可用地图名称列表。
+     */
+    virtual AgvResult listMaps(std::vector<std::string>& maps) const
+    {
+        (void)maps;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listMaps not implemented");
+    }
+    
+    /**
+     * @brief 查询当前活动地图中的站点列表。
+     */
+    virtual AgvResult listStations(std::vector<AgvStation>& stations) const
+    {
+        (void)stations;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listStations not implemented");
+    }
+    
+    /**
+     * @brief 切换当前活动地图。
+     */
+    virtual AgvResult switchMap(const std::string& map_name)
+    {
+        (void)map_name;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "switchMap not implemented");
+    }
+    
+    /**
+     * @brief 按名称上传或替换地图。
+     */
+    virtual AgvResult uploadMap(const std::string& map_name, const std::string& content)
+    {
+        (void)map_name;
+        (void)content;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "uploadMap not implemented");
+    }
+    
+    /**
+     * @brief 按名称下载地图内容。
+     */
+    virtual AgvResult downloadMap(const std::string& map_name, std::string& content) const
+    {
+        (void)map_name;
+        (void)content;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "downloadMap not implemented");
+    }
+
+    /**
+     * @brief 开始扫图/建图。
+     */
+    virtual AgvResult startMapping(const AgvMappingOptions& options = {})
+    {
+        (void)options;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "startMapping not implemented");
+    }
+
+    /**
+     * @brief 从指定下标开始获取厂商原始扫图数据。
+     *
+     * 该接口主要保留给具体驱动内部使用。对外 gRPC 地图流应优先使用
+     * getUnifiedMapUpdate()，避免把厂商文件格式暴露给上位机。
+     */
+    virtual AgvResult getMappingData(int start_index, AgvMappingData& data) const
+    {
+        (void)start_index;
+        (void)data;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getMappingData not implemented");
+    }
+
+    /**
+     * @brief 获取统一地图更新。
+     *
+     * after_sequence 为 0 时通常返回最近可用的全量快照；大于 0 时返回
+     * 指定序号之后的下一条更新。如果当前没有新地图，具体实现可在
+     * options.wait_timeout_ms 内等待后台更新线程写入缓存。
+     */
+    virtual AgvResult getUnifiedMapUpdate(
+        std::uint64_t after_sequence,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const
+    {
+        (void)after_sequence;
+        (void)options;
+        (void)update;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getUnifiedMapUpdate not implemented");
+    }
+
+    /**
+     * @brief 停止扫图/建图。
+     */
+    virtual AgvResult stopMapping()
+    {
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "stopMapping not implemented");
+    }
+
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_ABSTRACT_AGV_H

cmvr-es/devices/agv/agv_factory.h

@@ -7,6 +7,7 @@
 #include "common/base/logging/logger.h"
 #include "devices/agv/abstract_agv.h"
 #include "devices/agv/my_agv/include/my_agv.h"
+#include "devices/agv/src1100/include/src1100_agv.h"
 
 namespace cmvr::device {
 
@@ -30,6 +31,16 @@ public:
             backend.set_id(cfg.id());
             return std::make_shared<MyAgv>(backend);
         }
+        case config::AGVDeviceConfig::kSrc1100Agv:
+        {
+            if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) {
+                CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id();
+                return nullptr;
+            }
+            auto backend = cfg.src1100_agv();
+            backend.set_id(cfg.id());
+            return std::make_shared<Src1100Agv>(backend);
+        }
 
         case config::AGVDeviceConfig::BACKEND_NOT_SET:
         default:

cmvr-es/devices/agv/my_agv/include/my_agv.h

@@ -20,15 +20,13 @@ public:
     bool stop() override;
     bool update() override;
 
-    bool getState(AGVState& state) override;
-    bool eStop() override;
-    bool goHome() override;
-    bool moveto(math::Pose2d& location, double speed_ratio) override;
-    bool setVelocity(math::Vec3 linear, math::Vec3 angular) override;
-    bool initMap(float resolution, int width, int height) override;
-    bool updateMap() override;
-    bool saveMap(const std::string& file_path) override;
-    bool loadMap(const std::string& file_path) override;
+    AgvRuntimeState runtimeState() const override;
+    AgvResult emergencyStop() override;
+    AgvResult navigateToPose(
+        const math::Pose2d& pose,
+        const AgvMotionOptions& options = {},
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
+    AgvResult setVelocity(const AgvVelocity& velocity) override;
 
 private:
     config::MyAgvConfig config_;

cmvr-es/devices/agv/my_agv/src/my_agv.cpp

@@ -27,49 +27,27 @@ bool MyAgv::update()
     return true;
 }
 
-bool MyAgv::getState(AGVState&)
+AgvRuntimeState MyAgv::runtimeState() const
 {
-    return true;
+    return {};
 }
 
-bool MyAgv::eStop()
+AgvResult MyAgv::emergencyStop()
 {
-    return true;
+    return AgvResult::success();
 }
 
-bool MyAgv::goHome()
+AgvResult MyAgv::navigateToPose(
+    const math::Pose2d&,
+    const AgvMotionOptions&,
+    const AgvAdapterParams&)
 {
-    return true;
+    return AgvResult::success();
 }
 
-bool MyAgv::moveto(math::Pose2d&, double)
+AgvResult MyAgv::setVelocity(const AgvVelocity&)
 {
-    return true;
-}
-
-bool MyAgv::setVelocity(math::Vec3, math::Vec3)
-{
-    return true;
-}
-
-bool MyAgv::initMap(float, int, int)
-{
-    return true;
-}
-
-bool MyAgv::updateMap()
-{
-    return true;
-}
-
-bool MyAgv::saveMap(const std::string&)
-{
-    return true;
-}
-
-bool MyAgv::loadMap(const std::string&)
-{
-    return true;
+    return AgvResult::success();
 }
 
 } // namespace cmvr::device

cmvr-es/devices/agv/src1100/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_library(src1100_agv SHARED src/src1100_agv.cpp)
+
+target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+
+target_link_libraries(src1100_agv
+        PUBLIC
+        cmvr_es::proto
+        jsoncpp
+)
+
+add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv)
+install(TARGETS src1100_agv LIBRARY DESTINATION lib)

cmvr-es/devices/agv/src1100/include/src1100_agv.h

@@ -0,0 +1,179 @@
+#ifndef CMVR_ES_SRC1100_AGV_H
+#define CMVR_ES_SRC1100_AGV_H
+
+#include <atomic>
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <json/json.h>
+
+#include "cmvr/config/agv_config/agv_config.pb.h"
+#include "devices/agv/abstract_agv.h"
+
+namespace cmvr::device {
+
+class Src1100Agv final : public AbstractAGV {
+public:
+    explicit Src1100Agv(const config::Src1100AgvConfig& cfg);
+    ~Src1100Agv() override;
+
+    std::string typeName() const override { return "Src1100Agv"; }
+
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+    bool update() override;
+
+    AgvRuntimeState runtimeState() const override;
+    AgvNavigationStatus navigationStatus() const override;
+
+    AgvResult emergencyStop() override;
+    AgvResult clearFault() override;
+
+    AgvResult navigateToPose(
+        const math::Pose2d& pose,
+        const AgvMotionOptions& options = {},
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
+    AgvResult navigateToStation(
+        const std::string& station_id,
+        const AgvMotionOptions& options = {},
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
+    AgvResult followPath(const std::vector<AgvPathSegment>& path) override;
+    AgvResult pauseNavigation() override;
+    AgvResult resumeNavigation() override;
+    AgvResult cancelNavigation() override;
+
+    AgvResult setVelocity(const AgvVelocity& velocity) override;
+
+    AgvResult listMaps(std::vector<std::string>& maps) const override;
+    AgvResult listStations(std::vector<AgvStation>& stations) const override;
+    AgvResult switchMap(const std::string& map_name) override;
+    AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
+    AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
+    AgvResult startMapping(const AgvMappingOptions& options = {}) override;
+    AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
+    AgvResult getUnifiedMapUpdate(
+        std::uint64_t after_sequence,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const override;
+    AgvResult stopMapping() override;
+
+private:
+    struct Ports {
+        int status{19204};
+        int control{19205};
+        int navigation{19206};
+        int config{19207};
+        int other{19210};
+        int push{19301};
+    };
+
+    AgvResult connect_();
+    AgvResult disconnect_();
+    AgvResult connectSocket_(int& sock, int port);
+    AgvResult ensureOtherSocket_();
+    void closeSocket_(int& sock) const;
+    bool connected_() const;
+
+    AgvResult sendCommand_(int sock,
+                           std::uint16_t command,
+                           const Json::Value& payload,
+                           Json::Value* response) const;
+    AgvResult sendCommandRaw_(int sock,
+                              std::uint16_t command,
+                              const Json::Value& payload,
+                              std::string* response_payload) const;
+    AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
+    AgvResult configurePush_();
+    void startPushThread_();
+    void stopPushThread_();
+    void pushLoop_();
+    AgvRuntimeState queryRuntimeState_() const;
+    void updateCachedRuntimeState_(const Json::Value& payload);
+    void startMapUpdateThread_();
+    void stopMapUpdateThread_();
+    void mapUpdateLoop_();
+    AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
+    AgvResult parseMapFileToUpdates_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        std::vector<AgvUnifiedMapUpdate>& updates) const;
+    AgvResult parseSrc1100MapArchive_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        std::vector<AgvUnifiedMapUpdate>& updates) const;
+    AgvResult parseSrc1100Map2D_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const;
+    AgvResult parseSrc1100Map3D_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const;
+    void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
+    bool findCachedMapUpdate_(
+        std::uint64_t after_sequence,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const;
+    bool mapUpdateMatches_(
+        const AgvUnifiedMapUpdate& update,
+        const AgvMapStreamOptions& options) const;
+
+    static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
+    static std::string toJsonString_(const Json::Value& value);
+    static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
+    static std::string extractJson_(const std::string& raw);
+    static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
+    static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback);
+    static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback);
+    static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options);
+    static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
+    static AgvResult resultFromResponse_(const Json::Value& response);
+
+    config::Src1100AgvConfig config_;
+    std::string ip_;
+    int recv_timeout_ms_{1000};
+    Ports ports_;
+    bool state_push_enabled_{false};
+    bool map_update_enabled_{false};
+    int map_update_interval_ms_{1000};
+    std::size_t map_update_history_size_{8};
+
+    mutable std::mutex mutex_;
+    int sock_status_{-1};
+    int sock_control_{-1};
+    int sock_navigation_{-1};
+    int sock_config_{-1};
+    int sock_other_{-1};
+    int sock_push_{-1};
+    std::string last_error_;
+
+    std::atomic<bool> push_running_{false};
+    std::thread push_thread_;
+    mutable std::mutex runtime_state_mutex_;
+    AgvRuntimeState cached_runtime_state_;
+    bool cached_runtime_state_valid_{false};
+
+    mutable std::atomic<bool> map_update_running_{false};
+    mutable std::thread map_update_thread_;
+    mutable std::mutex map_update_mutex_;
+    mutable std::condition_variable map_update_cv_;
+    mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
+    mutable std::uint64_t map_sequence_{0};
+    mutable int next_mapping_index_{0};
+    mutable std::size_t last_map_content_hash_{0};
+    mutable std::string map_session_id_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_SRC1100_AGV_H

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

@@ -1,13 +1,61 @@
 add_library(aubo_arm SHARED
-        src/aubo_arm.cpp
+        aubo_arm.cpp
 )
 
 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_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib)
+set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
+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/aubo_arm.h

@@ -29,13 +29,21 @@ public:
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
-    ControlMode getControlMode() const override { return ControlMode::Position; }
+    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
 
     Result torqueOn() override;
     Result torqueOff() override;
     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; }
@@ -98,8 +106,10 @@ private:
     std::string username_;
     std::string password_;
     double speed_scaling_{1.0};
+    ServoOptions servo_options_;
     std::atomic<bool> connected_{false};
     std::atomic<bool> busy_{false};
+    std::atomic<bool> servo_mode_{false};
     bool emergency_stopped_{false};
     mutable std::mutex mutex_;
 

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

@@ -1,575 +0,0 @@
-#include "devices/arm/aubo_arm/include/aubo_arm.h"
-
-#include <algorithm>
-#include <chrono>
-#include <exception>
-#include <thread>
-
-#include "common/base/logging/logger.h"
-
-#if defined(CMVR_HAS_AUBO_SDK)
-#include "aubo_sdk/rpc.h"
-#endif
-
-namespace cmvr::device {
-namespace {
-
-struct BusyGuard {
-    std::atomic<bool>& busy;
-    ~BusyGuard() { busy.store(false); }
-};
-
-std::vector<std::string> defaultJointNames(const std::size_t dof)
-{
-    std::vector<std::string> names;
-    names.reserve(dof);
-    for (std::size_t i = 0; i < dof; ++i) {
-        names.push_back("joint_" + std::to_string(i + 1));
-    }
-    return names;
-}
-
-std::string vendorBrandName(const config::VendorRobotArmBrand brand)
-{
-    switch (brand) {
-    case config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM:
-        return "AuboARM";
-    case config::VENDOR_ROBOT_ARM_BRAND_UNKNOWN:
-    default:
-        return "Unknown";
-    }
-}
-
-} // namespace
-
-#if defined(CMVR_HAS_AUBO_SDK)
-struct AuboArm::SdkState {
-    std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
-};
-#endif
-
-AuboArm::AuboArm(const config::RobotArmConfig& cfg)
-    : cfg_(cfg)
-{
-    id_ = cfg.id();
-    if (cfg.has_vendor()) {
-        vendor_cfg_ = cfg.vendor();
-    }
-
-    ip_ = vendor_cfg_.ip();
-    port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 30004;
-    username_ = vendor_cfg_.username().empty() ? "aubo" : vendor_cfg_.username();
-    password_ = vendor_cfg_.password().empty() ? "123456" : vendor_cfg_.password();
-
-    const auto dof = vendor_cfg_.dof() > 0 ? static_cast<std::size_t>(vendor_cfg_.dof()) : 6U;
-    model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model();
-    model_.manufacturer = vendorBrandName(vendor_cfg_.brand());
-    model_.dof = dof;
-    model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end());
-    if (model_.joint_names.empty()) {
-        model_.joint_names = defaultJointNames(dof);
-    }
-    if (model_.joint_names.size() != dof) {
-        CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_;
-        model_.joint_names = defaultJointNames(dof);
-    }
-}
-
-AuboArm::~AuboArm()
-{
-    (void)disconnect();
-}
-
-bool AuboArm::init()
-{
-    if (ip_.empty()) {
-        CMVR_LOG(ERROR) << "[AuboArm] ip is empty, id=" << id_;
-        return false;
-    }
-    const auto result = connect(ip_, port_);
-    if (!result.ok()) {
-        CMVR_LOG(ERROR) << "[AuboArm] init failed: " << result.message;
-        return false;
-    }
-    return true;
-}
-
-bool AuboArm::stop()
-{
-    return stopMotion().ok();
-}
-
-ArmState AuboArm::getRobotState() const
-{
-    ArmState state;
-    state.connected = connected_.load();
-    state.powered_on = state.connected;
-    state.brake_released = state.connected;
-    state.moving = busy_.load();
-    state.robot_mode = getRobotMode();
-    state.safety_mode = getSafetyMode();
-    state.control_mode = getControlMode();
-    state.emergency_stopped = emergency_stopped_;
-    state.speed_scaling = speed_scaling_;
-    state.actual_joint_state = getJointState();
-    state.target_joint_state = state.actual_joint_state;
-    return state;
-}
-
-JointGroupState AuboArm::getJointState() const
-{
-    JointGroupState state;
-    state.position.assign(model_.dof, 0.0);
-    state.velocity.assign(model_.dof, 0.0);
-    state.effort.assign(model_.dof, 0.0);
-
-#if defined(CMVR_HAS_AUBO_SDK)
-    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
-        return state;
-    }
-    try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
-        if (robot_names.empty()) {
-            CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot name list is empty";
-            return state;
-        }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (!robot_interface) {
-            CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot interface is null";
-            return state;
-        }
-        const auto robot_state = robot_interface->getRobotState();
-        const auto positions = robot_state->getJointPositions();
-        const auto velocities = robot_state->getJointSpeeds();
-        const auto n = std::min<std::size_t>(model_.dof, positions.size());
-        for (std::size_t i = 0; i < n; ++i) {
-            state.position[i] = positions[i];
-        }
-        const auto vn = std::min<std::size_t>(model_.dof, velocities.size());
-        for (std::size_t i = 0; i < vn; ++i) {
-            state.velocity[i] = velocities[i];
-        }
-    } catch (const std::exception& e) {
-        CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: " << e.what();
-    }
-#endif
-    return state;
-}
-
-CartesianPose AuboArm::getTcpPose(FrameType frame) const
-{
-    (void)frame;
-    return {};
-}
-
-RobotMode AuboArm::getRobotMode() const
-{
-    if (!connected_.load()) {
-        return RobotMode::Disconnected;
-    }
-    if (emergency_stopped_) {
-        return RobotMode::Stopped;
-    }
-    return busy_.load() ? RobotMode::Running : RobotMode::Idle;
-}
-
-Result AuboArm::torqueOn()
-{
-    const auto ready = ensureConnected_("torqueOn");
-    if (!ready.ok()) {
-        return ready;
-    }
-
-#if defined(CMVR_HAS_AUBO_SDK)
-    try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
-        if (robot_names.empty()) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
-        }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (!robot_interface) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
-        }
-
-        double mass = 0.0;
-        std::vector<double> cog(3, 0.0);
-        std::vector<double> aom(3, 0.0);
-        std::vector<double> inertia(6, 0.0);
-        robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);
-
-        if (robot_interface->getRobotState()->getRobotModeType() !=
-            arcs::common_interface::RobotModeType::Running) {
-            robot_interface->getRobotManage()->poweron();
-            std::this_thread::sleep_for(std::chrono::milliseconds(200));
-            robot_interface->getRobotManage()->startup();
-        }
-        emergency_stopped_ = false;
-        return Result::success();
-    } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what());
-    }
-#else
-    return unsupported_("torqueOn");
-#endif
-}
-
-Result AuboArm::torqueOff()
-{
-    const auto ready = ensureConnected_("torqueOff");
-    if (!ready.ok()) {
-        return ready;
-    }
-
-#if defined(CMVR_HAS_AUBO_SDK)
-    try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
-        if (robot_names.empty()) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
-        }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (!robot_interface) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
-        }
-        robot_interface->getRobotManage()->poweroff();
-        return Result::success();
-    } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOff failed: ") + e.what());
-    }
-#else
-    return unsupported_("torqueOff");
-#endif
-}
-
-Result AuboArm::calibrateZeroQ(const std::string& joint_name)
-{
-    (void)joint_name;
-    return unsupported_("calibrateZeroQ");
-}
-
-Result AuboArm::emergencyStop()
-{
-    emergency_stopped_ = true;
-    return stopMotion();
-}
-
-Result AuboArm::setSpeedScaling(const double scaling)
-{
-    if (scaling < 0.0 || scaling > 1.0) {
-        return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]");
-    }
-    speed_scaling_ = scaling;
-    return Result::success();
-}
-
-Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
-{
-    std::string error;
-    if (!validDof_(target.position.size(), error)) {
-        return Result::failure(ArmErrorCode::InvalidDof, error);
-    }
-    const auto ready = ensureConnected_("moveJ");
-    if (!ready.ok()) {
-        return ready;
-    }
-    if (busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
-    }
-    BusyGuard busy_guard{busy_};
-
-#if defined(CMVR_HAS_AUBO_SDK)
-    try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
-        if (robot_names.empty()) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
-        }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (!robot_interface) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
-        }
-        robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
-        robot_interface->getMotionControl()->moveJoint(
-            target.position,
-            options.acceleration > 0.0 ? options.acceleration : 0.5,
-            options.velocity > 0.0 ? options.velocity : 0.5,
-            options.blend_radius,
-            0);
-        return Result::success();
-    } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what());
-    }
-#else
-    return unsupported_("moveJ");
-#endif
-}
-
-Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration)
-{
-    (void)velocity;
-    (void)acceleration;
-    (void)duration;
-    return unsupported_("speedJ");
-}
-
-Result AuboArm::stopJ(double acceleration)
-{
-    (void)acceleration;
-    return stopMotion();
-}
-
-Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame)
-{
-    (void)frame;
-    const auto ready = ensureConnected_("moveL");
-    if (!ready.ok()) {
-        return ready;
-    }
-    if (busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
-    }
-    BusyGuard busy_guard{busy_};
-
-#if defined(CMVR_HAS_AUBO_SDK)
-    try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
-        if (robot_names.empty()) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
-        }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (!robot_interface) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
-        }
-        robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
-        std::vector<double> tcp_offset(6, 0.0);
-        robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
-        std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
-        robot_interface->getMotionControl()->moveLine(
-            pose,
-            options.acceleration > 0.0 ? options.acceleration : 0.5,
-            options.velocity > 0.0 ? options.velocity : 0.25,
-            options.blend_radius,
-            0);
-        return Result::success();
-    } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what());
-    }
-#else
-    return unsupported_("moveL");
-#endif
-}
-
-Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
-{
-    (void)velocity;
-    (void)acceleration;
-    (void)duration;
-    (void)frame;
-    return unsupported_("speedL");
-}
-
-Result AuboArm::stopL(std::optional<double> acceleration)
-{
-    (void)acceleration;
-    return stopMotion();
-}
-
-Result AuboArm::stopMotion()
-{
-#if defined(CMVR_HAS_AUBO_SDK)
-    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
-        return Result::success();
-    }
-    try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
-        if (robot_names.empty()) {
-            return Result::success();
-        }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (robot_interface) {
-            robot_interface->getMotionControl()->stopMove();
-        }
-        busy_.store(false);
-        return Result::success();
-    } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
-    }
-#else
-    busy_.store(false);
-    return Result::success();
-#endif
-}
-
-Result AuboArm::startServoMode(const ServoOptions& options)
-{
-    (void)options;
-    return unsupported_("startServoMode");
-}
-
-Result AuboArm::servoJ(const JointPositionCommand& target)
-{
-    (void)target;
-    return unsupported_("servoJ");
-}
-
-Result AuboArm::servoL(const CartesianPose& target, FrameType frame)
-{
-    (void)target;
-    (void)frame;
-    return unsupported_("servoL");
-}
-
-Result AuboArm::servoSpeedJ(const JointVelocityCommand& velocity)
-{
-    (void)velocity;
-    return unsupported_("servoSpeedJ");
-}
-
-Result AuboArm::servoSpeedL(const CartesianVelocity& velocity, FrameType frame)
-{
-    (void)velocity;
-    (void)frame;
-    return unsupported_("servoSpeedL");
-}
-
-Result AuboArm::stopServoMode()
-{
-    return Result::success();
-}
-
-Result AuboArm::connect(const std::string& ip, const int port)
-{
-    if (connected_.load()) {
-        return Result::success();
-    }
-    if (ip.empty()) {
-        return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] ip is empty");
-    }
-
-#if defined(CMVR_HAS_AUBO_SDK)
-    try {
-        sdk_ = std::make_unique<SdkState>();
-        sdk_->rpc_client = std::make_shared<arcs::aubo_sdk::RpcClient>();
-        sdk_->rpc_client->setRequestTimeout(1000);
-        sdk_->rpc_client->connect(ip, port > 0 ? port : 30004);
-        sdk_->rpc_client->login(username_, password_);
-        ip_ = ip;
-        port_ = port > 0 ? port : 30004;
-        connected_.store(true);
-        return Result::success();
-    } catch (const std::exception& e) {
-        sdk_.reset();
-        connected_.store(false);
-        return Result::failure(ArmErrorCode::ConnectionFailed,
-                               std::string("[AuboArm] connect failed: ") + e.what());
-    }
-#else
-    (void)port;
-    return Result::failure(ArmErrorCode::UnsupportedCommand,
-                           "[AuboArm] Aubo SDK is not available in this build");
-#endif
-}
-
-Result AuboArm::disconnect()
-{
-#if defined(CMVR_HAS_AUBO_SDK)
-    try {
-        if (sdk_ && sdk_->rpc_client) {
-            sdk_->rpc_client->logout();
-            sdk_->rpc_client->disconnect();
-        }
-    } catch (const std::exception& e) {
-        CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what();
-    }
-    sdk_.reset();
-#endif
-    connected_.store(false);
-    busy_.store(false);
-    return Result::success();
-}
-
-Result AuboArm::shutdown()
-{
-    (void)stopMotion();
-    return disconnect();
-}
-
-Result AuboArm::loadProgram(const std::string& program_name)
-{
-    (void)program_name;
-    return unsupported_("loadProgram");
-}
-
-Result AuboArm::playProgram()
-{
-    return unsupported_("playProgram");
-}
-
-Result AuboArm::pauseProgram()
-{
-    return unsupported_("pauseProgram");
-}
-
-Result AuboArm::stopProgram()
-{
-    return unsupported_("stopProgram");
-}
-
-std::vector<double> AuboArm::ik(const std::string& base_link,
-                                const std::string& ee_link,
-                                const CartesianPose& pose)
-{
-    (void)base_link;
-    (void)ee_link;
-    (void)pose;
-    CMVR_LOG(ERROR) << "[AuboArm] ik is not implemented";
-    return {};
-}
-
-CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_link)
-{
-    (void)base_link;
-    (void)ee_link;
-    CMVR_LOG(ERROR) << "[AuboArm] fk(base,ee) is not implemented";
-    return {};
-}
-
-CartesianPose AuboArm::fk(bool is_tcp)
-{
-    (void)is_tcp;
-    CMVR_LOG(ERROR) << "[AuboArm] fk is not implemented";
-    return {};
-}
-
-Result AuboArm::unsupported_(const std::string& name) const
-{
-    const std::string message = "[AuboArm] " + name + " is not implemented";
-    CMVR_LOG(ERROR) << message;
-    return Result::failure(ArmErrorCode::UnsupportedCommand, message);
-}
-
-bool AuboArm::validDof_(const std::size_t size, std::string& error) const
-{
-    if (size != model_.dof) {
-        error = "[AuboArm] command dof mismatch, expected=" + std::to_string(model_.dof) +
-                ", actual=" + std::to_string(size);
-        CMVR_LOG(ERROR) << error;
-        return false;
-    }
-    return true;
-}
-
-Result AuboArm::ensureConnected_(const std::string& context) const
-{
-    if (!connected_.load()) {
-        return Result::failure(ArmErrorCode::NotConnected,
-                               "[AuboArm] " + context + " failed: arm is not connected");
-    }
-#if defined(CMVR_HAS_AUBO_SDK)
-    if (!sdk_ || !sdk_->rpc_client) {
-        return Result::failure(ArmErrorCode::NotConnected,
-                               "[AuboArm] " + context + " failed: SDK client is null");
-    }
-#endif
-    return Result::success();
-}
-
-} // namespace cmvr::device

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

@@ -117,6 +117,7 @@ bool HuayanRobot::init()
         CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message;
         return false;
     }
+    setSpeedScaling(1);
     return true;
 }
 

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 isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isProtectiveStopped() const override { return protective_stopped_.load(); }
+    bool isEmergencyStopped() const override { return emergency_stopped_.load(); }
     bool isFault() const override { return false; }
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
@@ -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,6 +97,10 @@ 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;
@@ -126,7 +133,10 @@ private:
     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);
@@ -268,7 +522,7 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
     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,24 +538,38 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
         if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-            motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+            if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "failed to set cyclic position mode for joint: " +
+                                           joint_name);
+            }
         }
         motors.push_back(std::move(motor));
     }
 
     const auto t0 = std::chrono::steady_clock::now();
     constexpr double fallback_dt = 0.001;
+    std::vector<double> command_velocity(motors.size(), 0.0);
     for (std::size_t k = 1; k < samples.size(); ++k) {
+        if (const auto stopped = safetyStopResult_("moveJ", true)) {
+            return *stopped;
+        }
         const auto& sample = samples[k];
         if (sample.position.size() != motors.size()) {
             return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
         }
-        for (std::size_t i = 0; i < motors.size(); ++i) {
-            const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0;
-            motors[i]->setTarget(sample.position[i], qd);
+        std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
+        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)));
@@ -315,6 +583,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 +600,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 +623,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,6 +635,9 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                             const MotionOptions& options,
                             const FrameType frame)
 {
+    if (const auto stopped = safetyStopResult_("moveL")) {
+        return *stopped;
+    }
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
@@ -394,8 +679,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                           << ", executable_path_m=" << trajectory.executable_path_length;
     }
 
-    return executeMoveLTrajectory_(trajectory) ? Result::success()
-                                               : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
+    if (executeMoveLTrajectory_(trajectory)) {
+        return Result::success();
+    }
+    if (const auto stopped = safetyStopResult_("moveL", true)) {
+        return *stopped;
+    }
+    return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
 }
 
 Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@@ -403,6 +693,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");
     }
@@ -442,12 +735,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 +753,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();
 }
@@ -725,7 +1032,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 +1042,17 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
 
     auto next_deadline = std::chrono::steady_clock::now();
     for (std::size_t i = 1; i < trajectory.position.size(); ++i) {
+        if (safetyStopRequested_()) {
+            return false;
+        }
         const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]);
         const auto& position = trajectory.position[i];
         const auto& velocity = trajectory.velocity[i];
         if (position.size() != motors.size() || velocity.size() != motors.size()) {
             return false;
         }
-        for (std::size_t j = 0; j < motors.size(); ++j) {
-            motors[j]->setTarget(position[j], velocity[j]);
+        if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
+            return false;
         }
         next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
             std::chrono::duration<double>(dt_segment));

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

@@ -0,0 +1,881 @@
+#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 <unordered_set>
+#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));
+
+        std::unordered_set<std::string> right_arm_joints;
+        for (const auto* joint_name : kJointNames) {
+            right_arm_joints.insert(joint_name);
+        }
+        MotorManager::clearActiveJoints();
+        MotorManager::setActiveJoints(
+            "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}});
+
+        motor_system_ = std::make_shared<MotorManager>(
+            "mujoco_motors", motor_root_config.motor());
+        ASSERT_TRUE(motor_system_->init());
+        world_ = MotorManager::mujocoWorldFor("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();
+        MotorManager::clearActiveJoints();
+    }
+
+    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/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/arm/robot_arm_factory.h

@@ -6,7 +6,7 @@
 
 #include "cmvr/config/arm_config/arm_config.pb.h"
 #include "common/base/logging/logger.h"
-#include "devices/arm/aubo_arm/include/aubo_arm.h"
+#include "devices/arm/aubo_arm/aubo_arm.h"
 #include "devices/arm/huayan_arm/huayan_arm.h"
 #include "devices/arm/motor_robot_arm/include/motor_robot_arm.h"
 

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

@@ -70,6 +70,87 @@ namespace cmvr::device {
         std::string type_name;
     };
 
+    // DeviceManager lifecycle and device-reported health are deliberately
+    // separate. A device can, for example, be READY from the manager's point
+    // of view while its backend has not implemented health reporting yet.
+    enum class ManagedDeviceState {
+        Unknown,
+        Disabled,
+        Initializing,
+        Registered,
+        Ready,
+        Running,
+        Stopped,
+        Error,
+    };
+
+    enum class DeviceHealthState {
+        Unknown,
+        Healthy,
+        Degraded,
+        Fault,
+    };
+
+    inline std::string toString(ManagedDeviceState state) {
+        switch (state) {
+            case ManagedDeviceState::Disabled:
+                return "Disabled";
+            case ManagedDeviceState::Initializing:
+                return "Initializing";
+            case ManagedDeviceState::Registered:
+                return "Registered";
+            case ManagedDeviceState::Ready:
+                return "Ready";
+            case ManagedDeviceState::Running:
+                return "Running";
+            case ManagedDeviceState::Stopped:
+                return "Stopped";
+            case ManagedDeviceState::Error:
+                return "Error";
+            case ManagedDeviceState::Unknown:
+            default:
+                return "Unknown";
+        }
+    }
+
+    inline std::string toString(DeviceHealthState state) {
+        switch (state) {
+            case DeviceHealthState::Healthy:
+                return "Healthy";
+            case DeviceHealthState::Degraded:
+                return "Degraded";
+            case DeviceHealthState::Fault:
+                return "Fault";
+            case DeviceHealthState::Unknown:
+            default:
+                return "Unknown";
+        }
+    }
+
+    struct DeviceHealthSnapshot {
+        DeviceHealthState state = DeviceHealthState::Unknown;
+        std::string error_message;
+    };
+
+    struct ManagedDeviceSnapshot {
+        std::string id;
+        DeviceKind kind = DeviceKind::Unknown;
+        std::string type_name;
+        bool enabled = false;
+        ManagedDeviceState state = ManagedDeviceState::Unknown;
+        DeviceHealthSnapshot health;
+        bool abnormal = false;
+        std::string error_message;
+        std::uint64_t status_updated_at_unix_ms = 0;
+    };
+
+    struct DeviceManagerSnapshot {
+        std::string name;
+        std::string version;
+        std::string description;
+        std::vector<ManagedDeviceSnapshot> devices;
+    };
+
 } // namespace cmvr::device
 
 #endif // CMVR_ES_DEVICE_TYPES_H

cmvr-es/devices/dexhand/abstract_dexhand.h

@@ -130,6 +130,24 @@ namespace cmvr::device {
         virtual Status state() const = 0;
         virtual std::string lastError() const = 0;
 
+        DeviceHealthSnapshot healthSnapshot() override {
+            const auto lifecycle = state();
+            const auto error = lastError();
+
+            DeviceHealthSnapshot health;
+            health.error_message = error;
+            if (lifecycle == Status::FAULT) {
+                health.state = DeviceHealthState::Fault;
+            } else if (!error.empty()) {
+                health.state = DeviceHealthState::Degraded;
+            } else if (lifecycle == Status::INITIALIZED ||
+                       lifecycle == Status::STREAMING ||
+                       lifecycle == Status::STOPPED) {
+                health.state = DeviceHealthState::Healthy;
+            }
+            return health;
+        }
+
         virtual void getState(DexHandState& state) {
             state = DexHandState{};
             const auto lifecycle = this->state();

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_);
-        }
-        /**
-         *
-         * @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);
+            return protocol_->brakeRelease(node_id_);
         }
 
-        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;

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/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,284 @@
+#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 status_changed =
+        !had_previous || !previous.read_ok ||
+        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)
+                       << ", previous_statusword=0x" << std::hex << std::uppercase
+                       << std::setw(4) << std::setfill('0') << previous.statusword
+                       << std::dec << std::nouppercase << std::setfill(' ');
+    }
+}
+
+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,382 @@
+#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 = "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");
+    config.set_init_all_motors_when_no_active_joints(true);
+
+    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<std::shared_ptr<AbstractMotor>>& motors,
         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);
-    world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd));
+    mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+    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,7 +217,8 @@ double MujocoMotor::getQd()
     return qd;
 }
 
-bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
+bool MujocoMotor::commandCyclicPositionsAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
     const std::vector<double>& positions,
     const std::vector<double>& velocities)
 {
@@ -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_);
-        motors[i]->target_q_ = clamped_positions[i];
-        motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+        const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
+        if (!motor) {
+            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_);
@@ -45,8 +60,8 @@ namespace  cmvr {
                     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);
-                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
-                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
+                    // 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 +70,10 @@ namespace  cmvr {
                 }
                 return true;
             }
+
+        private:
+            double encoder_counts_per_rev_{0.0};
+            double gear_ratio_{0.0};
         };