feat: add safe UME teleoperation framework

Add the UME RobotArm and Damiao CAN-FD path, migrate the legacy UME controller, and introduce guarded cross-machine gRPC teleoperation with lifecycle, authority, configuration, and test coverage.

CMakeLists.txt

@@ -9,7 +9,29 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 #set(CMAKE_CXX_STANDARD_REQUIRED True)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+# Preserve the project's production-build behavior: tests are opt-in via
+# -DBUILD_TESTING=ON, while still registering them with CTest when requested.
+option(BUILD_TESTING "Build the test targets" OFF)
+include(CTest)
 
+if(BUILD_TESTING AND UNIX AND NOT APPLE)
+    # Test executables can still inherit the AUBO imported target's build-tree
+    # RUNPATH. Keep the active toolchain runtime ahead of that vendor path.
+    execute_process(
+            COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6
+            OUTPUT_VARIABLE CMVR_TEST_SYSTEM_LIBSTDCXX
+            OUTPUT_STRIP_TRAILING_WHITESPACE
+    )
+    if(EXISTS "${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+        get_filename_component(
+                CMVR_TEST_SYSTEM_LIBSTDCXX
+                "${CMVR_TEST_SYSTEM_LIBSTDCXX}"
+                REALPATH
+        )
+    else()
+        unset(CMVR_TEST_SYSTEM_LIBSTDCXX)
+    endif()
+endif()
 
 # Install to <source>/output
 set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE)
@@ -18,9 +40,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 transitive RPATH so CLion can run build-tree test executables without
-# manually setting LD_LIBRARY_PATH for indirect third-party dependencies.
-add_link_options(-Wl,--disable-new-dtags)
 set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF)
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
@@ -31,6 +50,9 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 include(FindExternalLib)
 set(ARCH "x86")
 setup_external_libs(${ARCH})
+if(BUILD_TESTING AND CMVR_EXTERNAL_LIBRARY_DIRS)
+    list(JOIN CMVR_EXTERNAL_LIBRARY_DIRS ":" CMVR_TEST_EXTERNAL_LIBRARY_PATH)
+endif()
 # 在调用 setup_external_libs 之后
 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}")
 message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}")

README.md

@@ -138,3 +138,120 @@ $IGH_ETHERCAT_ROOT/bin/ethercat pdos
 sudo script/ethercat/stop_ethercat.sh eno1
 sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
 ```
+
+## MotorService 与 Modbus TCP PLC
+
+工程包含从 gRPC `MotorService`、`MotorManager`、`AbstractMotor` 到
+`ModbusTcpMotorBusRuntime` 的 CMVR PLC v1 电机控制链，x86-64 的 libmodbus
+3.1.11 已放在 `dependency/x86/third_party/modbus/3.1.11`。
+
+PLC 对接时特别注意：
+
+- `host` 必须配置为 IPv4 字面量，PLC boot ID 必须非零且每次重启变化；
+- owner 决策、命令 ACK 都必须回显对应 session，重连不得执行旧 mailbox；
+- PLC 在进程启动时可以离线；连接 supervisor 会继续退避重试，离线期间状态
+  返回不可用且运动命令不会写 mailbox；
+- 状态区按 odd/even seqlock 发布，上位机使用
+  sequence-before → 64-word block → sequence-after 三段读取验证；
+- 每次 `OpenCyclicPosition/Velocity` 创建新 stream epoch，PLC 必须原子清零
+  `last_applied_cyclic_sequence`、旧样本去重状态和 cyclic watchdog，确认
+  `StreamActive` 与正确 mode 后才 ACK；重开后的首样本序列从 `1` 开始并必须
+  重新应用；
+- 活动 cyclic 流跨 `connection_epoch` 后不会自动重开；旧流的当前和后续
+  setpoint 均被拒绝并在 Quick Stop 后终止，客户端必须新建 gRPC 流。断链前
+  或断链期间 pending 的 setpoint 不会应用到新 session；
+- 任何清理 Quick Stop 未确认时，MotorService 都会 fail-closed 锁存，并在
+  成功执行 `setEnabled(true)` 前拒绝新的运动命令；
+- Modbus Quick Stop 只是功能性停止，不能替代硬接线急停或驱动器 STO。
+- 当前 Modbus 后端只提供 x86-64 的 libmodbus 3.1.11；`dependency/arm`
+  尚无对应库，因此 ARM 构建暂不支持该后端。
+
+完整 gRPC 语义、配置样例、寄存器表、TIA Portal 要求、构建测试和安全边界见
+[`docs/motor_service_modbus_tcp.md`](docs/motor_service_modbus_tcp.md)。
+
+## AUBO 控制柜 IO
+
+`AuboArm` 通过通用的 `executeJsonCommand` 接口提供控制柜 Standard 数字 IO
+读写。第一版支持以下命令：
+
+| `operation` | 说明 | 必填字段 |
+| --- | --- | --- |
+| `get_di` | 读取控制柜数字输入 | `index` |
+| `get_do` | 读取控制柜数字输出及其 runstate | `index` |
+| `set_do` | 设置控制柜数字输出 | `index`、`value` |
+
+JSON 命令示例：
+
+```json
+{"command":"cabinet_io","operation":"get_di","index":0}
+{"command":"cabinet_io","operation":"get_do","index":0}
+{"command":"cabinet_io","operation":"set_do","index":0,"value":true}
+```
+
+其中 `index` 从 `0` 开始，运行时会根据控制器返回的 IO 数量检查范围。
+`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。
+`set_do` 成功响应中的 `requested_value` 表示 SDK 已接受的请求值；需要确认控制器
+当前输出状态时，再调用一次 `get_do` 读取实际值。
+
+成功响应示例：
+
+```json
+{
+  "success": true,
+  "command": "cabinet_io",
+  "operation": "get_di",
+  "index": 0,
+  "count": 16,
+  "value": false
+}
+```
+
+### 通过 gRPC 调用
+
+该功能复用 `cmvr.api.SystemService/ExecuteJsonCommand`。默认配置中的 gRPC
+端口是 `50052`，读取 DI0：
+
+```shell
+grpcurl -plaintext \
+  -d '{
+    "header":{"deviceId":"aubo_arm"},
+    "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}"
+  }' \
+  127.0.0.1:50052 \
+  cmvr.api.SystemService/ExecuteJsonCommand
+```
+
+设置 DO0 为高电平：
+
+```shell
+grpcurl -plaintext \
+  -d '{
+    "header":{"deviceId":"aubo_arm"},
+    "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}"
+  }' \
+  127.0.0.1:50052 \
+  cmvr.api.SystemService/ExecuteJsonCommand
+```
+
+使用源码树默认配置时，先在 `cmvr-es/config/manager/device_manager.pb.txt` 中把
+`aubo_arm` 的 `enable` 改为 `true`，并在
+`cmvr-es/config/devices/arm/aubo_arm.pb.txt` 中配置正确的控制器地址和登录信息，
+然后重新安装配置并启动安装产物：
+
+```shell
+cmake --install build
+./output/bin/cmvr_es
+```
+
+`output/bin/cmvr_es` 读取的是 `output/bin/config/`；如果进程使用显式配置路径，
+应修改该配置根下的对应文件。
+设备未启用或初始化失败时，gRPC 会返回 `Device not found: aubo_arm`。
+
+### 安全约束
+
+- 该接口只访问控制柜 Standard 数字 IO，不操作工具端 IO、可配置 IO 或安全 IO。
+- `set_do` 不会修改控制器的输出 runstate。只有目标通道的 runstate 为
+  `StandardOutputRunState::None` 时才允许写入，否则返回
+  `output_managed_by_runstate`。
+- 接口不会调用会重置全部输出配置的 `setDigitalOutputRunstateDefault()`。
+- 模拟量 IO 涉及 domain、单位和量程，第一版暂不通过该 JSON 接口开放。

cmake/FindExternalLib.cmake

@@ -55,7 +55,17 @@ function(setup_external_libs ARCH)
 
         # ---- library dirs ----
         if(EXISTS "${FULL_PATH}/lib")
-            list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib")
+            file(GLOB _BUNDLED_LIBSTDCXX_FILES
+                    "${FULL_PATH}/lib/libstdc++.so"
+                    "${FULL_PATH}/lib/libstdc++.so.*"
+            )
+            if(_BUNDLED_LIBSTDCXX_FILES)
+                message(STATUS
+                        "${LIB_NAME}: excluding vendor lib directory from global "
+                        "link paths because it contains a private libstdc++")
+            else()
+                list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib")
+            endif()
             set(HAS_LIB TRUE)
 
             # Collect shared libs for install: *.so and *.so.*
@@ -129,6 +139,7 @@ function(setup_external_libs ARCH)
         list(REMOVE_DUPLICATES LIBRARY_DIRS)
         link_directories(${LIBRARY_DIRS})
     endif()
+    set(CMVR_EXTERNAL_LIBRARY_DIRS "${LIBRARY_DIRS}" PARENT_SCOPE)
 
     # ---- install third-party shared libs into <prefix>/lib ----
     if(INSTALL_SO_FILES)

cmvr-es/CMakeLists.txt

@@ -6,11 +6,14 @@ add_subdirectory(hardware)
 add_subdirectory(algorithms)
 add_subdirectory(simulate)
 add_subdirectory(devices)
+add_subdirectory(manager/control_authority)
 add_subdirectory(manager/device_manager)
 add_subdirectory(manager/media_source_hub)
 add_subdirectory(service/quic_edge)
+add_subdirectory(service/arm_teleop_client)
 add_subdirectory(task)
 add_subdirectory(task/quic_edge_task)
+add_subdirectory(task/ume_teleop_task)
 add_subdirectory(manager/task_manager)
 add_subdirectory(service)
 add_subdirectory(runtime)

cmvr-es/algorithms/controllers/CMakeLists.txt

@@ -1,5 +1,6 @@
 
 add_subdirectory(arm_control)
+add_subdirectory(ume_legacy)
 
 #find_package(VISP REQUIRED)
 

cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt

@@ -0,0 +1,78 @@
+add_library(ume_legacy_controller SHARED
+        src/ume_legacy_controller.cpp
+        src/pinocchio_ume_legacy_model_adapter.cpp
+)
+
+target_include_directories(ume_legacy_controller
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+target_link_libraries(ume_legacy_controller
+        PRIVATE
+        pinocchio_default
+        pinocchio_parsers
+)
+
+add_library(
+        cmvr_es::algorithms::ume_legacy
+        ALIAS ume_legacy_controller
+)
+
+install(TARGETS ume_legacy_controller LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(ume_legacy_controller_golden_test
+            tests/ume_legacy_controller_golden_test.cpp
+    )
+    add_executable(pinocchio_ume_legacy_model_adapter_test
+            tests/pinocchio_ume_legacy_model_adapter_test.cpp
+    )
+
+    target_link_libraries(ume_legacy_controller_golden_test
+            PRIVATE
+            cmvr_es::algorithms::ume_legacy
+            gtest
+            gtest_main
+            pthread
+    )
+    target_link_libraries(pinocchio_ume_legacy_model_adapter_test
+            PRIVATE
+            cmvr_es::algorithms::ume_legacy
+            gtest
+            gtest_main
+            pthread
+    )
+
+    foreach(_ume_legacy_test_target
+            ume_legacy_controller_golden_test
+            pinocchio_ume_legacy_model_adapter_test)
+        target_compile_definitions(${_ume_legacy_test_target}
+                PRIVATE
+                CMVR_UME_FIXED_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_bimanual/robot.xml"
+                CMVR_UME_FLOATING_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_imu/robot.xml"
+        )
+        add_test(
+                NAME ${_ume_legacy_test_target}
+                COMMAND ${_ume_legacy_test_target}
+        )
+    endforeach()
+
+    set(_ume_legacy_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _ume_legacy_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+
+    foreach(_ume_legacy_test_target
+            ume_legacy_controller_golden_test
+            pinocchio_ume_legacy_model_adapter_test)
+        set_tests_properties(${_ume_legacy_test_target} PROPERTIES
+                TIMEOUT 10
+                ENVIRONMENT "${_ume_legacy_test_environment}"
+        )
+    endforeach()
+endif()

cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h

@@ -0,0 +1,72 @@
+#ifndef CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H
+#define CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H
+
+#include <cstddef>
+#include <memory>
+#include <string>
+
+#include "ume_legacy_model_adapter.h"
+
+namespace cmvr::ume_legacy {
+
+struct UmeLegacyModelContract {
+    std::size_t fixed_nq{0};
+    std::size_t fixed_nv{0};
+    std::size_t floating_nq{0};
+    std::size_t floating_nv{0};
+    Transform4x4RowMajor base_from_imu{};
+};
+
+// Concrete adapter for the two original UME MJCF models.
+//
+// Construction parses both models and throws std::runtime_error if their
+// dimensions, joint ordering, joint coordinate indices, or required frame
+// topology differ from the frozen structural legacy contract.
+//
+// The floating model evaluates the original rnea(q, measured_arm_velocity, 0)
+// path: base twist and all accelerations are zero. Consequently its result
+// preserves the legacy velocity-dependent terms as well as gravity.
+//
+// Pinocchio Data objects are mutable workspaces. One adapter instance must be
+// used by one controller thread at a time. All Eigen workspaces are allocated
+// at construction and reused on the control path.
+class PinocchioUmeLegacyModelAdapter final
+    : public UmeLegacyModelAdapter {
+public:
+    PinocchioUmeLegacyModelAdapter(
+        std::string fixed_model_path,
+        std::string floating_model_path);
+    ~PinocchioUmeLegacyModelAdapter() override;
+
+    PinocchioUmeLegacyModelAdapter(
+        const PinocchioUmeLegacyModelAdapter&) = delete;
+    PinocchioUmeLegacyModelAdapter& operator=(
+        const PinocchioUmeLegacyModelAdapter&) = delete;
+    PinocchioUmeLegacyModelAdapter(
+        PinocchioUmeLegacyModelAdapter&&) noexcept;
+    PinocchioUmeLegacyModelAdapter& operator=(
+        PinocchioUmeLegacyModelAdapter&&) noexcept;
+
+    bool computeGravityCompensation(
+        const BimanualModelState& state,
+        JointVector& right_gravity_nm,
+        JointVector& left_gravity_nm) const override;
+
+    bool projectHapticFeedback(
+        const BimanualModelState& state,
+        ArmSide side,
+        const RawHapticFeedback& feedback,
+        ProjectedHapticEffort& projected) const override;
+
+    const UmeLegacyModelContract& contract() const noexcept;
+    const std::string& fixedModelPath() const noexcept;
+    const std::string& floatingModelPath() const noexcept;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> impl_;
+};
+
+} // namespace cmvr::ume_legacy
+
+#endif // CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H

cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h

@@ -0,0 +1,34 @@
+#ifndef CMVR_ES_UME_LEGACY_CONTROLLER_H
+#define CMVR_ES_UME_LEGACY_CONTROLLER_H
+
+#include "ume_legacy_types.h"
+
+namespace cmvr::ume_legacy {
+
+// Constants from UME commit e087df5cd3b281418722e155d9975695f163698e:
+//   ume/robot/ume/v6_imu/ume_leader/controller.py
+//   ume/robot/openarm1/teleop_leader_tuning.py
+LegacyUmeTuning originalTuning() noexcept;
+
+JointVector frictionCompensation(
+    const JointVector& velocity_rad_s,
+    const LegacyUmeTuning& tuning) noexcept;
+
+JointVector stictionCompensation(
+    const JointVector& velocity_rad_s,
+    const LegacyUmeTuning& tuning) noexcept;
+
+// error_norm is non-negative in the legacy path because it is produced by
+// np.linalg.norm. std::abs is retained here to match the subsequent Python
+// expression exactly for direct unit-level use.
+double feedbackScale(
+    double error_norm,
+    const LegacyUmeTuning& tuning) noexcept;
+
+SideControlOutput computeSideCommand(
+    const SideControlInput& input,
+    const LegacyUmeTuning& tuning = originalTuning()) noexcept;
+
+} // namespace cmvr::ume_legacy
+
+#endif // CMVR_ES_UME_LEGACY_CONTROLLER_H

cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h

@@ -0,0 +1,32 @@
+#ifndef CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H
+#define CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H
+
+#include "ume_legacy_types.h"
+
+namespace cmvr::ume_legacy {
+
+// Boundary for the two model operations used by the original IMU controller:
+//   1. floating-base RNEA gravity compensation;
+//   2. fixed-base J_rot^T projection of shoulder/wrist moments.
+//
+// Concrete implementations must load and validate their model contract so the
+// pure controller cannot silently substitute guessed kinematics or dynamics.
+class UmeLegacyModelAdapter {
+public:
+    virtual ~UmeLegacyModelAdapter() = default;
+
+    virtual bool computeGravityCompensation(
+        const BimanualModelState& state,
+        JointVector& right_gravity_nm,
+        JointVector& left_gravity_nm) const = 0;
+
+    virtual bool projectHapticFeedback(
+        const BimanualModelState& state,
+        ArmSide side,
+        const RawHapticFeedback& feedback,
+        ProjectedHapticEffort& projected) const = 0;
+};
+
+} // namespace cmvr::ume_legacy
+
+#endif // CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H

cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h

@@ -0,0 +1,110 @@
+#ifndef CMVR_ES_UME_LEGACY_TYPES_H
+#define CMVR_ES_UME_LEGACY_TYPES_H
+
+#include <array>
+#include <cstddef>
+
+namespace cmvr::ume_legacy {
+
+inline constexpr std::size_t kArmDof = 8;
+inline constexpr std::size_t kTransformElementCount = 16;
+
+using JointVector = std::array<double, kArmDof>;
+using Vector3 = std::array<double, 3>;
+using Transform4x4RowMajor =
+    std::array<double, kTransformElementCount>;
+
+enum class ArmSide {
+    Right,
+    Left
+};
+
+// Shoulder and wrist entries have already been projected by J_rot^T. The
+// elbow and gripper entries are the scalar follower efforts received by the
+// original UME controller. Keeping this type separate prevents a 3-D moment
+// from being mislabeled as a 6-D Cartesian wrench.
+struct ProjectedHapticEffort {
+    Vector3 shoulder_joint_torque{};
+    double elbow_effort{0.0};
+    Vector3 wrist_joint_torque{};
+    double gripper_effort{0.0};
+};
+
+struct TrackingError {
+    Vector3 shoulder_rotation{};
+    double elbow{0.0};
+    Vector3 wrist_rotation{};
+    double gripper{0.0};
+};
+
+struct FeedbackScales {
+    double shoulder{0.0};
+    double elbow{0.0};
+    double wrist{0.0};
+    double gripper{0.0};
+};
+
+struct LegacyUmeTuning {
+    JointVector friction_coefficient{};
+    JointVector friction_max_compensation{};
+    JointVector stiction_threshold_min_rad_s{};
+    JointVector stiction_threshold_max_rad_s{};
+    JointVector stiction_compensation{};
+
+    double feedback_error_tolerance_rad{0.0};
+    double feedback_tanh_sharpness{0.0};
+    double feedback_scale{0.0};
+    double feedback_limit_dm4340_nm{0.0};
+    double feedback_limit_dm4310_nm{0.0};
+};
+
+struct SideControlInput {
+    ArmSide side{ArmSide::Right};
+    JointVector joint_velocity_rad_s{};
+    JointVector gravity_compensation_nm{};
+    ProjectedHapticEffort projected_haptic{};
+    TrackingError tracking_error{};
+};
+
+struct SideControlOutput {
+    JointVector friction_compensation_nm{};
+    JointVector stiction_compensation_nm{};
+    JointVector feedforward_without_haptic_nm{};
+
+    // This is the interaction effort after the legacy left/right scalar sign
+    // conventions, but before scaling and clipping.
+    JointVector signed_interaction_nm{};
+    FeedbackScales feedback_scales{};
+
+    // The legacy algorithm clips only this feedback contribution. It does not
+    // apply a final clamp to gravity, friction, stiction, or command_torque.
+    JointVector limited_feedback_nm{};
+    JointVector command_torque_nm{};
+};
+
+// Pure model inputs/outputs shared by the legacy controller and its
+// Pinocchio/MJCF model adapter.
+struct BimanualModelState {
+    // Joint arrays follow the frozen RJ1..RJ8 / LJ1..LJ8 MJCF order.
+    JointVector right_position_rad{};
+    JointVector right_velocity_rad_s{};
+    JointVector left_position_rad{};
+    JointVector left_velocity_rad_s{};
+
+    // Homogeneous rigid transform from the IMU frame to the gravity/world
+    // frame. The adapter rejects non-finite and non-rigid matrices.
+    Transform4x4RowMajor world_from_imu{};
+};
+
+struct RawHapticFeedback {
+    // Moments use the LOCAL_WORLD_ALIGNED frame expected by the original
+    // Pinocchio J_rot^T mapping.
+    Vector3 shoulder_moment{};
+    double elbow_effort{0.0};
+    Vector3 wrist_moment{};
+    double gripper_effort{0.0};
+};
+
+} // namespace cmvr::ume_legacy
+
+#endif // CMVR_ES_UME_LEGACY_TYPES_H

cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp

@@ -0,0 +1,585 @@
+#include "pinocchio_ume_legacy_model_adapter.h"
+
+#include <array>
+#include <cmath>
+#include <sstream>
+#include <stdexcept>
+#include <utility>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/algorithm/rnea.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/mjcf.hpp>
+
+namespace cmvr::ume_legacy {
+namespace {
+
+using ExpectedArmJointNames = std::array<std::string, 16>;
+
+const ExpectedArmJointNames& expectedArmJointNames()
+{
+    static const ExpectedArmJointNames names{
+        "RJ1", "RJ2", "RJ3", "RJ4",
+        "RJ5", "RJ6", "RJ7", "RJ8",
+        "LJ1", "LJ2", "LJ3", "LJ4",
+        "LJ5", "LJ6", "LJ7", "LJ8"};
+    return names;
+}
+
+std::runtime_error contractError(
+    const std::string& model_kind,
+    const std::string& detail)
+{
+    return std::runtime_error(
+        "UME " + model_kind + " MJCF contract violation: " + detail);
+}
+
+void requireDimensions(
+    const pinocchio::Model& model,
+    const std::string& model_kind,
+    int nq,
+    int nv,
+    pinocchio::JointIndex njoints)
+{
+    if (model.nq != nq ||
+        model.nv != nv ||
+        model.njoints != njoints) {
+        std::ostringstream detail;
+        detail << "expected nq/nv/njoints "
+               << nq << "/" << nv << "/" << njoints
+               << ", got " << model.nq << "/" << model.nv
+               << "/" << model.njoints;
+        throw contractError(model_kind, detail.str());
+    }
+}
+
+void requireJoint(
+    const pinocchio::Model& model,
+    const std::string& model_kind,
+    pinocchio::JointIndex joint_index,
+    const std::string& expected_name,
+    int expected_idx_q,
+    int expected_nq,
+    int expected_idx_v,
+    int expected_nv)
+{
+    if (joint_index >= model.njoints) {
+        throw contractError(
+            model_kind,
+            "missing joint " + expected_name);
+    }
+
+    if (model.names[joint_index] != expected_name ||
+        model.idx_qs[joint_index] != expected_idx_q ||
+        model.nqs[joint_index] != expected_nq ||
+        model.idx_vs[joint_index] != expected_idx_v ||
+        model.nvs[joint_index] != expected_nv) {
+        std::ostringstream detail;
+        detail << "joint[" << joint_index << "] expected "
+               << expected_name << " q(" << expected_idx_q
+               << "," << expected_nq << ") v(" << expected_idx_v
+               << "," << expected_nv << "), got "
+               << model.names[joint_index] << " q("
+               << model.idx_qs[joint_index] << ","
+               << model.nqs[joint_index] << ") v("
+               << model.idx_vs[joint_index] << ","
+               << model.nvs[joint_index] << ")";
+        throw contractError(model_kind, detail.str());
+    }
+}
+
+pinocchio::FrameIndex requireUniqueFrame(
+    const pinocchio::Model& model,
+    const std::string& model_kind,
+    const std::string& frame_name,
+    const std::string& expected_parent_joint_name)
+{
+    pinocchio::FrameIndex found = model.nframes;
+    std::size_t count = 0;
+    for (pinocchio::FrameIndex index = 0;
+         index < model.nframes;
+         ++index) {
+        if (model.frames[index].name == frame_name) {
+            found = index;
+            ++count;
+        }
+    }
+    if (count != 1) {
+        std::ostringstream detail;
+        detail << "expected exactly one frame " << frame_name
+               << ", got " << count;
+        throw contractError(model_kind, detail.str());
+    }
+
+    const auto parent_joint = model.frames[found].parentJoint;
+    if (parent_joint >= model.njoints ||
+        model.names[parent_joint] != expected_parent_joint_name) {
+        std::ostringstream detail;
+        detail << "frame " << frame_name
+               << " expected parent joint "
+               << expected_parent_joint_name;
+        if (parent_joint < model.njoints) {
+            detail << ", got " << model.names[parent_joint];
+        } else {
+            detail << ", got invalid index " << parent_joint;
+        }
+        throw contractError(model_kind, detail.str());
+    }
+    return found;
+}
+
+void validateFixedModel(
+    const pinocchio::Model& model,
+    std::array<pinocchio::FrameIndex, 4>& frame_ids)
+{
+    requireDimensions(model, "fixed", 16, 16, 17);
+    const auto& names = expectedArmJointNames();
+    for (std::size_t index = 0; index < names.size(); ++index) {
+        requireJoint(
+            model,
+            "fixed",
+            static_cast<pinocchio::JointIndex>(index + 1),
+            names[index],
+            static_cast<int>(index),
+            1,
+            static_cast<int>(index),
+            1);
+    }
+
+    frame_ids[0] =
+        requireUniqueFrame(model, "fixed", "R_shoulder", "RJ3");
+    frame_ids[1] =
+        requireUniqueFrame(model, "fixed", "R_wrist", "RJ7");
+    frame_ids[2] =
+        requireUniqueFrame(model, "fixed", "L_shoulder", "LJ3");
+    frame_ids[3] =
+        requireUniqueFrame(model, "fixed", "L_wrist", "LJ7");
+}
+
+pinocchio::FrameIndex validateFloatingModel(
+    const pinocchio::Model& model)
+{
+    requireDimensions(model, "floating", 23, 22, 18);
+    requireJoint(
+        model,
+        "floating",
+        1,
+        "dm_j4340_2ec_freejoint",
+        0,
+        7,
+        0,
+        6);
+
+    const auto& names = expectedArmJointNames();
+    for (std::size_t index = 0; index < names.size(); ++index) {
+        requireJoint(
+            model,
+            "floating",
+            static_cast<pinocchio::JointIndex>(index + 2),
+            names[index],
+            static_cast<int>(index + 7),
+            1,
+            static_cast<int>(index + 6),
+            1);
+    }
+
+    requireUniqueFrame(
+        model, "floating", "R_shoulder", "RJ3");
+    requireUniqueFrame(
+        model, "floating", "R_wrist", "RJ7");
+    requireUniqueFrame(
+        model, "floating", "L_shoulder", "LJ3");
+    requireUniqueFrame(
+        model, "floating", "L_wrist", "LJ7");
+    return requireUniqueFrame(
+        model,
+        "floating",
+        "imu",
+        "dm_j4340_2ec_freejoint");
+}
+
+bool finite(const JointVector& values) noexcept
+{
+    for (const double value : values) {
+        if (!std::isfinite(value)) {
+            return false;
+        }
+    }
+    return true;
+}
+
+bool finite(const Vector3& values) noexcept
+{
+    for (const double value : values) {
+        if (!std::isfinite(value)) {
+            return false;
+        }
+    }
+    return true;
+}
+
+bool toIsometry(
+    const Transform4x4RowMajor& source,
+    Eigen::Isometry3d& destination) noexcept
+{
+    Eigen::Matrix4d matrix;
+    for (Eigen::Index row = 0; row < 4; ++row) {
+        for (Eigen::Index column = 0; column < 4; ++column) {
+            matrix(row, column) =
+                source[static_cast<std::size_t>(row * 4 + column)];
+        }
+    }
+    if (!matrix.allFinite()) {
+        return false;
+    }
+
+    constexpr double kTransformTolerance = 1e-6;
+    if (std::abs(matrix(3, 0)) > kTransformTolerance ||
+        std::abs(matrix(3, 1)) > kTransformTolerance ||
+        std::abs(matrix(3, 2)) > kTransformTolerance ||
+        std::abs(matrix(3, 3) - 1.0) > kTransformTolerance) {
+        return false;
+    }
+
+    const Eigen::Matrix3d rotation =
+        matrix.template block<3, 3>(0, 0);
+    if (!(rotation.transpose() * rotation)
+             .isApprox(Eigen::Matrix3d::Identity(),
+                       kTransformTolerance) ||
+        std::abs(rotation.determinant() - 1.0) >
+            kTransformTolerance) {
+        return false;
+    }
+
+    destination = Eigen::Isometry3d::Identity();
+    destination.linear() = rotation;
+    destination.translation() =
+        matrix.template block<3, 1>(0, 3);
+    return true;
+}
+
+Transform4x4RowMajor toRowMajor(
+    const Eigen::Matrix4d& matrix) noexcept
+{
+    Transform4x4RowMajor result{};
+    for (Eigen::Index row = 0; row < 4; ++row) {
+        for (Eigen::Index column = 0; column < 4; ++column) {
+            result[static_cast<std::size_t>(row * 4 + column)] =
+                matrix(row, column);
+        }
+    }
+    return result;
+}
+
+Eigen::Vector3d toEigen(const Vector3& value) noexcept
+{
+    return {value[0], value[1], value[2]};
+}
+
+Vector3 fromEigen(const Eigen::Vector3d& value) noexcept
+{
+    return {value.x(), value.y(), value.z()};
+}
+
+} // namespace
+
+class PinocchioUmeLegacyModelAdapter::Impl {
+public:
+    Impl(std::string fixed_path, std::string floating_path)
+        : fixed_model_path(std::move(fixed_path)),
+          floating_model_path(std::move(floating_path))
+    {
+        try {
+            pinocchio::mjcf::buildModel(
+                fixed_model_path, fixed_model, false);
+        } catch (const std::exception& error) {
+            throw std::runtime_error(
+                "Failed to load fixed UME MJCF '" +
+                fixed_model_path + "': " + error.what());
+        }
+        try {
+            pinocchio::mjcf::buildModel(
+                floating_model_path, floating_model, false);
+        } catch (const std::exception& error) {
+            throw std::runtime_error(
+                "Failed to load floating UME MJCF '" +
+                floating_model_path + "': " + error.what());
+        }
+
+        validateFixedModel(fixed_model, fixed_frame_ids);
+        const auto imu_frame_id =
+            validateFloatingModel(floating_model);
+
+        fixed_data =
+            std::make_unique<pinocchio::Data>(fixed_model);
+        floating_data =
+            std::make_unique<pinocchio::Data>(floating_model);
+        fixed_q =
+            Eigen::VectorXd::Zero(fixed_model.nq);
+        floating_q =
+            Eigen::VectorXd::Zero(floating_model.nq);
+        floating_velocity =
+            Eigen::VectorXd::Zero(floating_model.nv);
+        floating_acceleration =
+            Eigen::VectorXd::Zero(floating_model.nv);
+        shoulder_jacobian =
+            Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(
+                6, fixed_model.nv);
+        wrist_jacobian =
+            Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(
+                6, fixed_model.nv);
+
+        Eigen::VectorXd neutral =
+            pinocchio::neutral(floating_model);
+        pinocchio::framesForwardKinematics(
+            floating_model, *floating_data, neutral);
+        base_from_imu =
+            floating_data->oMf[imu_frame_id];
+
+        const Eigen::Matrix4d base_from_imu_matrix =
+            base_from_imu.toHomogeneousMatrix();
+        if (!base_from_imu_matrix.allFinite()) {
+            throw contractError(
+                "floating", "non-finite base_from_imu transform");
+        }
+
+        contract_info.fixed_nq =
+            static_cast<std::size_t>(fixed_model.nq);
+        contract_info.fixed_nv =
+            static_cast<std::size_t>(fixed_model.nv);
+        contract_info.floating_nq =
+            static_cast<std::size_t>(floating_model.nq);
+        contract_info.floating_nv =
+            static_cast<std::size_t>(floating_model.nv);
+        contract_info.base_from_imu =
+            toRowMajor(base_from_imu_matrix);
+    }
+
+    std::string fixed_model_path;
+    std::string floating_model_path;
+    pinocchio::Model fixed_model;
+    pinocchio::Model floating_model;
+    std::unique_ptr<pinocchio::Data> fixed_data;
+    std::unique_ptr<pinocchio::Data> floating_data;
+    std::array<pinocchio::FrameIndex, 4> fixed_frame_ids{};
+    pinocchio::SE3 base_from_imu{pinocchio::SE3::Identity()};
+    UmeLegacyModelContract contract_info;
+
+    // Reused by the single controller thread. This keeps the 2 kHz legacy
+    // model path free of avoidable Eigen heap allocation after construction.
+    Eigen::VectorXd fixed_q;
+    Eigen::VectorXd floating_q;
+    Eigen::VectorXd floating_velocity;
+    Eigen::VectorXd floating_acceleration;
+    Eigen::Matrix<double, 6, Eigen::Dynamic> shoulder_jacobian;
+    Eigen::Matrix<double, 6, Eigen::Dynamic> wrist_jacobian;
+};
+
+PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter(
+    std::string fixed_model_path,
+    std::string floating_model_path)
+    : impl_(std::make_unique<Impl>(
+          std::move(fixed_model_path),
+          std::move(floating_model_path)))
+{
+}
+
+PinocchioUmeLegacyModelAdapter::
+    ~PinocchioUmeLegacyModelAdapter() = default;
+
+PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter(
+    PinocchioUmeLegacyModelAdapter&&) noexcept = default;
+
+PinocchioUmeLegacyModelAdapter&
+PinocchioUmeLegacyModelAdapter::operator=(
+    PinocchioUmeLegacyModelAdapter&&) noexcept = default;
+
+bool PinocchioUmeLegacyModelAdapter::computeGravityCompensation(
+    const BimanualModelState& state,
+    JointVector& right_gravity_nm,
+    JointVector& left_gravity_nm) const
+{
+    right_gravity_nm = {};
+    left_gravity_nm = {};
+
+    if (!impl_ ||
+        !finite(state.right_position_rad) ||
+        !finite(state.right_velocity_rad_s) ||
+        !finite(state.left_position_rad) ||
+        !finite(state.left_velocity_rad_s)) {
+        return false;
+    }
+
+    Eigen::Isometry3d world_from_imu;
+    if (!toIsometry(state.world_from_imu, world_from_imu)) {
+        return false;
+    }
+
+    Eigen::Isometry3d base_from_imu =
+        Eigen::Isometry3d::Identity();
+    base_from_imu.linear() =
+        impl_->base_from_imu.rotation();
+    base_from_imu.translation() =
+        impl_->base_from_imu.translation();
+    const Eigen::Isometry3d world_from_base =
+        world_from_imu * base_from_imu.inverse();
+
+    Eigen::Quaterniond world_q_base(
+        world_from_base.rotation());
+    if (!world_q_base.coeffs().allFinite() ||
+        world_q_base.norm() <= 0.0) {
+        return false;
+    }
+    world_q_base.normalize();
+
+    auto& q = impl_->floating_q;
+    auto& velocity = impl_->floating_velocity;
+    auto& acceleration = impl_->floating_acceleration;
+    q.setZero();
+    velocity.setZero();
+    acceleration.setZero();
+
+    q.segment<3>(0) = world_from_base.translation();
+    q.segment<4>(3) = world_q_base.coeffs();
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        q[static_cast<Eigen::Index>(7 + index)] =
+            state.right_position_rad[index];
+        q[static_cast<Eigen::Index>(15 + index)] =
+            state.left_position_rad[index];
+        velocity[static_cast<Eigen::Index>(6 + index)] =
+            state.right_velocity_rad_s[index];
+        velocity[static_cast<Eigen::Index>(14 + index)] =
+            state.left_velocity_rad_s[index];
+    }
+
+    const auto& torque = pinocchio::rnea(
+        impl_->floating_model,
+        *impl_->floating_data,
+        q,
+        velocity,
+        acceleration);
+    if (!torque.allFinite() || torque.size() != 22) {
+        return false;
+    }
+
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        right_gravity_nm[index] =
+            torque[static_cast<Eigen::Index>(6 + index)];
+        left_gravity_nm[index] =
+            torque[static_cast<Eigen::Index>(14 + index)];
+    }
+    return true;
+}
+
+bool PinocchioUmeLegacyModelAdapter::projectHapticFeedback(
+    const BimanualModelState& state,
+    ArmSide side,
+    const RawHapticFeedback& feedback,
+    ProjectedHapticEffort& projected) const
+{
+    projected = {};
+    if (!impl_ ||
+        (side != ArmSide::Right &&
+         side != ArmSide::Left) ||
+        !finite(state.right_position_rad) ||
+        !finite(state.left_position_rad) ||
+        !finite(feedback.shoulder_moment) ||
+        !finite(feedback.wrist_moment) ||
+        !std::isfinite(feedback.elbow_effort) ||
+        !std::isfinite(feedback.gripper_effort)) {
+        return false;
+    }
+
+    auto& q = impl_->fixed_q;
+    q.setZero();
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        q[static_cast<Eigen::Index>(index)] =
+            state.right_position_rad[index];
+        q[static_cast<Eigen::Index>(8 + index)] =
+            state.left_position_rad[index];
+    }
+
+    pinocchio::framesForwardKinematics(
+        impl_->fixed_model, *impl_->fixed_data, q);
+
+    const std::size_t frame_offset =
+        side == ArmSide::Right ? 0 : 2;
+    const Eigen::Index shoulder_column =
+        side == ArmSide::Right ? 0 : 8;
+    const Eigen::Index wrist_column =
+        side == ArmSide::Right ? 4 : 12;
+
+    auto& shoulder_jacobian = impl_->shoulder_jacobian;
+    shoulder_jacobian.setZero();
+    pinocchio::computeFrameJacobian(
+        impl_->fixed_model,
+        *impl_->fixed_data,
+        q,
+        impl_->fixed_frame_ids[frame_offset],
+        pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
+        shoulder_jacobian);
+
+    auto& wrist_jacobian = impl_->wrist_jacobian;
+    wrist_jacobian.setZero();
+    pinocchio::computeFrameJacobian(
+        impl_->fixed_model,
+        *impl_->fixed_data,
+        q,
+        impl_->fixed_frame_ids[frame_offset + 1],
+        pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
+        wrist_jacobian);
+
+    if (!shoulder_jacobian.allFinite() ||
+        !wrist_jacobian.allFinite()) {
+        return false;
+    }
+
+    const Eigen::Vector3d shoulder_torque =
+        shoulder_jacobian
+            .block<3, 3>(3, shoulder_column)
+            .transpose() *
+        toEigen(feedback.shoulder_moment);
+    const Eigen::Vector3d wrist_torque =
+        wrist_jacobian
+            .block<3, 3>(3, wrist_column)
+            .transpose() *
+        toEigen(feedback.wrist_moment);
+    if (!shoulder_torque.allFinite() ||
+        !wrist_torque.allFinite()) {
+        return false;
+    }
+
+    projected.shoulder_joint_torque =
+        fromEigen(shoulder_torque);
+    projected.elbow_effort = feedback.elbow_effort;
+    projected.wrist_joint_torque =
+        fromEigen(wrist_torque);
+    projected.gripper_effort = feedback.gripper_effort;
+    return true;
+}
+
+const UmeLegacyModelContract&
+PinocchioUmeLegacyModelAdapter::contract() const noexcept
+{
+    return impl_->contract_info;
+}
+
+const std::string&
+PinocchioUmeLegacyModelAdapter::fixedModelPath() const noexcept
+{
+    return impl_->fixed_model_path;
+}
+
+const std::string&
+PinocchioUmeLegacyModelAdapter::floatingModelPath() const noexcept
+{
+    return impl_->floating_model_path;
+}
+
+} // namespace cmvr::ume_legacy

cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp

@@ -0,0 +1,193 @@
+#include "ume_legacy_controller.h"
+
+#include <cmath>
+
+namespace cmvr::ume_legacy {
+namespace {
+
+constexpr double kPi =
+    3.141592653589793238462643383279502884;
+
+double legacyClip(double value, double minimum, double maximum) noexcept
+{
+    // Explicit comparisons preserve NaN propagation: both comparisons are
+    // false and value is returned, matching np.clip for a NaN input.
+    if (value < minimum) {
+        return minimum;
+    }
+    if (value > maximum) {
+        return maximum;
+    }
+    return value;
+}
+
+double norm(const Vector3& value) noexcept
+{
+    return std::sqrt(value[0] * value[0] +
+                     value[1] * value[1] +
+                     value[2] * value[2]);
+}
+
+JointVector flattenInteraction(
+    ArmSide side,
+    const ProjectedHapticEffort& projected) noexcept
+{
+    JointVector interaction{
+        projected.shoulder_joint_torque[0],
+        projected.shoulder_joint_torque[1],
+        projected.shoulder_joint_torque[2],
+        projected.elbow_effort,
+        projected.wrist_joint_torque[0],
+        projected.wrist_joint_torque[1],
+        projected.wrist_joint_torque[2],
+        projected.gripper_effort};
+
+    // Exact scalar sign conventions from the legacy controller:
+    //   right elbow +, right gripper -
+    //   left  elbow -, left  gripper +
+    if (side == ArmSide::Right) {
+        interaction[7] = -interaction[7];
+    } else {
+        interaction[3] = -interaction[3];
+    }
+    return interaction;
+}
+
+} // namespace
+
+LegacyUmeTuning originalTuning() noexcept
+{
+    LegacyUmeTuning tuning;
+    tuning.friction_coefficient =
+        {1.6, 1.6, 1.6, 1.6, 0.032, 0.032, 0.032, 0.032};
+    tuning.friction_max_compensation =
+        {0.4, 0.4, 0.4, 0.4, 0.1, 0.1, 0.1, 0.1};
+
+    const double one_degree = kPi / 180.0;
+    const double ten_degrees = 10.0 * one_degree;
+    tuning.stiction_threshold_min_rad_s =
+        {one_degree, one_degree, one_degree, one_degree,
+         one_degree, one_degree, one_degree, one_degree};
+    tuning.stiction_threshold_max_rad_s =
+        {ten_degrees, ten_degrees, ten_degrees, ten_degrees,
+         ten_degrees, ten_degrees, ten_degrees, ten_degrees};
+    tuning.stiction_compensation =
+        {0.5, 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0};
+
+    tuning.feedback_error_tolerance_rad = one_degree;
+    tuning.feedback_tanh_sharpness = 10.0;
+    tuning.feedback_scale = 0.5;
+    tuning.feedback_limit_dm4340_nm = 4.0;
+    tuning.feedback_limit_dm4310_nm = 1.0;
+    return tuning;
+}
+
+JointVector frictionCompensation(
+    const JointVector& velocity_rad_s,
+    const LegacyUmeTuning& tuning) noexcept
+{
+    JointVector result{};
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        const double maximum =
+            tuning.friction_max_compensation[index];
+        result[index] = legacyClip(
+            tuning.friction_coefficient[index] *
+                velocity_rad_s[index],
+            -maximum,
+            maximum);
+    }
+    return result;
+}
+
+JointVector stictionCompensation(
+    const JointVector& velocity_rad_s,
+    const LegacyUmeTuning& tuning) noexcept
+{
+    JointVector result{};
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        const double velocity = velocity_rad_s[index];
+        const double speed = std::abs(velocity);
+
+        // Both inequalities are intentionally strict, matching:
+        // min < abs(qvel) < max.
+        if (tuning.stiction_threshold_min_rad_s[index] < speed &&
+            speed < tuning.stiction_threshold_max_rad_s[index]) {
+            if (velocity > 0.0) {
+                result[index] =
+                    tuning.stiction_compensation[index];
+            } else if (velocity < 0.0) {
+                result[index] =
+                    -tuning.stiction_compensation[index];
+            }
+        }
+    }
+    return result;
+}
+
+double feedbackScale(
+    double error_norm,
+    const LegacyUmeTuning& tuning) noexcept
+{
+    return tuning.feedback_scale *
+           (std::tanh(
+                tuning.feedback_tanh_sharpness *
+                (std::abs(error_norm) -
+                 tuning.feedback_error_tolerance_rad)) +
+            1.0) /
+           2.0;
+}
+
+SideControlOutput computeSideCommand(
+    const SideControlInput& input,
+    const LegacyUmeTuning& tuning) noexcept
+{
+    SideControlOutput output;
+    output.friction_compensation_nm =
+        frictionCompensation(input.joint_velocity_rad_s, tuning);
+    output.stiction_compensation_nm =
+        stictionCompensation(input.joint_velocity_rad_s, tuning);
+    output.signed_interaction_nm =
+        flattenInteraction(input.side, input.projected_haptic);
+
+    output.feedback_scales.shoulder =
+        feedbackScale(norm(input.tracking_error.shoulder_rotation),
+                      tuning);
+    output.feedback_scales.elbow =
+        feedbackScale(std::abs(input.tracking_error.elbow), tuning);
+    output.feedback_scales.wrist =
+        feedbackScale(norm(input.tracking_error.wrist_rotation),
+                      tuning);
+    output.feedback_scales.gripper =
+        feedbackScale(std::abs(input.tracking_error.gripper), tuning);
+
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        output.feedforward_without_haptic_nm[index] =
+            input.gravity_compensation_nm[index] +
+            output.friction_compensation_nm[index] +
+            output.stiction_compensation_nm[index];
+
+        double scale = output.feedback_scales.gripper;
+        double limit = tuning.feedback_limit_dm4310_nm;
+        if (index < 3) {
+            scale = output.feedback_scales.shoulder;
+            limit = tuning.feedback_limit_dm4340_nm;
+        } else if (index == 3) {
+            scale = output.feedback_scales.elbow;
+            limit = tuning.feedback_limit_dm4340_nm;
+        } else if (index < 7) {
+            scale = output.feedback_scales.wrist;
+        }
+
+        output.limited_feedback_nm[index] = legacyClip(
+            scale * output.signed_interaction_nm[index],
+            -limit,
+            limit);
+        output.command_torque_nm[index] =
+            output.feedforward_without_haptic_nm[index] -
+            output.limited_feedback_nm[index];
+    }
+
+    return output;
+}
+
+} // namespace cmvr::ume_legacy

cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp

@@ -0,0 +1,392 @@
+#include "pinocchio_ume_legacy_model_adapter.h"
+
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <stdexcept>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#ifndef CMVR_UME_FIXED_MODEL_PATH
+#error "CMVR_UME_FIXED_MODEL_PATH must identify the deployed fixed UME MJCF"
+#endif
+
+#ifndef CMVR_UME_FLOATING_MODEL_PATH
+#error "CMVR_UME_FLOATING_MODEL_PATH must identify the deployed floating UME MJCF"
+#endif
+
+namespace cmvr::ume_legacy {
+namespace {
+
+constexpr double kNumericalTolerance = 1e-10;
+
+PinocchioUmeLegacyModelAdapter makeAdapter()
+{
+    return PinocchioUmeLegacyModelAdapter(
+        CMVR_UME_FIXED_MODEL_PATH,
+        CMVR_UME_FLOATING_MODEL_PATH);
+}
+
+BimanualModelState makeGoldenState(
+    const PinocchioUmeLegacyModelAdapter& adapter)
+{
+    BimanualModelState state;
+    state.world_from_imu =
+        adapter.contract().base_from_imu;
+    state.right_position_rad =
+        {0.1, -0.2, 0.3, -0.4,
+         0.2, -0.1, 0.15, -0.05};
+    state.left_position_rad =
+        {-0.1, 0.2, -0.3, 0.4,
+         -0.2, 0.1, -0.15, 0.05};
+    return state;
+}
+
+template <std::size_t Size>
+void expectFinite(const std::array<double, Size>& values)
+{
+    for (std::size_t index = 0; index < Size; ++index) {
+        EXPECT_TRUE(std::isfinite(values[index]))
+            << "index " << index;
+    }
+}
+
+template <std::size_t Size>
+void expectNear(
+    const std::array<double, Size>& actual,
+    const std::array<double, Size>& expected,
+    double tolerance = kNumericalTolerance)
+{
+    for (std::size_t index = 0; index < Size; ++index) {
+        EXPECT_NEAR(actual[index], expected[index], tolerance)
+            << "index " << index;
+    }
+}
+
+Transform4x4RowMajor multiplyTransforms(
+    const Transform4x4RowMajor& left,
+    const Transform4x4RowMajor& right)
+{
+    Transform4x4RowMajor result{};
+    for (std::size_t row = 0; row < 4; ++row) {
+        for (std::size_t column = 0; column < 4; ++column) {
+            for (std::size_t inner = 0; inner < 4; ++inner) {
+                result[row * 4 + column] +=
+                    left[row * 4 + inner] *
+                    right[inner * 4 + column];
+            }
+        }
+    }
+    return result;
+}
+
+Transform4x4RowMajor makeNoncommutingWorldFromBase()
+{
+    constexpr double roll = 0.2;
+    constexpr double pitch = -0.35;
+    constexpr double yaw = 0.47;
+    const double sr = std::sin(roll);
+    const double cr = std::cos(roll);
+    const double sp = std::sin(pitch);
+    const double cp = std::cos(pitch);
+    const double sy = std::sin(yaw);
+    const double cy = std::cos(yaw);
+    return {
+        cy * cp,
+        cy * sp * sr - sy * cr,
+        cy * sp * cr + sy * sr,
+        0.4,
+        sy * cp,
+        sy * sp * sr + cy * cr,
+        sy * sp * cr - cy * sr,
+        -0.1,
+        -sp,
+        cp * sr,
+        cp * cr,
+        0.8,
+        0.0, 0.0, 0.0, 1.0};
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     LoadsOriginalMjcfWithoutGeometryAssetsAndFreezesContract)
+{
+    const auto adapter = makeAdapter();
+    const auto& contract = adapter.contract();
+
+    EXPECT_EQ(contract.fixed_nq, 16U);
+    EXPECT_EQ(contract.fixed_nv, 16U);
+    EXPECT_EQ(contract.floating_nq, 23U);
+    EXPECT_EQ(contract.floating_nv, 22U);
+    EXPECT_EQ(adapter.fixedModelPath(), CMVR_UME_FIXED_MODEL_PATH);
+    EXPECT_EQ(
+        adapter.floatingModelPath(),
+        CMVR_UME_FLOATING_MODEL_PATH);
+
+    // This transform comes from the original floating model's imu site.
+    // Pinocchio buildModel parses it without loading STL geometry.
+    const Transform4x4RowMajor expected_base_from_imu{
+        0.0, 0.0, -1.0, -0.0298,
+        0.0, 1.0, 0.0, 0.0,
+        1.0, 0.0, 0.0, -0.229564,
+        0.0, 0.0, 0.0, 1.0};
+    expectNear(
+        contract.base_from_imu,
+        expected_base_from_imu,
+        1e-5);
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     FloatingBaseRneaProducesFiniteFrozenJointEfforts)
+{
+    const auto adapter = makeAdapter();
+    const auto state = makeGoldenState(adapter);
+    JointVector right{};
+    JointVector left{};
+
+    ASSERT_TRUE(
+        adapter.computeGravityCompensation(
+            state, right, left));
+    expectFinite(right);
+    expectFinite(left);
+    expectNear(
+        right,
+        {4.2579946000243867,
+         -3.101883528283977,
+         5.8349126697703291,
+         -3.2335040596068012,
+         0.54313804667559806,
+         -0.28419763993223052,
+         0.075420409617272505,
+         -0.0050792810993999194});
+    expectNear(
+        left,
+        {-4.2570142852347947,
+         3.1002195124462104,
+         -5.8319486672094438,
+         3.2334454894750602,
+         -0.54274278459746039,
+         0.28419800074629464,
+         -0.075420524366616282,
+         0.0050792800399334561});
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     ImuDerivedBaseOrientationReversesGravityUnderHalfTurn)
+{
+    const auto adapter = makeAdapter();
+    auto state = makeGoldenState(adapter);
+    JointVector upright_right{};
+    JointVector upright_left{};
+    ASSERT_TRUE(adapter.computeGravityCompensation(
+        state, upright_right, upright_left));
+
+    const Transform4x4RowMajor world_from_base_half_turn_x{
+        1.0, 0.0, 0.0, 0.0,
+        0.0, -1.0, 0.0, 0.0,
+        0.0, 0.0, -1.0, 0.0,
+        0.0, 0.0, 0.0, 1.0};
+    state.world_from_imu = multiplyTransforms(
+        world_from_base_half_turn_x,
+        adapter.contract().base_from_imu);
+
+    JointVector inverted_right{};
+    JointVector inverted_left{};
+    ASSERT_TRUE(adapter.computeGravityCompensation(
+        state, inverted_right, inverted_left));
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        EXPECT_NEAR(
+            inverted_right[index],
+            -upright_right[index],
+            kNumericalTolerance)
+            << "right joint index " << index;
+        EXPECT_NEAR(
+            inverted_left[index],
+            -upright_left[index],
+            kNumericalTolerance)
+            << "left joint index " << index;
+    }
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     NoncommutingImuPoseAndAsymmetricVelocitiesMatchFrozenRnea)
+{
+    const auto adapter = makeAdapter();
+    auto state = makeGoldenState(adapter);
+    state.world_from_imu = multiplyTransforms(
+        makeNoncommutingWorldFromBase(),
+        adapter.contract().base_from_imu);
+    state.right_velocity_rad_s =
+        {0.7, -0.4, 0.2, -0.1,
+         1.1, -0.8, 0.5, -0.3};
+    state.left_velocity_rad_s =
+        {-0.6, 0.9, -0.2, 0.4,
+         -1.0, 0.7, -0.5, 0.25};
+
+    JointVector right{};
+    JointVector left{};
+    ASSERT_TRUE(adapter.computeGravityCompensation(
+        state, right, left));
+    expectFinite(right);
+    expectFinite(left);
+    expectNear(
+        right,
+        {2.080988418159027,
+         -1.3277143244666021,
+         2.3194639604892364,
+         -2.0699331198781317,
+         0.40716561983947641,
+         -0.20631486209184946,
+         0.19335176308712126,
+         -0.01319194690287678});
+    expectNear(
+        left,
+        {-5.6976332776660232,
+         2.9915426497221254,
+         -2.5700109870406949,
+         2.1379083447512071,
+         -0.36441212045948712,
+         0.19226687457105307,
+         -0.085601264411386338,
+         0.0065129700338426369});
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     FixedModelRotationalProjectionMatchesFrozenValues)
+{
+    const auto adapter = makeAdapter();
+    const auto state = makeGoldenState(adapter);
+    RawHapticFeedback feedback;
+    feedback.shoulder_moment = {0.5, -0.2, 0.3};
+    feedback.elbow_effort = 1.2;
+    feedback.wrist_moment = {-0.4, 0.1, 0.6};
+    feedback.gripper_effort = -0.7;
+
+    ProjectedHapticEffort right{};
+    ASSERT_TRUE(adapter.projectHapticFeedback(
+        state, ArmSide::Right, feedback, right));
+    expectFinite(right.shoulder_joint_torque);
+    expectFinite(right.wrist_joint_torque);
+    expectNear(
+        right.shoulder_joint_torque,
+        {-0.5,
+         -0.12674237993402607,
+         -0.30915027509149773});
+    expectNear(
+        right.wrist_joint_torque,
+        {-0.065872923184419674,
+         -0.028130723042130143,
+         -0.72743566625468636});
+    EXPECT_DOUBLE_EQ(right.elbow_effort, feedback.elbow_effort);
+    EXPECT_DOUBLE_EQ(
+        right.gripper_effort,
+        feedback.gripper_effort);
+
+    ProjectedHapticEffort left{};
+    ASSERT_TRUE(adapter.projectHapticFeedback(
+        state, ArmSide::Left, feedback, left));
+    expectFinite(left.shoulder_joint_torque);
+    expectFinite(left.wrist_joint_torque);
+    expectNear(
+        left.shoulder_joint_torque,
+        {-0.5,
+         -0.36032671657345572,
+         0.083245914753940151});
+    expectNear(
+        left.wrist_joint_torque,
+        {-0.13497315246130309,
+         0.15764759010789803,
+         -0.70779204949804098});
+    EXPECT_DOUBLE_EQ(left.elbow_effort, feedback.elbow_effort);
+    EXPECT_DOUBLE_EQ(
+        left.gripper_effort,
+        feedback.gripper_effort);
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     RejectsNonRigidOrNonFiniteInputsAndZerosOutputs)
+{
+    const auto adapter = makeAdapter();
+    auto state = makeGoldenState(adapter);
+    JointVector right;
+    JointVector left;
+    right.fill(1.0);
+    left.fill(1.0);
+
+    state.world_from_imu = {};
+    EXPECT_FALSE(adapter.computeGravityCompensation(
+        state, right, left));
+    expectNear(right, JointVector{});
+    expectNear(left, JointVector{});
+
+    state = makeGoldenState(adapter);
+    state.right_position_rad[3] =
+        std::numeric_limits<double>::quiet_NaN();
+    right.fill(1.0);
+    left.fill(1.0);
+    EXPECT_FALSE(adapter.computeGravityCompensation(
+        state, right, left));
+    expectNear(right, JointVector{});
+    expectNear(left, JointVector{});
+
+    state = makeGoldenState(adapter);
+    RawHapticFeedback feedback;
+    feedback.shoulder_moment[1] =
+        std::numeric_limits<double>::infinity();
+    ProjectedHapticEffort projected;
+    projected.elbow_effort = 1.0;
+    EXPECT_FALSE(adapter.projectHapticFeedback(
+        state, ArmSide::Right, feedback, projected));
+    expectNear(
+        projected.shoulder_joint_torque,
+        Vector3{});
+    expectNear(projected.wrist_joint_torque, Vector3{});
+    EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0);
+    EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0);
+
+    feedback = {};
+    projected.elbow_effort = 1.0;
+    EXPECT_FALSE(adapter.projectHapticFeedback(
+        state,
+        static_cast<ArmSide>(99),
+        feedback,
+        projected));
+    expectNear(
+        projected.shoulder_joint_torque,
+        Vector3{});
+    expectNear(projected.wrist_joint_torque, Vector3{});
+    EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0);
+    EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0);
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     MissingModelFailsAtConstruction)
+{
+    EXPECT_THROW(
+        PinocchioUmeLegacyModelAdapter(
+            "/definitely/missing/ume_fixed.xml",
+            CMVR_UME_FLOATING_MODEL_PATH),
+        std::runtime_error);
+}
+
+TEST(PinocchioUmeLegacyModelAdapterTest,
+     RejectsModelRoleSwapEvenThoughBothMjcfFilesParse)
+{
+    try {
+        PinocchioUmeLegacyModelAdapter adapter(
+            CMVR_UME_FLOATING_MODEL_PATH,
+            CMVR_UME_FIXED_MODEL_PATH);
+        (void)adapter;
+        FAIL() << "swapped fixed/floating models were accepted";
+    } catch (const std::runtime_error& error) {
+        EXPECT_NE(
+            std::string(error.what()).find(
+                "UME fixed MJCF contract violation: "
+                "expected nq/nv/njoints 16/16/17"),
+            std::string::npos);
+    }
+}
+
+} // namespace
+} // namespace cmvr::ume_legacy

cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp

@@ -0,0 +1,216 @@
+#include "ume_legacy_controller.h"
+#include "ume_legacy_model_adapter.h"
+
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::ume_legacy {
+namespace {
+
+constexpr double kTolerance = 1e-12;
+
+void expectJointVectorNear(
+    const JointVector& actual,
+    const JointVector& expected,
+    double tolerance = kTolerance)
+{
+    for (std::size_t index = 0; index < kArmDof; ++index) {
+        EXPECT_NEAR(actual[index], expected[index], tolerance)
+            << "joint index " << index;
+    }
+}
+
+TEST(UmeLegacyControllerGoldenTest,
+     OriginalTuningAndFrictionMatchPythonOracle)
+{
+    const auto tuning = originalTuning();
+    EXPECT_DOUBLE_EQ(tuning.friction_coefficient[0], 1.6);
+    EXPECT_DOUBLE_EQ(tuning.friction_coefficient[4], 0.032);
+    EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4340_nm, 4.0);
+    EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4310_nm, 1.0);
+
+    const JointVector velocity{
+        -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0,
+        -10.0, -1.0, 1.0, 10.0};
+    const JointVector expected{
+        -0.4, -0.16000000000000003, 0.0,
+        0.055850536063818547,
+        -0.1, -0.032, 0.032, 0.1};
+
+    expectJointVectorNear(
+        frictionCompensation(velocity, tuning),
+        expected);
+}
+
+TEST(UmeLegacyControllerGoldenTest,
+     StictionUsesStrictLegacyThresholds)
+{
+    const auto tuning = originalTuning();
+    const double minimum =
+        tuning.stiction_threshold_min_rad_s[0];
+    const double maximum =
+        tuning.stiction_threshold_max_rad_s[0];
+    const JointVector at_threshold{
+        minimum,
+        -minimum,
+        maximum,
+        -maximum,
+        0.0, 0.0, 0.0, 0.0};
+    expectJointVectorNear(
+        stictionCompensation(at_threshold, tuning),
+        JointVector{});
+
+    const JointVector strictly_inside{
+        2.0 * minimum,
+        -2.0 * minimum,
+        std::nextafter(
+            minimum, std::numeric_limits<double>::infinity()),
+        std::nextafter(maximum, 0.0),
+        2.0 * minimum,
+        -2.0 * minimum,
+        std::nextafter(
+            minimum, std::numeric_limits<double>::infinity()),
+        std::nextafter(maximum, 0.0)};
+    expectJointVectorNear(
+        stictionCompensation(strictly_inside, tuning),
+        {0.5, -0.5, 0.5, 0.5,
+         0.0, 0.0, 0.0, 0.0});
+}
+
+TEST(UmeLegacyControllerGoldenTest,
+     FeedbackScaleMatchesLegacyNormAndTanhGoldenValues)
+{
+    const auto tuning = originalTuning();
+    EXPECT_NEAR(
+        feedbackScale(0.0, tuning),
+        0.20680448412090907,
+        kTolerance);
+    EXPECT_DOUBLE_EQ(
+        feedbackScale(tuning.feedback_error_tolerance_rad, tuning),
+        0.25);
+    EXPECT_NEAR(
+        feedbackScale(0.05, tuning),
+        0.32861047013244982,
+        kTolerance);
+    EXPECT_NEAR(
+        feedbackScale(-0.2, tuning),
+        0.48734517579834447,
+        kTolerance);
+}
+
+TEST(UmeLegacyControllerGoldenTest,
+     CompleteRightSideCommandMatchesPythonGoldenVector)
+{
+    SideControlInput input;
+    input.side = ArmSide::Right;
+    input.joint_velocity_rad_s = {
+        -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0,
+        -10.0, -1.0, 1.0, 10.0};
+    input.gravity_compensation_nm =
+        {0.5, -0.5, 1.0, -1.0,
+         0.25, -0.25, 0.75, -0.75};
+    input.projected_haptic.shoulder_joint_torque =
+        {1.2, -3.0, 10.0};
+    input.projected_haptic.elbow_effort = 2.0;
+    input.projected_haptic.wrist_joint_torque =
+        {0.5, -2.0, 5.0};
+    input.projected_haptic.gripper_effort = 3.0;
+    input.tracking_error.shoulder_rotation = {0.0, 0.0, 0.0};
+    input.tracking_error.elbow =
+        originalTuning().feedback_error_tolerance_rad;
+    input.tracking_error.wrist_rotation = {0.03, 0.04, 0.0};
+    input.tracking_error.gripper = -0.2;
+
+    const auto output = computeSideCommand(input);
+
+    expectJointVectorNear(
+        output.friction_compensation_nm,
+        {-0.4, -0.16000000000000003, 0.0,
+         0.055850536063818547,
+         -0.1, -0.032, 0.032, 0.1});
+    expectJointVectorNear(
+        output.stiction_compensation_nm,
+        {0.0, -0.5, 0.0, 0.5,
+         0.0, 0.0, 0.0, 0.0});
+    expectJointVectorNear(
+        output.feedforward_without_haptic_nm,
+        {0.099999999999999978, -1.1600000000000001,
+         1.0, -0.44414946393618149,
+         0.14999999999999999, -0.28200000000000003,
+         0.78200000000000003, -0.65000000000000002});
+    expectJointVectorNear(
+        output.signed_interaction_nm,
+        {1.2, -3.0, 10.0, 2.0,
+         0.5, -2.0, 5.0, -3.0});
+    expectJointVectorNear(
+        output.limited_feedback_nm,
+        {0.24816538094509089, -0.62041345236272716,
+         2.0680448412090908, 0.5,
+         0.16430523506622491, -0.65722094026489963,
+         1.0, -1.0});
+    expectJointVectorNear(
+        output.command_torque_nm,
+        {-0.14816538094509091, -0.53958654763727298,
+         -1.0680448412090908, -0.94414946393618149,
+         -0.014305235066224914, 0.37522094026489961,
+         -0.21799999999999997, 0.34999999999999998});
+}
+
+TEST(UmeLegacyControllerGoldenTest,
+     LeftAndRightScalarSignsAndGroupLimitsArePreserved)
+{
+    SideControlInput input;
+    input.projected_haptic.shoulder_joint_torque =
+        {100.0, -100.0, 100.0};
+    input.projected_haptic.elbow_effort = 100.0;
+    input.projected_haptic.wrist_joint_torque =
+        {100.0, -100.0, 100.0};
+    input.projected_haptic.gripper_effort = 100.0;
+    input.tracking_error.shoulder_rotation = {10.0, 0.0, 0.0};
+    input.tracking_error.elbow = 10.0;
+    input.tracking_error.wrist_rotation = {10.0, 0.0, 0.0};
+    input.tracking_error.gripper = 10.0;
+
+    input.side = ArmSide::Right;
+    const auto right = computeSideCommand(input);
+    expectJointVectorNear(
+        right.signed_interaction_nm,
+        {100.0, -100.0, 100.0, 100.0,
+         100.0, -100.0, 100.0, -100.0});
+    expectJointVectorNear(
+        right.command_torque_nm,
+        {-4.0, 4.0, -4.0, -4.0,
+         -1.0, 1.0, -1.0, 1.0});
+
+    input.side = ArmSide::Left;
+    const auto left = computeSideCommand(input);
+    expectJointVectorNear(
+        left.signed_interaction_nm,
+        {100.0, -100.0, 100.0, -100.0,
+         100.0, -100.0, 100.0, 100.0});
+    expectJointVectorNear(
+        left.command_torque_nm,
+        {-4.0, 4.0, -4.0, 4.0,
+         -1.0, 1.0, -1.0, -1.0});
+}
+
+TEST(UmeLegacyControllerGoldenTest,
+     FeedbackClipDoesNotClampOtherFeedforwardTerms)
+{
+    SideControlInput input;
+    input.gravity_compensation_nm =
+        {50.0, -50.0, 0.0, 0.0, 0.0, 0.0, 20.0, -20.0};
+    const auto output = computeSideCommand(input);
+
+    EXPECT_DOUBLE_EQ(output.command_torque_nm[0], 50.0);
+    EXPECT_DOUBLE_EQ(output.command_torque_nm[1], -50.0);
+    EXPECT_DOUBLE_EQ(output.command_torque_nm[6], 20.0);
+    EXPECT_DOUBLE_EQ(output.command_torque_nm[7], -20.0);
+}
+
+} // namespace
+} // namespace cmvr::ume_legacy

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

@@ -103,6 +103,11 @@ struct JointGroupState {
     std::vector<double> position;
     std::vector<double> velocity;
     std::vector<double> effort;
+    std::uint64_t sequence{0};
+    std::int64_t sample_monotonic_ns{0};
+    bool position_valid{false};
+    bool velocity_valid{false};
+    bool effort_valid{false};
 
     bool validForModel(const RobotModel& model) const
     {
@@ -165,6 +170,14 @@ struct ServoOptions {
     double gain{300.0};
 };
 
+struct TorqueServoOptions {
+    // The UME legacy loop runs at 800 Hz by default.
+    double period{0.00125};
+    // A producer must continuously refresh the latest torque command. A stale
+    // command latches a fault and disables the actuator chain.
+    std::uint32_t command_watchdog_ms{20};
+};
+
 enum class RobotMode {
     Unknown = 0,
     Disconnected,
@@ -197,6 +210,14 @@ enum class ControlMode {
     Freedrive
 };
 
+enum class JointEffortSource {
+    Unspecified = 0,
+    MotorEstimate,
+    JointSensor,
+    ForceTorqueSensor,
+    Observer
+};
+
 struct ArmState {
     double timestamp{0.0};
     RobotMode robot_mode{RobotMode::Unknown};

cmvr-es/config/README.md

@@ -18,6 +18,8 @@ cmvr_es.pb.txt
 入口文件：
 
 - [`cmvr_es.pb.txt`](cmvr_es.pb.txt)
+- [`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)：UME 主端样例
+- [`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)：人形机械臂从端样例
 - [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt)
 - [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt)
 
@@ -29,10 +31,10 @@ cmvr_es.pb.txt
 <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`，不会自动跳到安装目录。传入显式根配置时：
+安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时使用 `--config`：
 
 ```bash
-./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt
+./output/bin/cmvr_es --config /etc/cmvr-es/cmvr_es.pb.txt
 ```
 
 设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。
@@ -109,6 +111,48 @@ output/bin/protoc \
 
 该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。
 
+## 双边遥操部署样例
+
+仓库提供两个相互独立的 CMVR-ES 配置入口：
+
+- UME 主端：[`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)，只声明
+  `ume_left` 和 `ume_right`。两条机械臂在 DeviceManager 层默认关闭，
+  [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的
+  `hardware_enabled` 也默认关闭；两层开关必须经过标定与安全验收后分别启用。
+  出站 `ume_teleop` Task 默认关闭，样例不包含机器人地址或凭据。当前 Task
+  只实现会话/重连/心跳和 latest-only 指令邮箱，尚无生产算法调用
+  `submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。
+- 人形机械臂从端：[`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)，通用 gRPC
+  server 可以启动，但 `ti5_motors` 和 `right_arm` 仍默认关闭。生产
+  `ArmTeleop` 已实现真实 `RobotArm` 适配，但服务配置的 `enable` 显式关闭且
+  样例哈希故意留空。`MotorRobotArm.enable_teleop_group_servo` 目前只是预留字段；
+  因为现有 `servoJ` 仍是逐关节顺序写，代码即使看到该字段为 true 也会拒绝能力。
+  必须先实现并验收原子或定时的组下发原语。因此启动 gRPC server 不等于允许遥操
+  执行，也不能绕过设备层硬件门。
+
+在两台边缘设备各自的源码或安装目录运行：
+
+```bash
+# UME 主端（源码配置）
+./output/bin/cmvr_es \
+  --config ./cmvr-es/config/cmvr_es_ume.pb.txt
+
+# 人形机械臂从端（源码配置）
+./output/bin/cmvr_es \
+  --config ./cmvr-es/config/cmvr_es_robot.pb.txt
+```
+
+如果使用安装后的配置副本，则相应命令为：
+
+```bash
+./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_ume.pb.txt
+./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_robot.pb.txt
+```
+
+上线前应把两套配置分别复制到两台机器的外部配置目录。主端需要填写从端地址、
+会话 manifest 和认证配置；从端需要换成现场机械臂设备配置，并在真实硬件测试后
+逐层开启。不要把生产 IP、token、私钥或设备标定值提交到仓库样例。
+
 ## 生产配置
 
 `cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。

cmvr-es/config/cmvr_es_robot.pb.txt

@@ -0,0 +1,11 @@
+# Follower robot-side CMVR-ES profile.
+#
+# The RobotArm ArmTeleop backend is implemented but explicitly disabled. The
+# physical arm and service backend remain closed. Current MotorRobotArm
+# sequential joint writes are rejected as a teleop group-servo capability until
+# an atomic/timed group primitive and its safety timing gates are accepted.
+cmvr_es {
+  logger_config_file: "logger/logger.pb.txt"
+  device_manager_config_file: "manager/device_manager_robot.pb.txt"
+  task_manager_config_file: "manager/task_manager_robot.pb.txt"
+}

cmvr-es/config/cmvr_es_ume.pb.txt

@@ -0,0 +1,10 @@
+# UME leader-side CMVR-ES profile.
+#
+# All relative paths below are resolved from this file's directory. This
+# checked-in profile contains no production endpoint, credentials or hardware
+# enablement.
+cmvr_es {
+  logger_config_file: "logger/logger.pb.txt"
+  device_manager_config_file: "manager/device_manager_ume.pb.txt"
+  task_manager_config_file: "manager/task_manager_ume.pb.txt"
+}

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

@@ -17,6 +17,10 @@ arm {
       buffer_size: 50
       default_vel: 1.0
       default_acc: 2.0
+      # Reserved only: current MotorRobotArm servoJ writes joints sequentially,
+      # so code rejects the teleop group-servo capability even if this is true.
+      # A reviewed atomic/timed group primitive is required before changing it.
+      enable_teleop_group_servo: false
     }
 
     kinematics {

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

@@ -0,0 +1,72 @@
+# UME leader-arm device templates. They are deliberately disabled in
+# manager/device_manager.pb.txt and hardware_enabled remains false here.
+#
+# Before real hardware use, independently verify interface bitrate
+# (1 Mbit/s arbitration, 5 Mbit/s data, FD+BRS), motor/feedback IDs,
+# direction, zero offsets, mechanical joint limits and safe torque limits.
+# Each joint must also receive reviewed healthy_feedback_status and raw
+# temperature thresholds. They are deliberately absent below, so changing
+# hardware_enabled alone is insufficient to arm these placeholder profiles.
+arm {
+  robot_arms {
+    id: "ume_right"
+    ume {
+      can {
+        dev_id: "can4"
+        channel_id: 4
+        interface_name: "can4"
+        enable_fd: true
+        bitrate_switch: true
+        send_timeout_us: 100
+        receive_timeout_us: 100
+        receive_own_messages: false
+        enable_error_frames: true
+      }
+      control_frequency_hz: 800
+      cycle_deadline_us: 1000
+      feedback_watchdog_ms: 20
+      shutdown_timeout_ms: 50
+      hardware_enabled: false
+
+      joints { joint_name: "RJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "RJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "RJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "RJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "RJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+      joints { joint_name: "RJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+      joints { joint_name: "RJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+      joints { joint_name: "RJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+    }
+  }
+
+  robot_arms {
+    id: "ume_left"
+    ume {
+      can {
+        dev_id: "can5"
+        channel_id: 5
+        interface_name: "can5"
+        enable_fd: true
+        bitrate_switch: true
+        send_timeout_us: 100
+        receive_timeout_us: 100
+        receive_own_messages: false
+        enable_error_frames: true
+      }
+      control_frequency_hz: 800
+      cycle_deadline_us: 1000
+      feedback_watchdog_ms: 20
+      shutdown_timeout_ms: 50
+      hardware_enabled: false
+
+      joints { joint_name: "LJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "LJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "LJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "LJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
+      joints { joint_name: "LJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+      joints { joint_name: "LJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+      joints { joint_name: "LJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+      joints { joint_name: "LJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
+    }
+  }
+}

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

@@ -0,0 +1,56 @@
+motor {
+  id: "plc_motors"
+
+  motor_groups {
+    id: "plc_axis_group"
+    bus_type: MOTOR_BUS_MODBUS_TCP
+    vendor: MOTOR_VENDOR_PLC_GENERIC
+    protocol: MOTOR_PROTOCOL_CMVR_PLC_V1
+
+    modbus_tcp {
+      host: "192.168.0.10"
+      port: 502
+      unit_id: 1
+
+      connect_timeout_ms: 500
+      io_timeout_ms: 100
+      heartbeat_period_ms: 100
+      communication_watchdog_ms: 1000
+      cyclic_watchdog_ms: 500
+      status_poll_period_ms: 20
+      reconnect_min_ms: 100
+      reconnect_max_ms: 2000
+      command_ack_timeout_ms: 500
+
+      protocol_major: 1
+      protocol_minor: 0
+
+      axes { motor_id: 1 axis_index: 0 }
+      axes { motor_id: 2 axis_index: 1 }
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints {
+        joint_name: "PLC_AXIS_1"
+        q_lb: -3.141592653589793
+        q_ub: 3.141592653589793
+        qd: 1.0
+        qdd: 2.0
+      }
+      joints {
+        joint_name: "PLC_AXIS_2"
+        q_lb: -1.5707963267948966
+        q_ub: 1.5707963267948966
+        qd: 0.5
+        qdd: 1.0
+      }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "PLC_AXIS_1" }
+      motors { id: 2 joint_name: "PLC_AXIS_2" }
+    }
+  }
+}

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

@@ -82,6 +82,14 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "plc_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/plc_motors.pb.txt"
+    # Configure the PLC endpoint and complete the safety checkout before enabling.
+    enable: false
+  }
+
   devices {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
@@ -103,6 +111,23 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "ume_right"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/ume_arms.pb.txt"
+    # Two gates must be explicitly changed after the physical safety review:
+    # this entry and ume.hardware_enabled in the arm config.
+    enable: false
+  }
+
+  devices {
+    id: "ume_left"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/ume_arms.pb.txt"
+    # Two gates must be explicitly changed after the physical safety review.
+    enable: false
+  }
+
   devices {
     id: "bio_head"
     type: DEVICE_TYPE_BIO_HEAD_ROBOT

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

@@ -0,0 +1,24 @@
+# Follower robot-side devices.
+device_manager {
+  name: "cmvr_es_robot"
+  version: "0.1"
+  description: "CMVR humanoid follower edge system"
+  init_all_motors_when_no_active_joints: false
+
+  devices {
+    id: "ti5_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    # Physical motor communication remains fail-closed in this example.
+    enable: false
+  }
+
+  devices {
+    id: "right_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm.pb.txt"
+    # Do not enable until the motor system, URDF, limits, servoJ timing and
+    # independent emergency-stop path have passed the robot safety checkout.
+    enable: false
+  }
+}

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

@@ -0,0 +1,24 @@
+# UME leader-side devices only.
+device_manager {
+  name: "cmvr_es_ume"
+  version: "0.1"
+  description: "UME leader edge system"
+  init_all_motors_when_no_active_joints: false
+
+  devices {
+    id: "ume_right"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/ume_arms.pb.txt"
+    # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config.
+    # Keep both false until CAN mapping, limits and physical safety are verified.
+    enable: false
+  }
+
+  devices {
+    id: "ume_left"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/ume_arms.pb.txt"
+    # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config.
+    enable: false
+  }
+}

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

@@ -30,4 +30,13 @@ task_manager {
     # Host-development default: no QUIC Gateway or physical media devices.
     enable: true
   }
+  tasks {
+    id: "ume_teleop"
+    type: TASK_TYPE_UME_TELEOP
+    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
+    config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt"
+    # Fail-safe default: configure the remote robot endpoint, manifest and
+    # deployment security policy before enabling this outbound control task.
+    enable: false
+  }
 }

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

@@ -0,0 +1,14 @@
+# Follower robot-side tasks.
+task_manager {
+  tasks {
+    id: "grpc_server"
+    type: TASK_TYPE_GRPC_SERVER
+    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
+    config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt"
+    # The generic gRPC server may be enabled for integration. This does not
+    # enable a physical arm: device entries and the implemented RobotArm
+    # ArmTeleop adapter are explicitly disabled. Current MotorRobotArm
+    # sequential joint dispatch also fails the group-servo capability gate.
+    enable: true
+  }
+}

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

@@ -0,0 +1,12 @@
+# UME leader-side tasks.
+task_manager {
+  tasks {
+    id: "ume_teleop"
+    type: TASK_TYPE_UME_TELEOP
+    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
+    config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt"
+    # Fail-closed: configure the follower endpoint, expected manifest and
+    # transport security before enabling outbound teleoperation.
+    enable: false
+  }
+}

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

@@ -5,4 +5,22 @@ grpc_server {
   enable_reflection: true
   camera_stream_max_pending_frames: 2
   camera_stream_max_frame_age_ms: 250
+
+  # The RobotArm adapter is implemented, but remains explicitly closed until
+  # the device itself enables teleop group servo, real hashes are provisioned,
+  # and group-write timing and independent stop behavior pass hardware review.
+  arm_teleop_backend {
+    enable: false
+    device_id: "right_arm"
+    # Deliberately empty placeholders are invalid when enable=true.
+    model_sha256: ""
+    calibration_sha256: ""
+    base_frame: "PELVIS_S"
+    tool_frame: "R_FINGER_TIP_FIXED"
+    servo_period_s: 0.001
+    max_apply_duration_us: 800
+    require_powered: true
+    max_initial_position_step_rad: 0.02
+    max_position_step_rad: 0.003
+  }
 }

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

@@ -0,0 +1,35 @@
+ume_teleop {
+  id: "ume_teleop"
+
+  # Deliberately left empty. The TaskManager entry is disabled by default, and
+  # init fails closed if it is enabled before a robot endpoint is configured.
+  server_address: ""
+
+  # M6 implements explicit insecure transport for isolated development only.
+  # Production deployment must add and configure channel credentials first.
+  allow_insecure: false
+
+  open_session {
+    protocol_major: 1
+    protocol_minor: 0
+    client_instance_id: "ume-controller"
+    requested_command_rate_hz: 250
+    requested_state_rate_hz: 250
+    watchdog_timeout_ms: 100
+    requested_lease_ms: 500
+
+    # Replace with the manifest exported by the CMVR-ES robot instance.
+    expected_robot {
+      robot_id: ""
+      position_unit: "rad"
+      velocity_unit: "rad/s"
+      effort_unit: "N*m"
+    }
+  }
+
+  reconnect {
+    initial_delay_ms: 100
+    maximum_delay_ms: 5000
+    multiplier: 2.0
+  }
+}

cmvr-es/devices/arm/CMakeLists.txt

@@ -1,6 +1,7 @@
 add_subdirectory(motor_robot_arm)
 add_subdirectory(aubo_arm)
 add_subdirectory(huayan_arm)
+add_subdirectory(ume_robot_arm)
 
 add_library(robot_arm INTERFACE)
 
@@ -11,6 +12,7 @@ target_link_libraries(robot_arm
         cmvr_es::device::motor_robot_arm
         cmvr_es::device::aubo_arm
         cmvr_es::device::huayan_arm
+        cmvr_es::device::ume_robot_arm
         cmvr_es::proto
 )
 

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

@@ -46,7 +46,54 @@ target_link_libraries(aubo_arm
         cmvr_es::proto
         PRIVATE
         glog
+        jsoncpp
 )
 
 add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm)
 install(TARGETS aubo_arm LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    enable_testing()
+    add_executable(aubo_arm_json_command_test
+            tests/aubo_arm_json_command_test.cpp
+    )
+    target_link_libraries(aubo_arm_json_command_test
+            PRIVATE
+            cmvr_es::device::aubo_arm
+            cmvr_es::proto
+    )
+    add_test(
+            NAME aubo_arm_json_command_test
+            COMMAND aubo_arm_json_command_test
+    )
+    set_tests_properties(aubo_arm_json_command_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(UNIX AND NOT APPLE)
+        # The imported AUBO target still contributes its vendor directory to
+        # direct consumers' build-tree RUNPATH. Put the system runtime first
+        # for this test; installed artifacts exclude the vendor libstdc++.
+        execute_process(
+                COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6
+                OUTPUT_VARIABLE AUBO_TEST_SYSTEM_LIBSTDCXX
+                OUTPUT_STRIP_TRAILING_WHITESPACE
+        )
+        if(EXISTS "${AUBO_TEST_SYSTEM_LIBSTDCXX}")
+            get_filename_component(
+                    AUBO_TEST_SYSTEM_LIBSTDCXX_REAL
+                    "${AUBO_TEST_SYSTEM_LIBSTDCXX}"
+                    REALPATH
+            )
+            get_filename_component(
+                    AUBO_TEST_SYSTEM_LIBSTDCXX_DIR
+                    "${AUBO_TEST_SYSTEM_LIBSTDCXX_REAL}"
+                    DIRECTORY
+            )
+            set_property(
+                    TARGET aubo_arm_json_command_test
+                    PROPERTY BUILD_RPATH "${AUBO_TEST_SYSTEM_LIBSTDCXX_DIR}"
+            )
+        endif()
+    endif()
+endif()

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

@@ -1,12 +1,15 @@
 #include "devices/arm/aubo_arm/aubo_arm.h"
 
 #include <algorithm>
+#include <cctype>
 #include <chrono>
+#include <cstring>
 #include <exception>
 #include <thread>
 #include <tuple>
 
 #include "common/base/logging/logger.h"
+#include "json/json.h"
 
 #include "aubo_sdk/rpc.h"
 
@@ -44,6 +47,103 @@ using arcs::aubo_sdk::RobotInterfacePtr;
 
 constexpr int kAuboServoMode = 3;
 
+enum class CabinetIoOperation {
+    GetDigitalInput,
+    GetDigitalOutput,
+    SetDigitalOutput,
+};
+
+std::string lowerString(std::string value)
+{
+    std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char c) {
+        return static_cast<char>(std::tolower(c));
+    });
+    return value;
+}
+
+bool parseJsonCommand(const std::string& request_json,
+                      Json::Value& root,
+                      std::string& error)
+{
+    Json::CharReaderBuilder builder;
+    std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
+    return reader->parse(
+        request_json.data(),
+        request_json.data() + request_json.size(),
+        &root,
+        &error);
+}
+
+std::string compactJson(const Json::Value& value)
+{
+    Json::StreamWriterBuilder builder;
+    builder[std::string("indentation")] = "";
+    return Json::writeString(builder, value);
+}
+
+Json::Value& jsonMember(Json::Value& root, const char* name)
+{
+    return *root.demand(name, name + std::strlen(name));
+}
+
+const Json::Value* findJsonMember(const Json::Value& root, const char* name)
+{
+    return root.find(name, name + std::strlen(name));
+}
+
+bool requiredJsonString(const Json::Value& root,
+                        const char* name,
+                        std::string& value)
+{
+    const Json::Value* member = findJsonMember(root, name);
+    if (!member || !member->isString() || member->asString().empty()) {
+        return false;
+    }
+    value = member->asString();
+    return true;
+}
+
+bool requiredJsonInt(const Json::Value& root, const char* name, int& value)
+{
+    const Json::Value* member = findJsonMember(root, name);
+    if (!member || !member->isInt()) {
+        return false;
+    }
+    value = member->asInt();
+    return true;
+}
+
+bool requiredJsonBool(const Json::Value& root, const char* name, bool& value)
+{
+    const Json::Value* member = findJsonMember(root, name);
+    if (!member || !member->isBool()) {
+        return false;
+    }
+    value = member->asBool();
+    return true;
+}
+
+bool parseCabinetIoOperation(const std::string& name, CabinetIoOperation& operation)
+{
+    const std::string normalized = lowerString(name);
+    if (normalized == "get_di") {
+        operation = CabinetIoOperation::GetDigitalInput;
+    } else if (normalized == "get_do") {
+        operation = CabinetIoOperation::GetDigitalOutput;
+    } else if (normalized == "set_do") {
+        operation = CabinetIoOperation::SetDigitalOutput;
+    } else {
+        return false;
+    }
+    return true;
+}
+
+std::string standardOutputRunstateName(
+    const arcs::common_interface::StandardOutputRunState runstate)
+{
+    return arcs::common_interface::toString(runstate);
+}
+
 RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
                                            const std::string& context,
                                            Result& result)
@@ -179,6 +279,142 @@ bool AuboArm::stop()
     return stopMotion().ok();
 }
 
+bool AuboArm::executeJsonCommand(const std::string& request_json,
+                                 std::string& response_json)
+{
+    Json::Value response(Json::objectValue);
+    jsonMember(response, "success") = false;
+    const auto fail = [&](const std::string& error_code,
+                          const std::string& error_message) {
+        jsonMember(response, "success") = false;
+        jsonMember(response, "error_code") = error_code;
+        jsonMember(response, "error_message") = error_message;
+        response_json = compactJson(response);
+        return false;
+    };
+
+    Json::Value root;
+    std::string parse_error;
+    if (!parseJsonCommand(request_json, root, parse_error)) {
+        return fail("invalid_json", "invalid json: " + parse_error);
+    }
+    if (!root.isObject()) {
+        return fail("invalid_json", "invalid json: root must be an object");
+    }
+
+    std::string command;
+    if (!requiredJsonString(root, "command", command)) {
+        return fail("invalid_argument",
+                    "field 'command' is required and must be a non-empty string");
+    }
+    command = lowerString(command);
+    if (command != "cabinet_io") {
+        return fail("unsupported_command", "unsupported json command: " + command);
+    }
+    jsonMember(response, "command") = command;
+
+    std::string operation_name;
+    if (!requiredJsonString(root, "operation", operation_name)) {
+        return fail("invalid_argument",
+                    "field 'operation' is required and must be a non-empty string");
+    }
+    operation_name = lowerString(operation_name);
+    CabinetIoOperation operation{};
+    if (!parseCabinetIoOperation(operation_name, operation)) {
+        return fail("invalid_operation",
+                    "unsupported cabinet_io operation: " + operation_name);
+    }
+    jsonMember(response, "operation") = operation_name;
+
+    int index = -1;
+    if (!requiredJsonInt(root, "index", index) || index < 0) {
+        return fail("invalid_argument",
+                    "field 'index' is required and must be a non-negative JSON integer");
+    }
+    jsonMember(response, "index") = index;
+
+    bool output_value = false;
+    if (operation == CabinetIoOperation::SetDigitalOutput &&
+        !requiredJsonBool(root, "value", output_value)) {
+        return fail("invalid_argument",
+                    "field 'value' is required for set_do and must be a JSON boolean");
+    }
+
+    std::lock_guard lock(mutex_);
+    const auto ready = ensureConnected_("cabinet_io");
+    if (!ready.ok()) {
+        return fail("not_connected", ready.message);
+    }
+
+    try {
+        Result interface_result;
+        auto robot_interface =
+            getPrimaryRobotInterface(sdk_->rpc_client, "cabinet_io", interface_result);
+        if (!interface_result.ok() || !robot_interface) {
+            return fail("robot_interface_unavailable", interface_result.message);
+        }
+
+        auto io = robot_interface->getIoControl();
+        if (!io) {
+            return fail("io_interface_unavailable",
+                        "[AuboArm] cabinet_io failed: IO interface is null");
+        }
+
+        const bool is_input = operation == CabinetIoOperation::GetDigitalInput;
+        const int count = is_input
+            ? io->getStandardDigitalInputNum()
+            : io->getStandardDigitalOutputNum();
+        jsonMember(response, "count") = count;
+        if (index >= count) {
+            return fail(
+                "index_out_of_range",
+                "[AuboArm] cabinet_io index out of range: index=" +
+                    std::to_string(index) + ", count=" + std::to_string(count));
+        }
+
+        if (operation == CabinetIoOperation::GetDigitalInput) {
+            jsonMember(response, "value") = io->getStandardDigitalInput(index);
+        } else {
+            const auto runstate = io->getStandardDigitalOutputRunstate(index);
+            jsonMember(response, "runstate") = standardOutputRunstateName(runstate);
+            jsonMember(response, "runstate_code") = static_cast<int>(runstate);
+
+            if (operation == CabinetIoOperation::GetDigitalOutput) {
+                jsonMember(response, "value") =
+                    io->getStandardDigitalOutput(index);
+            } else {
+                if (runstate != arcs::common_interface::StandardOutputRunState::None) {
+                    return fail(
+                        "output_managed_by_runstate",
+                        "[AuboArm] cabinet_io set_do rejected: output is managed by "
+                        "controller runstate; configure this channel as None before writing");
+                }
+
+                const int ret = io->setStandardDigitalOutput(index, output_value);
+                jsonMember(response, "sdk_return_code") = ret;
+                if (ret != 0) {
+                    return fail(
+                        "sdk_command_failed",
+                        "[AuboArm] cabinet_io set_do failed: sdk ret=" +
+                            std::to_string(ret));
+                }
+                jsonMember(response, "requested_value") = output_value;
+            }
+        }
+
+        jsonMember(response, "success") = true;
+        response_json = compactJson(response);
+        return true;
+    } catch (const arcs::common_interface::AuboException& e) {
+        jsonMember(response, "sdk_return_code") = e.code();
+        return fail("sdk_exception",
+                    std::string("[AuboArm] cabinet_io failed: ") + e.what());
+    } catch (const std::exception& e) {
+        return fail("sdk_exception",
+                    std::string("[AuboArm] cabinet_io failed: ") + e.what());
+    }
+}
+
 ArmState AuboArm::getRobotState() const
 {
     ArmState state;
@@ -788,6 +1024,7 @@ Result AuboArm::stopServoMode()
 
 Result AuboArm::connect(const std::string& ip, const int port)
 {
+    std::lock_guard lock(mutex_);
     if (connected_.load()) {
         return Result::success();
     }
@@ -855,6 +1092,7 @@ Result AuboArm::connect(const std::string& ip, const int port)
 
 Result AuboArm::disconnect()
 {
+    std::lock_guard lock(mutex_);
     try {
         if (sdk_ && sdk_->rpc_client) {
             if (sdk_->rpc_client->hasLogined()) {

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

@@ -21,6 +21,8 @@ public:
     std::string typeName() const override { return "AuboARM"; }
     bool init() override;
     bool stop() override;
+    bool executeJsonCommand(const std::string& request_json,
+                            std::string& response_json) override;
 
     RobotModel getRobotModel() const override { return model_; }
     std::size_t getDof() const override { return model_.dof; }

cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp

@@ -0,0 +1,84 @@
+#include "devices/arm/aubo_arm/aubo_arm.h"
+
+#include <iostream>
+#include <string>
+
+namespace {
+
+#define CHECK_TRUE(condition)                                                        \
+    do {                                                                             \
+        if (!(condition)) {                                                          \
+            std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": "           \
+                      << #condition << std::endl;                                     \
+            return 1;                                                                \
+        }                                                                            \
+    } while (false)
+
+bool contains(const std::string& value, const std::string& expected)
+{
+    return value.find(expected) != std::string::npos;
+}
+
+cmvr::config::RobotArmConfig makeConfig()
+{
+    cmvr::config::RobotArmConfig config;
+    config.set_id("aubo_arm_json_test");
+    auto* vendor = config.mutable_vendor();
+    vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM);
+    vendor->set_model("AuboTest");
+    vendor->set_dof(6);
+    return config;
+}
+
+} // namespace
+
+int main()
+{
+    cmvr::device::AuboArm arm(makeConfig());
+    cmvr::device::AbstractDevice* device = &arm;
+    std::string response;
+
+    CHECK_TRUE(!device->executeJsonCommand("{", response));
+    CHECK_TRUE(contains(response, R"("error_code":"invalid_json")"));
+
+    CHECK_TRUE(!device->executeJsonCommand("[]", response));
+    CHECK_TRUE(contains(response, R"("error_code":"invalid_json")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"ptz","operation":"get_di","index":0})", response));
+    CHECK_TRUE(contains(response, R"("error_code":"unsupported_command")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"cabinet_io","operation":"get_ai","index":0})", response));
+    CHECK_TRUE(contains(response, R"("error_code":"invalid_operation")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"cabinet_io","operation":"get_di","index":-1})", response));
+    CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"cabinet_io","operation":"set_do","index":0,"value":1})",
+        response));
+    CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"cabinet_io","operation":"set_do","index":0})", response));
+    CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"cabinet_io","operation":"get_di","index":0})", response));
+    CHECK_TRUE(contains(response, R"("error_code":"not_connected")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"cabinet_io","operation":"get_do","index":0})", response));
+    CHECK_TRUE(contains(response, R"("operation":"get_do")"));
+    CHECK_TRUE(contains(response, R"("error_code":"not_connected")"));
+
+    CHECK_TRUE(!device->executeJsonCommand(
+        R"({"command":"cabinet_io","operation":"set_do","index":0,"value":true})",
+        response));
+    CHECK_TRUE(contains(response, R"("operation":"set_do")"));
+    CHECK_TRUE(contains(response, R"("error_code":"not_connected")"));
+
+    return 0;
+}

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

@@ -36,6 +36,13 @@ public:
     RobotMode getRobotMode() const override { return RobotMode::Idle; }
     SafetyMode getSafetyMode() const override;
     ControlMode getControlMode() const override { return ControlMode::Position; }
+    bool supportsTeleopGroupServo() const noexcept override
+    {
+        // commandCyclicPosition is currently dispatched one joint at a time.
+        // A config switch cannot turn that partial-write behavior into the
+        // atomic/timed group primitive required by network teleoperation.
+        return false;
+    }
 
     Result torqueOn() override;
     Result torqueOff() override;
@@ -125,6 +132,8 @@ private:
 
     mutable std::mutex mutex_;
     std::atomic<bool> busy_{false};
+    std::atomic<bool> powered_on_{false};
+    mutable std::atomic<std::uint64_t> joint_state_sequence_{0};
     double speed_scaling_{1.0};
     bool emergency_stopped_{false};
     ServoOptions servo_options_;

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

@@ -1,6 +1,7 @@
 #include "arm/motor_robot_arm/include/motor_robot_arm.h"
 
 #include <chrono>
+#include <cmath>
 #include <Eigen/Dense>
 #include <stdexcept>
 #include <thread>
@@ -28,6 +29,23 @@ struct BusyGuard {
     ~BusyGuard() { busy.store(false); }
 };
 
+const config::JointLimitsConfig* configuredJointLimits(
+    const config::ArmKinematicsConfig& kinematics)
+{
+    switch (kinematics.algorithm_case()) {
+        case config::ArmKinematicsConfig::kPinocchioDlsIkSolver:
+            return &kinematics.pinocchio_dls_ik_solver()
+                        .joint_limit_policy()
+                        .limits();
+        case config::ArmKinematicsConfig::kPinocchioQpIkSolver:
+            return &kinematics.pinocchio_qp_ik_solver()
+                        .joint_limit_policy()
+                        .limits();
+        default:
+            return nullptr;
+    }
+}
+
 } // namespace
 
 MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
@@ -66,6 +84,39 @@ MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
     model_.manufacturer = "cmvr";
     model_.dof = static_cast<std::size_t>(dof_);
     model_.joint_names = joint_names_;
+
+    const auto* configured_limits = configuredJointLimits(cfg_.kinematics());
+    if (configured_limits != nullptr && configured_limits->enable() &&
+        configured_limits->source() ==
+            config::JOINT_LIMIT_SOURCE_CUSTOM &&
+        configured_limits->joints_size() == dof_) {
+        bool valid_limits = true;
+        model_.joint_limits.reserve(static_cast<std::size_t>(dof_));
+        for (int index = 0; index < dof_; ++index) {
+            const auto& source = configured_limits->joints(index);
+            if (source.joint_name() != joint_names_[static_cast<std::size_t>(index)] ||
+                !std::isfinite(source.q_lb()) ||
+                !std::isfinite(source.q_ub()) ||
+                !std::isfinite(source.qd()) ||
+                source.q_lb() >= source.q_ub() ||
+                source.qd() <= 0.0) {
+                valid_limits = false;
+                break;
+            }
+            JointLimit limit;
+            limit.lower = source.q_lb();
+            limit.upper = source.q_ub();
+            limit.max_velocity = source.qd();
+            limit.max_acceleration = source.qdd();
+            model_.joint_limits.push_back(limit);
+        }
+        if (!valid_limits) {
+            model_.joint_limits.clear();
+            CMVR_LOG(ERROR)
+                << "[MotorRobotArm] invalid or misordered custom joint limits: "
+                << id_;
+        }
+    }
 }
 
 MotorRobotArm::~MotorRobotArm()
@@ -134,7 +185,7 @@ ArmState MotorRobotArm::getRobotState() const
 {
     ArmState state;
     state.connected = motor_manager_ != nullptr;
-    state.powered_on = true;
+    state.powered_on = powered_on_.load(std::memory_order_acquire);
     state.brake_released = !emergency_stopped_;
     state.moving = busy();
     state.emergency_stopped = emergency_stopped_;
@@ -154,15 +205,35 @@ JointGroupState MotorRobotArm::getJointState() const
     state.position.reserve(joint_names_.size());
     state.velocity.reserve(joint_names_.size());
     state.effort.reserve(joint_names_.size());
+    bool values_valid = true;
     for (const auto& joint_name : joint_names_) {
         auto motor = getMotor_(joint_name);
         if (!motor) {
+            values_valid = false;
             continue;
         }
-        state.position.push_back(motor->getQ());
+        const double position = motor->getQ();
-        state.velocity.push_back(motor->getQd());
+        const double velocity = motor->getQd();
+        values_valid =
+            values_valid && std::isfinite(position) && std::isfinite(velocity);
+        state.position.push_back(position);
+        state.velocity.push_back(velocity);
         state.effort.push_back(0.0);
     }
+    values_valid =
+        values_valid && state.position.size() == joint_names_.size() &&
+        state.velocity.size() == joint_names_.size();
+    state.sequence =
+        joint_state_sequence_.fetch_add(1, std::memory_order_relaxed) + 1;
+    state.sample_monotonic_ns =
+        std::chrono::duration_cast<std::chrono::nanoseconds>(
+            std::chrono::steady_clock::now().time_since_epoch())
+            .count();
+    state.position_valid = values_valid;
+    state.velocity_valid = values_valid;
+    // MotorRobotArm currently has no verified effort feedback path. The zero
+    // placeholders above must never be advertised as measured torque.
+    state.effort_valid = false;
     return state;
 }
 
@@ -202,11 +273,15 @@ Result MotorRobotArm::torqueOn()
         }
     }
     emergency_stopped_ = false;
+    powered_on_.store(true, std::memory_order_release);
     return Result::success();
 }
 
 Result MotorRobotArm::torqueOff()
 {
+    // Until every joint reports a successful disable, the aggregate powered
+    // state is unknown and therefore must not satisfy a require_powered gate.
+    powered_on_.store(false, std::memory_order_release);
     for (const auto& joint_name : joint_names_) {
         auto motor = getMotor_(joint_name);
         if (!motor) {

cmvr-es/devices/arm/robot_arm.h

@@ -28,6 +28,15 @@ public:
     virtual SafetyMode getSafetyMode() const = 0;
     virtual ControlMode getControlMode() const = 0;
 
+    // ArmTeleop requires an explicitly reviewed group-servo implementation.
+    // Existing and vendor arms remain unavailable until their implementations
+    // override this capability after timing and partial-write validation.
+    virtual bool supportsTeleopGroupServo() const noexcept { return false; }
+    virtual JointEffortSource jointEffortSource() const noexcept
+    {
+        return JointEffortSource::Unspecified;
+    }
+
     virtual Result torqueOn() = 0;
     virtual Result torqueOff() = 0;
     virtual Result calibrateZeroQ(const std::string& joint_name) = 0;
@@ -73,6 +82,27 @@ public:
                                FrameType frame = FrameType::Base) = 0;
     virtual Result stopServoMode() = 0;
 
+    // Torque streaming is optional. Backends which do not provide an atomic
+    // group torque port retain source compatibility and fail explicitly.
+    virtual Result startTorqueMode(const TorqueServoOptions&)
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "torque servo mode is unsupported by this RobotArm");
+    }
+    virtual Result servoTorque(const JointTorqueCommand&)
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "torque servo command is unsupported by this RobotArm");
+    }
+    virtual Result stopTorqueMode()
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "torque servo mode is unsupported by this RobotArm");
+    }
+
     virtual Result connect(const std::string& ip, int port) = 0;
     virtual Result disconnect() = 0;
     virtual bool isConnected() const = 0;

cmvr-es/devices/arm/robot_arm_factory.h

@@ -9,6 +9,7 @@
 #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"
+#include "devices/arm/ume_robot_arm/include/ume_robot_arm.h"
 
 namespace cmvr::device {
 
@@ -36,6 +37,9 @@ public:
             return nullptr;
         }
 
+        case config::RobotArmConfig::kUme:
+            return std::make_shared<UmeRobotArm>(cfg);
+
         case config::RobotArmConfig::BACKEND_NOT_SET:
         default:
         {

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

@@ -0,0 +1,92 @@
+add_library(ume_robot_arm SHARED
+        src/damiao_mit_codec.cpp
+        src/damiao_can_fd_chain.cpp
+        src/ume_robot_arm.cpp
+)
+
+target_include_directories(ume_robot_arm PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(ume_robot_arm
+        PUBLIC
+        cmvr_es::device::canbus
+        cmvr_es::ik_solver
+        cmvr_es::common
+        PRIVATE
+        cmvr_es::proto
+        cmvr_es::logging
+        pthread
+)
+
+add_library(cmvr_es::device::ume_robot_arm ALIAS ume_robot_arm)
+install(TARGETS ume_robot_arm LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(damiao_mit_codec_test
+            tests/damiao_mit_codec_test.cpp
+    )
+    target_link_libraries(damiao_mit_codec_test
+            PRIVATE
+            cmvr_es::device::ume_robot_arm
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME damiao_mit_codec_test
+            COMMAND damiao_mit_codec_test
+    )
+    set(_ume_robot_arm_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _ume_robot_arm_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(damiao_mit_codec_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_ume_robot_arm_test_environment}"
+    )
+
+    add_executable(damiao_can_fd_chain_test
+            tests/damiao_can_fd_chain_test.cpp
+    )
+    target_link_libraries(damiao_can_fd_chain_test
+            PRIVATE
+            cmvr_es::device::ume_robot_arm
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME damiao_can_fd_chain_test
+            COMMAND damiao_can_fd_chain_test
+    )
+    set_tests_properties(damiao_can_fd_chain_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_ume_robot_arm_test_environment}"
+    )
+
+    add_executable(ume_robot_arm_test
+            tests/ume_robot_arm_test.cpp
+    )
+    target_link_libraries(ume_robot_arm_test
+            PRIVATE
+            cmvr_es::device::ume_robot_arm
+            gtest
+            gtest_main
+            pthread
+    )
+    target_compile_definitions(ume_robot_arm_test PRIVATE
+            CMVR_UME_ARM_CONFIG_PATH="${PROJECT_SOURCE_DIR}/cmvr-es/config/devices/arm/ume_arms.pb.txt"
+    )
+    add_test(
+            NAME ume_robot_arm_test
+            COMMAND ume_robot_arm_test
+    )
+    set_tests_properties(ume_robot_arm_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_ume_robot_arm_test_environment}"
+    )
+endif()

cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h

@@ -0,0 +1,145 @@
+#ifndef CMVR_ES_DAMIAO_CAN_FD_CHAIN_H
+#define CMVR_ES_DAMIAO_CAN_FD_CHAIN_H
+
+#include <atomic>
+#include <chrono>
+#include <cstddef>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::device {
+
+class AbstractCanbus;
+
+struct DamiaoJointSpec {
+    std::string joint_name;
+    std::uint32_t command_id{0};
+    std::uint32_t feedback_id{0};
+    std::uint8_t reported_motor_id{0};
+    DamiaoMotorModel model{DamiaoMotorModel::Unknown};
+    int direction{1};
+    double zero_offset_rad{0.0};
+    double joint_lower_rad{0.0};
+    double joint_upper_rad{0.0};
+    double max_velocity_rad_s{0.0};
+    double max_torque_nm{0.0};
+    std::uint16_t healthy_status_mask{0};
+    std::uint8_t max_driver_temperature_raw{0};
+    std::uint8_t max_motor_temperature_raw{0};
+};
+
+struct DamiaoChainOptions {
+    bool is_fd{true};
+    bool bitrate_switch{true};
+    bool hardware_enabled{false};
+};
+
+struct DamiaoChainStatistics {
+    std::uint64_t exchanges{0};
+    std::uint64_t deadline_misses{0};
+    std::uint64_t unknown_feedback{0};
+    std::uint64_t duplicate_feedback{0};
+    std::uint64_t rejected_commands{0};
+    std::uint64_t protocol_saturations{0};
+};
+
+enum class DamiaoChainState : std::uint8_t {
+    Closed = 0,
+    Initialized,
+    Passive,
+    Armed,
+    Active,
+    FaultLatched,
+    Stopped
+};
+
+class DamiaoCanFdChain {
+public:
+    DamiaoCanFdChain(std::shared_ptr<AbstractCanbus> bus,
+                     std::vector<DamiaoJointSpec> joints,
+                     DamiaoChainOptions options);
+    ~DamiaoCanFdChain();
+
+    DamiaoCanFdChain(const DamiaoCanFdChain&) = delete;
+    DamiaoCanFdChain& operator=(const DamiaoCanFdChain&) = delete;
+
+    Result init();
+    Result openPassive();
+    Result clearFault(std::chrono::steady_clock::time_point deadline);
+    Result arm(std::chrono::steady_clock::time_point deadline);
+    Result setZero(std::size_t joint_index,
+                   std::chrono::steady_clock::time_point deadline);
+    Result exchange(const DamiaoMitCommand* joint_commands,
+                    std::size_t command_count,
+                    DamiaoJointFeedback* joint_feedback,
+                    std::size_t feedback_count,
+                    std::chrono::steady_clock::time_point deadline);
+    Result disable() noexcept;
+    Result latchFault(const std::string& reason) noexcept;
+    void stop() noexcept;
+
+    DamiaoChainState state() const noexcept { return state_.load(); }
+    std::size_t size() const noexcept { return joints_.size(); }
+    bool hardwareEnabled() const noexcept { return options_.hardware_enabled; }
+    DamiaoChainStatistics statistics() const;
+    std::string lastError() const;
+    const std::vector<DamiaoJointSpec>& joints() const noexcept { return joints_; }
+
+private:
+    Result validateConfig_() const;
+    Result sendModeAll_(
+        DamiaoMode mode,
+        std::chrono::steady_clock::time_point deadline,
+        bool expect_feedback);
+    Result sendModeOne_(
+        std::size_t joint_index,
+        DamiaoMode mode,
+        std::chrono::steady_clock::time_point deadline);
+    Result receiveCycle_(
+        DamiaoJointFeedback* feedback,
+        std::size_t feedback_count,
+        std::chrono::steady_clock::time_point deadline);
+    bool sendFrames_(
+        const std::vector<CanFrame>& frames,
+        std::chrono::steady_clock::time_point deadline) noexcept;
+    bool sendFramesBestEffort_(
+        const std::vector<CanFrame>& frames) noexcept;
+    bool feedbackTransportAndHealthValid_(
+        const CanFrame& frame,
+        const DamiaoJointSpec& joint,
+        const DamiaoJointFeedback& feedback) const noexcept;
+    bool latchFaultAndDisable_(const std::string& reason) noexcept;
+    bool bestEffortZeroAndDisable_() noexcept;
+    void setError_(const std::string& error) noexcept;
+    std::size_t jointIndexForFeedbackId_(std::uint32_t id) const noexcept;
+    DamiaoMitCommand toMotorCommand_(
+        const DamiaoJointSpec& spec,
+        const DamiaoMitCommand& command,
+        bool& safety_saturated) const noexcept;
+    void toJointFeedback_(const DamiaoJointSpec& spec,
+                          DamiaoJointFeedback& feedback) const noexcept;
+
+    std::shared_ptr<AbstractCanbus> bus_;
+    std::vector<DamiaoJointSpec> joints_;
+    DamiaoChainOptions options_;
+    std::vector<CanFrame> tx_frames_;
+    std::vector<CanFrame> rx_frames_;
+    std::vector<DamiaoJointFeedback> feedback_scratch_;
+    std::vector<bool> feedback_seen_;
+
+    mutable std::mutex io_mutex_;
+    mutable std::mutex status_mutex_;
+    std::atomic<DamiaoChainState> state_{DamiaoChainState::Closed};
+    DamiaoChainStatistics statistics_;
+    std::string last_error_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_DAMIAO_CAN_FD_CHAIN_H

cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h

@@ -0,0 +1,135 @@
+#ifndef CMVR_ES_DAMIAO_MIT_CODEC_H
+#define CMVR_ES_DAMIAO_MIT_CODEC_H
+
+#include <cstdint>
+
+#include "canbus/abstract_canbus.h"
+
+namespace cmvr::device {
+
+enum class DamiaoMotorModel : std::uint8_t {
+    Unknown = 0,
+    DM4310,
+    DM4310_48V,
+    DM4340,
+    DM4340_48V,
+    DM6006,
+    DM8006,
+    DM8009,
+    DM10010L,
+    DM10010,
+    DMH3510,
+    DMH6215,
+    DMG6220
+};
+
+struct DamiaoMotorLimits {
+    double q_max_rad{0.0};
+    double dq_max_rad_s{0.0};
+    double tau_max_nm{0.0};
+
+    bool valid() const noexcept;
+};
+
+struct DamiaoMitCommand {
+    double q_rad{0.0};
+    double dq_rad_s{0.0};
+    double kp{0.0};
+    double kd{0.0};
+    double tau_ff_nm{0.0};
+};
+
+enum DamiaoSaturation : std::uint8_t {
+    DAMIAO_SATURATION_NONE = 0,
+    DAMIAO_SATURATION_Q = 1U << 0U,
+    DAMIAO_SATURATION_DQ = 1U << 1U,
+    DAMIAO_SATURATION_KP = 1U << 2U,
+    DAMIAO_SATURATION_KD = 1U << 3U,
+    DAMIAO_SATURATION_TAU = 1U << 4U
+};
+
+enum class DamiaoCodecError : std::uint8_t {
+    None = 0,
+    UnknownModel,
+    InvalidLimits,
+    NonFiniteInput,
+    InvalidCanId,
+    InvalidFrame,
+    UnexpectedFeedbackId
+};
+
+struct DamiaoEncodeResult {
+    DamiaoCodecError error{DamiaoCodecError::None};
+    std::uint8_t saturation_mask{DAMIAO_SATURATION_NONE};
+
+    explicit operator bool() const noexcept
+    {
+        return error == DamiaoCodecError::None;
+    }
+};
+
+struct DamiaoJointFeedback {
+    std::uint8_t reported_motor_id{0};
+    std::uint8_t status{0};
+    std::uint8_t driver_temperature_raw{0};
+    std::uint8_t motor_temperature_raw{0};
+    double q_rad{0.0};
+    double dq_rad_s{0.0};
+    double tau_nm{0.0};
+    std::int64_t rx_monotonic_ns{0};
+    bool valid{false};
+};
+
+enum class DamiaoMode : std::uint8_t {
+    ClearFault,
+    Enable,
+    Disable,
+    SetZero
+};
+
+class DamiaoMitCodec {
+public:
+    static constexpr double kKpMax = 500.0;
+    static constexpr double kKdMax = 5.0;
+
+    static DamiaoMotorLimits limitsFor(DamiaoMotorModel model) noexcept;
+
+    static DamiaoEncodeResult encodeMit(
+        std::uint32_t command_id,
+        DamiaoMotorModel model,
+        const DamiaoMitCommand& command,
+        bool is_fd,
+        bool bitrate_switch,
+        CanFrame& frame) noexcept;
+
+    static DamiaoCodecError decodeFeedback(
+        const CanFrame& frame,
+        std::uint32_t expected_feedback_id,
+        DamiaoMotorModel model,
+        DamiaoJointFeedback& feedback) noexcept;
+
+    static DamiaoCodecError encodeMode(
+        std::uint32_t command_id,
+        DamiaoMode mode,
+        bool is_fd,
+        bool bitrate_switch,
+        CanFrame& frame) noexcept;
+
+    // Public for protocol golden-vector tests. The unusual +1 decode behavior
+    // intentionally matches the legacy UME Python implementation.
+    static std::uint16_t floatToUint(
+        double value,
+        double minimum,
+        double maximum,
+        unsigned bits,
+        bool& saturated) noexcept;
+    static double uintToFloat(
+        std::uint16_t value,
+        double minimum,
+        double maximum,
+        unsigned bits) noexcept;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_DAMIAO_MIT_CODEC_H

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

@@ -0,0 +1,181 @@
+#ifndef CMVR_ES_UME_ROBOT_ARM_H
+#define CMVR_ES_UME_ROBOT_ARM_H
+
+#include <array>
+#include <atomic>
+#include <cstddef>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "arm/robot_arm.h"
+#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
+#include "cmvr/config/arm_config/arm_config.pb.h"
+
+namespace cmvr::device {
+
+class AbstractCanbus;
+
+struct UmeArmSample {
+    static constexpr std::size_t kDof = 8;
+
+    std::uint64_t sequence{0};
+    std::int64_t sample_monotonic_ns{0};
+    std::array<double, kDof> q{};
+    std::array<double, kDof> dq{};
+    std::array<double, kDof> tau_measured{};
+    std::array<std::int64_t, kDof> motor_rx_time_ns{};
+    std::uint8_t valid_mask{0};
+};
+
+// One UmeRobotArm represents one physical eight-axis leader arm and one
+// SocketCAN-FD interface. The class owns its local high-frequency actuator
+// loop; networking and follower kinematics remain outside this device.
+class UmeRobotArm final : public RobotArm {
+public:
+    explicit UmeRobotArm(const config::RobotArmConfig& cfg);
+    UmeRobotArm(const config::RobotArmConfig& cfg,
+                std::shared_ptr<AbstractCanbus> canbus);
+    ~UmeRobotArm() override;
+
+    std::string typeName() const override { return "UmeRobotArm"; }
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+    DeviceHealthSnapshot healthSnapshot() override;
+
+    RobotModel getRobotModel() const override { return model_; }
+    std::size_t getDof() const override { return UmeArmSample::kDof; }
+    ArmState getRobotState() const override;
+    JointGroupState getJointState() const override;
+    Result readSample(UmeArmSample& sample) const;
+    CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
+    RobotMode getRobotMode() const override;
+    SafetyMode getSafetyMode() const override;
+    ControlMode getControlMode() const override;
+
+    Result torqueOn() override;
+    Result torqueOff() override;
+    Result calibrateZeroQ(const std::string& joint_name) override;
+
+    Result emergencyStop() override;
+    Result protectiveStop() override;
+    Result setSpeedScaling(double scaling) override;
+    double getSpeedScaling() const override { return 1.0; }
+    bool isProtectiveStopped() const override
+    {
+        return protective_stopped_.load();
+    }
+    bool isEmergencyStopped() const override
+    {
+        return emergency_stopped_.load();
+    }
+    bool isFault() const override { return fault_latched_.load(); }
+
+    Result moveJ(const JointPositionCommand& target,
+                 const MotionOptions& options) override;
+    Result speedJ(const JointVelocityCommand& velocity,
+                  double acceleration,
+                  double duration) override;
+    Result stopJ(double acceleration) override;
+    Result moveL(const CartesianPose& target,
+                 const MotionOptions& options,
+                 FrameType frame = FrameType::Base) override;
+    Result speedL(const CartesianVelocity& velocity,
+                  double acceleration,
+                  double duration,
+                  FrameType frame = FrameType::Base) override;
+    Result stopL(std::optional<double> acceleration = std::nullopt) override;
+    Result stopMotion() override;
+
+    Result startServoMode(const ServoOptions& options) override;
+    Result servoJ(const JointPositionCommand& target) override;
+    Result servoL(const CartesianPose& target,
+                  FrameType frame = FrameType::Base) override;
+    Result servoSpeedJ(const JointVelocityCommand& velocity) override;
+    Result servoSpeedL(const CartesianVelocity& velocity,
+                       FrameType frame = FrameType::Base) override;
+    Result stopServoMode() override;
+
+    Result startTorqueMode(const TorqueServoOptions& options) override;
+    Result servoTorque(const JointTorqueCommand& target) override;
+    Result stopTorqueMode() override;
+
+    Result connect(const std::string& ip, int port) override;
+    Result disconnect() override;
+    bool isConnected() const override { return initialized_.load(); }
+    Result powerOn() override { return torqueOn(); }
+    Result powerOff() override { return torqueOff(); }
+    Result brakeRelease() override;
+    Result shutdown() override;
+    Result clearFault() override;
+    Result unlockProtectiveStop() override;
+    Result loadProgram(const std::string& program_name) override;
+    Result playProgram() override;
+    Result pauseProgram() override;
+    Result stopProgram() override;
+
+    std::vector<double> ik(const std::string& base_link,
+                           const std::string& ee_link,
+                           const CartesianPose& pose) override;
+    std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override
+    {
+        return ik_solver_;
+    }
+    CartesianPose fk(const std::string& base_link,
+                     const std::string& ee_link) override;
+    CartesianPose fk(bool is_tcp = true) override;
+    CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
+    bool busy() const override { return powered_on_.load(); }
+
+private:
+    void normalizeConfig_();
+    bool buildModelAndChain_();
+    void controlLoop_() noexcept;
+    void recordFault_(const std::string& message) noexcept;
+    Result requirePassive_(const std::string& operation) const;
+    static Result unsupported_(const std::string& operation);
+    static std::int64_t monotonicNowNs_() noexcept;
+
+    config::RobotArmConfig cfg_;
+    config::UmeRobotArmBackendConfig ume_cfg_;
+    std::shared_ptr<AbstractCanbus> canbus_;
+    std::unique_ptr<DamiaoCanFdChain> chain_;
+    std::vector<DamiaoJointSpec> joint_specs_;
+    RobotModel model_;
+    std::shared_ptr<cmvr::IKSolver> ik_solver_;
+
+    mutable std::mutex lifecycle_mutex_;
+    mutable std::mutex command_mutex_;
+    mutable std::mutex sample_mutex_;
+    mutable std::mutex status_mutex_;
+    mutable std::mutex kinematics_mutex_;
+    std::thread control_thread_;
+
+    std::array<double, UmeArmSample::kDof> latest_torque_command_{};
+    UmeArmSample latest_sample_;
+    std::string last_error_;
+
+    std::atomic<bool> initialized_{false};
+    std::atomic<bool> running_{false};
+    std::atomic<bool> torque_mode_{false};
+    std::atomic<bool> powered_on_{false};
+    std::atomic<bool> fault_latched_{false};
+    std::atomic<bool> protective_stopped_{false};
+    std::atomic<bool> emergency_stopped_{false};
+    std::atomic<bool> command_ready_{false};
+    std::atomic<std::uint64_t> command_sequence_{0};
+    std::atomic<std::int64_t> command_time_ns_{0};
+    std::atomic<std::int64_t> loop_period_ns_{1250000};
+    std::atomic<std::uint32_t> command_watchdog_ms_{20};
+    std::uint32_t cycle_deadline_us_{900};
+    std::uint32_t feedback_watchdog_ms_{20};
+    std::uint32_t shutdown_timeout_ms_{50};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_UME_ROBOT_ARM_H

cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp

@@ -0,0 +1,705 @@
+#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <unordered_set>
+
+#include "canbus/abstract_canbus.h"
+
+namespace cmvr::device {
+namespace {
+
+Result invalidArgument(const std::string& message)
+{
+    return Result::failure(ArmErrorCode::InvalidArgument, message);
+}
+
+Result commandFailed(const std::string& message)
+{
+    return Result::failure(ArmErrorCode::CommandFailed, message);
+}
+
+Result notReady(const std::string& message)
+{
+    return Result::failure(ArmErrorCode::RobotNotReady, message);
+}
+
+} // namespace
+
+DamiaoCanFdChain::DamiaoCanFdChain(
+    std::shared_ptr<AbstractCanbus> bus,
+    std::vector<DamiaoJointSpec> joints,
+    DamiaoChainOptions options)
+    : bus_(std::move(bus)),
+      joints_(std::move(joints)),
+      options_(options),
+      feedback_scratch_(joints_.size()),
+      feedback_seen_(joints_.size(), false)
+{
+    tx_frames_.reserve(joints_.size());
+    rx_frames_.reserve(1);
+}
+
+DamiaoCanFdChain::~DamiaoCanFdChain()
+{
+    stop();
+}
+
+Result DamiaoCanFdChain::validateConfig_() const
+{
+    if (!bus_) {
+        return invalidArgument("Damiao CAN bus is null");
+    }
+    if (joints_.empty()) {
+        return invalidArgument("Damiao joint list is empty");
+    }
+
+    std::unordered_set<std::string> names;
+    std::unordered_set<std::uint32_t> command_ids;
+    std::unordered_set<std::uint32_t> feedback_ids;
+    std::unordered_set<std::uint32_t> reported_ids;
+    for (const auto& joint : joints_) {
+        if (joint.joint_name.empty() ||
+            !names.insert(joint.joint_name).second) {
+            return invalidArgument("Damiao joint names must be non-empty and unique");
+        }
+        if (joint.command_id == 0 || joint.command_id > 0x7FFU ||
+            !command_ids.insert(joint.command_id).second) {
+            return invalidArgument("Damiao command IDs must be unique standard CAN IDs");
+        }
+        if (joint.feedback_id == 0 || joint.feedback_id > 0x7FFU ||
+            !feedback_ids.insert(joint.feedback_id).second) {
+            return invalidArgument("Damiao feedback IDs must be unique standard CAN IDs");
+        }
+        if (joint.reported_motor_id > 0x0FU ||
+            !reported_ids.insert(joint.reported_motor_id).second) {
+            return invalidArgument("Damiao reported motor IDs must be unique 4-bit values");
+        }
+        if (!DamiaoMitCodec::limitsFor(joint.model).valid()) {
+            return invalidArgument("Damiao motor model is unknown");
+        }
+        if (joint.direction != 1 && joint.direction != -1) {
+            return invalidArgument("Damiao joint direction must be +1 or -1");
+        }
+        if (!std::isfinite(joint.zero_offset_rad) ||
+            !std::isfinite(joint.joint_lower_rad) ||
+            !std::isfinite(joint.joint_upper_rad) ||
+            joint.joint_upper_rad <= joint.joint_lower_rad ||
+            !std::isfinite(joint.max_velocity_rad_s) ||
+            joint.max_velocity_rad_s <= 0.0 ||
+            !std::isfinite(joint.max_torque_nm) ||
+            joint.max_torque_nm <= 0.0) {
+            return invalidArgument("Damiao mechanical limits are invalid");
+        }
+        if (options_.hardware_enabled &&
+            (joint.healthy_status_mask == 0U ||
+             joint.max_driver_temperature_raw == 0U ||
+             joint.max_motor_temperature_raw == 0U)) {
+            return invalidArgument(
+                "Damiao hardware enable requires a reviewed feedback-status "
+                "whitelist and nonzero raw temperature thresholds");
+        }
+    }
+    return Result::success();
+}
+
+Result DamiaoCanFdChain::init()
+{
+    std::lock_guard lock(io_mutex_);
+    const auto config_result = validateConfig_();
+    if (!config_result.ok()) {
+        setError_(config_result.message);
+        state_.store(DamiaoChainState::FaultLatched);
+        return config_result;
+    }
+    if (state_.load() != DamiaoChainState::Closed &&
+        state_.load() != DamiaoChainState::Stopped) {
+        return Result::success();
+    }
+    if (!bus_->init()) {
+        setError_("failed to initialize Damiao CAN bus");
+        state_.store(DamiaoChainState::FaultLatched);
+        return notReady(lastError());
+    }
+    state_.store(DamiaoChainState::Initialized);
+    return Result::success();
+}
+
+Result DamiaoCanFdChain::openPassive()
+{
+    std::lock_guard lock(io_mutex_);
+    if (state_.load() != DamiaoChainState::Initialized) {
+        return notReady("Damiao chain is not initialized");
+    }
+    if (!bus_->start()) {
+        setError_("failed to start Damiao CAN bus");
+        state_.store(DamiaoChainState::FaultLatched);
+        return notReady(lastError());
+    }
+    // Deliberately no clear-fault or enable command here.
+    state_.store(DamiaoChainState::Passive);
+    return Result::success();
+}
+
+Result DamiaoCanFdChain::clearFault(
+    const std::chrono::steady_clock::time_point deadline)
+{
+    std::lock_guard lock(io_mutex_);
+    if (!options_.hardware_enabled) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "Damiao hardware commands are disabled by configuration");
+    }
+    const auto current = state_.load();
+    if (current != DamiaoChainState::Passive &&
+        current != DamiaoChainState::FaultLatched) {
+        return notReady("clearFault requires a disabled Damiao chain");
+    }
+    const auto result = sendModeAll_(
+        DamiaoMode::ClearFault, deadline, true);
+    if (!result.ok()) {
+        state_.store(DamiaoChainState::FaultLatched);
+        return result;
+    }
+    // Clearing a fault never arms the motors.
+    state_.store(DamiaoChainState::Passive);
+    return Result::success();
+}
+
+Result DamiaoCanFdChain::arm(
+    const std::chrono::steady_clock::time_point deadline)
+{
+    std::lock_guard lock(io_mutex_);
+    if (!options_.hardware_enabled) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "Damiao hardware commands are disabled by configuration");
+    }
+    if (state_.load() != DamiaoChainState::Passive) {
+        return notReady("Damiao chain must be passive before arm");
+    }
+    const auto result = sendModeAll_(DamiaoMode::Enable, deadline, true);
+    if (!result.ok()) {
+        latchFaultAndDisable_(result.message);
+        return Result::failure(result.code, lastError());
+    }
+    state_.store(DamiaoChainState::Armed);
+    return Result::success();
+}
+
+Result DamiaoCanFdChain::setZero(
+    const std::size_t joint_index,
+    const std::chrono::steady_clock::time_point deadline)
+{
+    std::lock_guard lock(io_mutex_);
+    if (!options_.hardware_enabled) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "Damiao hardware commands are disabled by configuration");
+    }
+    if (state_.load() != DamiaoChainState::Passive) {
+        return notReady("setZero requires a passive Damiao chain");
+    }
+    return sendModeOne_(joint_index, DamiaoMode::SetZero, deadline);
+}
+
+DamiaoMitCommand DamiaoCanFdChain::toMotorCommand_(
+    const DamiaoJointSpec& spec,
+    const DamiaoMitCommand& command,
+    bool& safety_saturated) const noexcept
+{
+    DamiaoMitCommand motor = command;
+    safety_saturated = false;
+
+    const double limited_q =
+        std::clamp(command.q_rad, spec.joint_lower_rad, spec.joint_upper_rad);
+    const double limited_dq =
+        std::clamp(command.dq_rad_s,
+                   -spec.max_velocity_rad_s, spec.max_velocity_rad_s);
+    const double limited_tau =
+        std::clamp(command.tau_ff_nm,
+                   -spec.max_torque_nm, spec.max_torque_nm);
+    safety_saturated =
+        limited_q != command.q_rad ||
+        limited_dq != command.dq_rad_s ||
+        limited_tau != command.tau_ff_nm;
+
+    motor.q_rad =
+        spec.direction * (limited_q - spec.zero_offset_rad);
+    motor.dq_rad_s = spec.direction * limited_dq;
+    motor.tau_ff_nm = spec.direction * limited_tau;
+    return motor;
+}
+
+void DamiaoCanFdChain::toJointFeedback_(
+    const DamiaoJointSpec& spec,
+    DamiaoJointFeedback& feedback) const noexcept
+{
+    feedback.q_rad =
+        spec.direction * feedback.q_rad + spec.zero_offset_rad;
+    feedback.dq_rad_s = spec.direction * feedback.dq_rad_s;
+    feedback.tau_nm = spec.direction * feedback.tau_nm;
+}
+
+Result DamiaoCanFdChain::exchange(
+    const DamiaoMitCommand* joint_commands,
+    const std::size_t command_count,
+    DamiaoJointFeedback* joint_feedback,
+    const std::size_t feedback_count,
+    const std::chrono::steady_clock::time_point deadline)
+{
+    std::lock_guard lock(io_mutex_);
+    if (!joint_commands || !joint_feedback ||
+        command_count != joints_.size() ||
+        feedback_count != joints_.size()) {
+        {
+            std::lock_guard status_lock(status_mutex_);
+            ++statistics_.rejected_commands;
+        }
+        return invalidArgument("Damiao exchange dimensions do not match configured joints");
+    }
+    const auto current = state_.load();
+    if (current != DamiaoChainState::Armed &&
+        current != DamiaoChainState::Active) {
+        return notReady("Damiao chain is not armed");
+    }
+    if (std::chrono::steady_clock::now() >= deadline) {
+        {
+            std::lock_guard status_lock(status_mutex_);
+            ++statistics_.deadline_misses;
+        }
+        latchFaultAndDisable_(
+            "Damiao exchange deadline expired before send");
+        return Result::failure(ArmErrorCode::Timeout, lastError());
+    }
+
+    tx_frames_.clear();
+    std::uint64_t saturation_count = 0;
+    for (std::size_t i = 0; i < joints_.size(); ++i) {
+        bool safety_saturated = false;
+        const auto motor_command =
+            toMotorCommand_(joints_[i], joint_commands[i], safety_saturated);
+        CanFrame frame;
+        const auto encoded = DamiaoMitCodec::encodeMit(
+            joints_[i].command_id, joints_[i].model, motor_command,
+            options_.is_fd, options_.bitrate_switch, frame);
+        if (!encoded) {
+            {
+                std::lock_guard status_lock(status_mutex_);
+                ++statistics_.rejected_commands;
+            }
+            return invalidArgument("Damiao command failed protocol validation");
+        }
+        if (safety_saturated ||
+            encoded.saturation_mask != DAMIAO_SATURATION_NONE) {
+            ++saturation_count;
+        }
+        tx_frames_.push_back(frame);
+    }
+    if (!bus_->discardPendingFrames()) {
+        latchFaultAndDisable_(
+            "failed to drain stale Damiao feedback before command");
+        return commandFailed(lastError());
+    }
+    if (std::chrono::steady_clock::now() >= deadline) {
+        {
+            std::lock_guard status_lock(status_mutex_);
+            ++statistics_.deadline_misses;
+        }
+        latchFaultAndDisable_(
+            "Damiao exchange deadline expired before command commit");
+        return Result::failure(ArmErrorCode::Timeout, lastError());
+    }
+    if (!sendFrames_(tx_frames_, deadline)) {
+        latchFaultAndDisable_(
+            "failed to send Damiao MIT command batch before deadline");
+        return commandFailed(lastError());
+    }
+    if (std::chrono::steady_clock::now() >= deadline) {
+        {
+            std::lock_guard status_lock(status_mutex_);
+            ++statistics_.deadline_misses;
+        }
+        latchFaultAndDisable_(
+            "Damiao MIT command batch exceeded its deadline");
+        return Result::failure(ArmErrorCode::Timeout, lastError());
+    }
+
+    const auto receive_result =
+        receiveCycle_(joint_feedback, feedback_count, deadline);
+    {
+        std::lock_guard status_lock(status_mutex_);
+        ++statistics_.exchanges;
+        statistics_.protocol_saturations += saturation_count;
+    }
+    if (!receive_result.ok()) {
+        latchFaultAndDisable_(receive_result.message);
+        return Result::failure(receive_result.code, lastError());
+    }
+    state_.store(DamiaoChainState::Active);
+    return Result::success();
+}
+
+Result DamiaoCanFdChain::sendModeAll_(
+    const DamiaoMode mode,
+    const std::chrono::steady_clock::time_point deadline,
+    const bool expect_feedback)
+{
+    tx_frames_.clear();
+    for (const auto& joint : joints_) {
+        CanFrame frame;
+        const auto error = DamiaoMitCodec::encodeMode(
+            joint.command_id, mode, options_.is_fd,
+            options_.bitrate_switch, frame);
+        if (error != DamiaoCodecError::None) {
+            return invalidArgument("failed to encode Damiao lifecycle command");
+        }
+        tx_frames_.push_back(frame);
+    }
+    if (expect_feedback && !bus_->discardPendingFrames()) {
+        return commandFailed(
+            "failed to drain stale Damiao lifecycle feedback");
+    }
+    if (std::chrono::steady_clock::now() >= deadline) {
+        return Result::failure(
+            ArmErrorCode::Timeout,
+            "Damiao lifecycle deadline expired before command commit");
+    }
+    if (!sendFrames_(tx_frames_, deadline)) {
+        return commandFailed(
+            "failed to send Damiao lifecycle command before deadline");
+    }
+    if (std::chrono::steady_clock::now() >= deadline) {
+        return Result::failure(
+            ArmErrorCode::Timeout,
+            "Damiao lifecycle command exceeded its deadline");
+    }
+    if (!expect_feedback) {
+        return Result::success();
+    }
+    return receiveCycle_(
+        feedback_scratch_.data(), feedback_scratch_.size(), deadline);
+}
+
+Result DamiaoCanFdChain::sendModeOne_(
+    const std::size_t joint_index,
+    const DamiaoMode mode,
+    const std::chrono::steady_clock::time_point deadline)
+{
+    if (joint_index >= joints_.size()) {
+        return invalidArgument("Damiao joint index is out of range");
+    }
+    CanFrame frame;
+    const auto error = DamiaoMitCodec::encodeMode(
+        joints_[joint_index].command_id, mode, options_.is_fd,
+        options_.bitrate_switch, frame);
+    if (error != DamiaoCodecError::None) {
+        return invalidArgument("failed to encode Damiao lifecycle command");
+    }
+    tx_frames_.assign(1, frame);
+    if (!bus_->discardPendingFrames()) {
+        return commandFailed(
+            "failed to drain stale Damiao lifecycle feedback");
+    }
+    if (std::chrono::steady_clock::now() >= deadline) {
+        return Result::failure(
+            ArmErrorCode::Timeout,
+            "Damiao lifecycle deadline expired before command commit");
+    }
+    if (!sendFrames_(tx_frames_, deadline)) {
+        return commandFailed(
+            "failed to send Damiao lifecycle command before deadline");
+    }
+    if (std::chrono::steady_clock::now() >= deadline) {
+        return Result::failure(
+            ArmErrorCode::Timeout,
+            "Damiao lifecycle command exceeded its deadline");
+    }
+
+    std::fill(feedback_seen_.begin(), feedback_seen_.end(), false);
+    while (std::chrono::steady_clock::now() < deadline) {
+        rx_frames_.clear();
+        int32_t count = 1;
+        if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) {
+            continue;
+        }
+        for (const auto& received : rx_frames_) {
+            if (received.id != joints_[joint_index].feedback_id) {
+                continue;
+            }
+            DamiaoJointFeedback feedback;
+            if (DamiaoMitCodec::decodeFeedback(
+                    received, joints_[joint_index].feedback_id,
+                    joints_[joint_index].model, feedback) !=
+                    DamiaoCodecError::None ||
+                feedback.reported_motor_id !=
+                    joints_[joint_index].reported_motor_id ||
+                !feedbackTransportAndHealthValid_(
+                    received, joints_[joint_index], feedback)) {
+                return commandFailed("invalid Damiao lifecycle feedback");
+            }
+            return Result::success();
+        }
+    }
+    return Result::failure(
+        ArmErrorCode::Timeout, "Damiao lifecycle feedback timed out");
+}
+
+Result DamiaoCanFdChain::receiveCycle_(
+    DamiaoJointFeedback* feedback,
+    const std::size_t feedback_count,
+    const std::chrono::steady_clock::time_point deadline)
+{
+    if (!feedback || feedback_count != joints_.size()) {
+        return invalidArgument("Damiao feedback dimensions do not match");
+    }
+
+    std::fill(feedback_seen_.begin(), feedback_seen_.end(), false);
+    std::size_t received_count = 0;
+    while (received_count < joints_.size() &&
+           std::chrono::steady_clock::now() < deadline) {
+        rx_frames_.clear();
+        int32_t count = 1;
+        if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) {
+            continue;
+        }
+        for (const auto& frame : rx_frames_) {
+            const auto index = jointIndexForFeedbackId_(frame.id);
+            if (index == joints_.size()) {
+                std::lock_guard status_lock(status_mutex_);
+                ++statistics_.unknown_feedback;
+                continue;
+            }
+            if (feedback_seen_[index]) {
+                std::lock_guard status_lock(status_mutex_);
+                ++statistics_.duplicate_feedback;
+                continue;
+            }
+            DamiaoJointFeedback decoded;
+            if (DamiaoMitCodec::decodeFeedback(
+                    frame, joints_[index].feedback_id,
+                    joints_[index].model, decoded) !=
+                    DamiaoCodecError::None ||
+                decoded.reported_motor_id !=
+                    joints_[index].reported_motor_id ||
+                !feedbackTransportAndHealthValid_(
+                    frame, joints_[index], decoded)) {
+                return commandFailed("Damiao feedback failed validation");
+            }
+            toJointFeedback_(joints_[index], decoded);
+            feedback[index] = decoded;
+            feedback_seen_[index] = true;
+            ++received_count;
+        }
+    }
+    if (received_count != joints_.size()) {
+        std::lock_guard status_lock(status_mutex_);
+        ++statistics_.deadline_misses;
+        return Result::failure(
+            ArmErrorCode::Timeout,
+            "Damiao feedback cycle missed its deadline");
+    }
+    return Result::success();
+}
+
+bool DamiaoCanFdChain::feedbackTransportAndHealthValid_(
+    const CanFrame& frame,
+    const DamiaoJointSpec& joint,
+    const DamiaoJointFeedback& feedback) const noexcept
+{
+    if (frame.is_fd != options_.is_fd) {
+        return false;
+    }
+    if (options_.is_fd && options_.bitrate_switch &&
+        !frame.bitrate_switch) {
+        return false;
+    }
+    if (joint.healthy_status_mask == 0U) {
+        // An empty whitelist is tolerated only while the actuator hardware
+        // gate is closed, so passive software/configuration checks can run.
+        return !options_.hardware_enabled;
+    }
+    if (feedback.status > 0x0FU ||
+        (joint.healthy_status_mask &
+         static_cast<std::uint16_t>(1U << feedback.status)) == 0U) {
+        return false;
+    }
+    return feedback.driver_temperature_raw <=
+               joint.max_driver_temperature_raw &&
+           feedback.motor_temperature_raw <=
+               joint.max_motor_temperature_raw;
+}
+
+bool DamiaoCanFdChain::sendFrames_(
+    const std::vector<CanFrame>& frames,
+    const std::chrono::steady_clock::time_point deadline) noexcept
+{
+    if (frames.empty() ||
+        frames.size() > static_cast<std::size_t>(
+                            std::numeric_limits<int32_t>::max())) {
+        return false;
+    }
+    int32_t count = static_cast<int32_t>(frames.size());
+    return bus_->sendUntil(frames, &count, deadline) ==
+               msgs::ErrorCode::OK &&
+           count == static_cast<int32_t>(frames.size());
+}
+
+bool DamiaoCanFdChain::sendFramesBestEffort_(
+    const std::vector<CanFrame>& frames) noexcept
+{
+    if (frames.empty() ||
+        frames.size() > static_cast<std::size_t>(
+                            std::numeric_limits<int32_t>::max())) {
+        return false;
+    }
+    int32_t count = static_cast<int32_t>(frames.size());
+    return bus_->send(frames, &count) == msgs::ErrorCode::OK &&
+           count == static_cast<int32_t>(frames.size());
+}
+
+Result DamiaoCanFdChain::disable() noexcept
+{
+    std::lock_guard lock(io_mutex_);
+    if (state_.load() == DamiaoChainState::Closed ||
+        state_.load() == DamiaoChainState::Initialized ||
+        state_.load() == DamiaoChainState::Stopped) {
+        return Result::success();
+    }
+    const bool disabled = bestEffortZeroAndDisable_();
+    if (!disabled) {
+        setError_(
+            "failed to send all Damiao zero/disable safety frames");
+        state_.store(DamiaoChainState::FaultLatched);
+        return commandFailed(lastError());
+    }
+    if (state_.load() != DamiaoChainState::FaultLatched) {
+        state_.store(DamiaoChainState::Passive);
+    }
+    return Result::success();
+}
+
+Result DamiaoCanFdChain::latchFault(const std::string& reason) noexcept
+{
+    std::lock_guard lock(io_mutex_);
+    if (!latchFaultAndDisable_(reason)) {
+        return commandFailed(lastError());
+    }
+    return Result::success();
+}
+
+bool DamiaoCanFdChain::latchFaultAndDisable_(
+    const std::string& reason) noexcept
+{
+    state_.store(DamiaoChainState::FaultLatched);
+    setError_(reason);
+    if (bestEffortZeroAndDisable_()) {
+        return true;
+    }
+    setError_(
+        reason +
+        "; failed to send all Damiao zero/disable safety frames");
+    return false;
+}
+
+bool DamiaoCanFdChain::bestEffortZeroAndDisable_() noexcept
+{
+    if (!bus_ || !options_.hardware_enabled) {
+        return true;
+    }
+
+    const auto current = state_.load();
+    if (current != DamiaoChainState::Passive &&
+        current != DamiaoChainState::Armed &&
+        current != DamiaoChainState::Active &&
+        current != DamiaoChainState::FaultLatched) {
+        return true;
+    }
+
+    bool all_sent = true;
+    if (current == DamiaoChainState::Armed ||
+        current == DamiaoChainState::Active ||
+        current == DamiaoChainState::FaultLatched) {
+        tx_frames_.clear();
+        for (const auto& joint : joints_) {
+            DamiaoMitCommand zero;
+            CanFrame frame;
+            if (DamiaoMitCodec::encodeMit(
+                    joint.command_id, joint.model, zero,
+                    options_.is_fd, options_.bitrate_switch, frame)) {
+                tx_frames_.push_back(frame);
+            }
+        }
+        if (!tx_frames_.empty()) {
+            all_sent = sendFramesBestEffort_(tx_frames_) && all_sent;
+        }
+    }
+
+    tx_frames_.clear();
+    for (const auto& joint : joints_) {
+        CanFrame frame;
+        if (DamiaoMitCodec::encodeMode(
+                joint.command_id, DamiaoMode::Disable,
+                options_.is_fd, options_.bitrate_switch, frame) ==
+            DamiaoCodecError::None) {
+            tx_frames_.push_back(frame);
+        }
+    }
+    if (!tx_frames_.empty()) {
+        all_sent = sendFramesBestEffort_(tx_frames_) && all_sent;
+    }
+    return all_sent;
+}
+
+void DamiaoCanFdChain::stop() noexcept
+{
+    std::lock_guard lock(io_mutex_);
+    const auto current = state_.load();
+    if (current == DamiaoChainState::Closed ||
+        current == DamiaoChainState::Stopped) {
+        return;
+    }
+    if (!bestEffortZeroAndDisable_()) {
+        setError_(
+            "failed to send all Damiao shutdown safety frames");
+    }
+    if (bus_) {
+        bus_->stop();
+    }
+    state_.store(DamiaoChainState::Stopped);
+}
+
+std::size_t DamiaoCanFdChain::jointIndexForFeedbackId_(
+    const std::uint32_t id) const noexcept
+{
+    for (std::size_t i = 0; i < joints_.size(); ++i) {
+        if (joints_[i].feedback_id == id) {
+            return i;
+        }
+    }
+    return joints_.size();
+}
+
+void DamiaoCanFdChain::setError_(const std::string& error) noexcept
+{
+    try {
+        std::lock_guard lock(status_mutex_);
+        last_error_ = error;
+    } catch (...) {
+    }
+}
+
+DamiaoChainStatistics DamiaoCanFdChain::statistics() const
+{
+    std::lock_guard lock(status_mutex_);
+    return statistics_;
+}
+
+std::string DamiaoCanFdChain::lastError() const
+{
+    std::lock_guard lock(status_mutex_);
+    return last_error_;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp

@@ -0,0 +1,254 @@
+#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+namespace cmvr::device {
+namespace {
+
+constexpr unsigned kPositionBits = 16;
+constexpr unsigned kVelocityBits = 12;
+constexpr unsigned kGainBits = 12;
+constexpr unsigned kTorqueBits = 12;
+constexpr std::uint32_t kCanStandardMaxId = 0x7FFU;
+
+bool finiteCommand(const DamiaoMitCommand& command) noexcept
+{
+    return std::isfinite(command.q_rad) &&
+           std::isfinite(command.dq_rad_s) &&
+           std::isfinite(command.kp) &&
+           std::isfinite(command.kd) &&
+           std::isfinite(command.tau_ff_nm);
+}
+
+std::uint8_t modeByte(const DamiaoMode mode) noexcept
+{
+    switch (mode) {
+    case DamiaoMode::ClearFault:
+        return 0xFBU;
+    case DamiaoMode::Enable:
+        return 0xFCU;
+    case DamiaoMode::Disable:
+        return 0xFDU;
+    case DamiaoMode::SetZero:
+        return 0xFEU;
+    }
+    return 0;
+}
+
+} // namespace
+
+bool DamiaoMotorLimits::valid() const noexcept
+{
+    return std::isfinite(q_max_rad) && q_max_rad > 0.0 &&
+           std::isfinite(dq_max_rad_s) && dq_max_rad_s > 0.0 &&
+           std::isfinite(tau_max_nm) && tau_max_nm > 0.0;
+}
+
+DamiaoMotorLimits DamiaoMitCodec::limitsFor(
+    const DamiaoMotorModel model) noexcept
+{
+    switch (model) {
+    case DamiaoMotorModel::DM4310:
+        return {12.5, 30.0, 10.0};
+    case DamiaoMotorModel::DM4310_48V:
+        return {12.5, 50.0, 10.0};
+    case DamiaoMotorModel::DM4340:
+        return {12.5, 8.0, 28.0};
+    case DamiaoMotorModel::DM4340_48V:
+        return {12.5, 10.0, 28.0};
+    case DamiaoMotorModel::DM6006:
+        return {12.5, 45.0, 20.0};
+    case DamiaoMotorModel::DM8006:
+        return {12.5, 45.0, 40.0};
+    case DamiaoMotorModel::DM8009:
+        return {12.5, 45.0, 54.0};
+    case DamiaoMotorModel::DM10010L:
+        return {12.5, 25.0, 200.0};
+    case DamiaoMotorModel::DM10010:
+        return {12.5, 20.0, 200.0};
+    case DamiaoMotorModel::DMH3510:
+        return {12.5, 280.0, 1.0};
+    case DamiaoMotorModel::DMH6215:
+        return {12.5, 45.0, 10.0};
+    case DamiaoMotorModel::DMG6220:
+        return {12.5, 45.0, 10.0};
+    case DamiaoMotorModel::Unknown:
+    default:
+        return {};
+    }
+}
+
+std::uint16_t DamiaoMitCodec::floatToUint(
+    const double value,
+    const double minimum,
+    const double maximum,
+    const unsigned bits,
+    bool& saturated) noexcept
+{
+    saturated = value < minimum || value > maximum;
+    if (!std::isfinite(value) || !std::isfinite(minimum) ||
+        !std::isfinite(maximum) || maximum <= minimum ||
+        bits == 0 || bits > 16) {
+        saturated = true;
+        return 0;
+    }
+
+    const double clamped = std::clamp(value, minimum, maximum);
+    const std::uint32_t levels = (std::uint32_t{1} << bits) - 1U;
+    const double normalized = (clamped - minimum) / (maximum - minimum);
+    return static_cast<std::uint16_t>(normalized * levels);
+}
+
+double DamiaoMitCodec::uintToFloat(
+    const std::uint16_t value,
+    const double minimum,
+    const double maximum,
+    const unsigned bits) noexcept
+{
+    if (!std::isfinite(minimum) || !std::isfinite(maximum) ||
+        maximum <= minimum || bits == 0 || bits > 16) {
+        return 0.0;
+    }
+    const double span = maximum - minimum;
+    const double levels = static_cast<double>(std::uint32_t{1} << bits);
+    return (static_cast<double>(value) + 1.0) * span / levels + minimum;
+}
+
+DamiaoEncodeResult DamiaoMitCodec::encodeMit(
+    const std::uint32_t command_id,
+    const DamiaoMotorModel model,
+    const DamiaoMitCommand& command,
+    const bool is_fd,
+    const bool bitrate_switch,
+    CanFrame& frame) noexcept
+{
+    DamiaoEncodeResult result;
+    const auto limits = limitsFor(model);
+    if (!limits.valid()) {
+        result.error = DamiaoCodecError::UnknownModel;
+        return result;
+    }
+    if (!finiteCommand(command)) {
+        result.error = DamiaoCodecError::NonFiniteInput;
+        return result;
+    }
+    if (command_id > kCanStandardMaxId) {
+        result.error = DamiaoCodecError::InvalidCanId;
+        return result;
+    }
+
+    bool saturated = false;
+    const auto q = floatToUint(
+        command.q_rad, -limits.q_max_rad, limits.q_max_rad,
+        kPositionBits, saturated);
+    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_Q;
+
+    const auto dq = floatToUint(
+        command.dq_rad_s, -limits.dq_max_rad_s, limits.dq_max_rad_s,
+        kVelocityBits, saturated);
+    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_DQ;
+
+    const auto kp = floatToUint(
+        command.kp, 0.0, kKpMax, kGainBits, saturated);
+    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KP;
+
+    const auto kd = floatToUint(
+        command.kd, 0.0, kKdMax, kGainBits, saturated);
+    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KD;
+
+    const auto tau = floatToUint(
+        command.tau_ff_nm, -limits.tau_max_nm, limits.tau_max_nm,
+        kTorqueBits, saturated);
+    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_TAU;
+
+    frame = {};
+    frame.id = command_id;
+    frame.len = 8;
+    frame.is_fd = is_fd;
+    frame.bitrate_switch = is_fd && bitrate_switch;
+    frame.data[0] = static_cast<std::uint8_t>((q >> 8U) & 0xFFU);
+    frame.data[1] = static_cast<std::uint8_t>(q & 0xFFU);
+    frame.data[2] = static_cast<std::uint8_t>((dq >> 4U) & 0xFFU);
+    frame.data[3] = static_cast<std::uint8_t>(
+        ((dq & 0xFU) << 4U) | ((kp >> 8U) & 0xFU));
+    frame.data[4] = static_cast<std::uint8_t>(kp & 0xFFU);
+    frame.data[5] = static_cast<std::uint8_t>((kd >> 4U) & 0xFFU);
+    frame.data[6] = static_cast<std::uint8_t>(
+        ((kd & 0xFU) << 4U) | ((tau >> 8U) & 0xFU));
+    frame.data[7] = static_cast<std::uint8_t>(tau & 0xFFU);
+    return result;
+}
+
+DamiaoCodecError DamiaoMitCodec::decodeFeedback(
+    const CanFrame& frame,
+    const std::uint32_t expected_feedback_id,
+    const DamiaoMotorModel model,
+    DamiaoJointFeedback& feedback) noexcept
+{
+    feedback = {};
+    const auto limits = limitsFor(model);
+    if (!limits.valid()) {
+        return DamiaoCodecError::UnknownModel;
+    }
+    if (frame.is_error_frame || frame.is_remote_frame ||
+        frame.is_extended_id || frame.error_state_indicator ||
+        frame.len != 8) {
+        return DamiaoCodecError::InvalidFrame;
+    }
+    if (frame.id != expected_feedback_id) {
+        return DamiaoCodecError::UnexpectedFeedbackId;
+    }
+
+    const std::uint16_t q =
+        static_cast<std::uint16_t>(
+            (static_cast<std::uint16_t>(frame.data[1]) << 8U) |
+            frame.data[2]);
+    const std::uint16_t dq =
+        static_cast<std::uint16_t>(
+            (static_cast<std::uint16_t>(frame.data[3]) << 4U) |
+            (frame.data[4] >> 4U));
+    const std::uint16_t tau =
+        static_cast<std::uint16_t>(
+            ((static_cast<std::uint16_t>(frame.data[4]) & 0xFU) << 8U) |
+            frame.data[5]);
+
+    feedback.reported_motor_id = frame.data[0] & 0x0FU;
+    feedback.status = frame.data[0] >> 4U;
+    feedback.driver_temperature_raw = frame.data[6];
+    feedback.motor_temperature_raw = frame.data[7];
+    feedback.q_rad =
+        uintToFloat(q, -limits.q_max_rad, limits.q_max_rad, kPositionBits);
+    feedback.dq_rad_s =
+        uintToFloat(dq, -limits.dq_max_rad_s, limits.dq_max_rad_s,
+                    kVelocityBits);
+    feedback.tau_nm =
+        uintToFloat(tau, -limits.tau_max_nm, limits.tau_max_nm,
+                    kTorqueBits);
+    feedback.rx_monotonic_ns = frame.rx_monotonic_ns;
+    feedback.valid = true;
+    return DamiaoCodecError::None;
+}
+
+DamiaoCodecError DamiaoMitCodec::encodeMode(
+    const std::uint32_t command_id,
+    const DamiaoMode mode,
+    const bool is_fd,
+    const bool bitrate_switch,
+    CanFrame& frame) noexcept
+{
+    if (command_id > kCanStandardMaxId) {
+        return DamiaoCodecError::InvalidCanId;
+    }
+    frame = {};
+    frame.id = command_id;
+    frame.len = 8;
+    frame.is_fd = is_fd;
+    frame.bitrate_switch = is_fd && bitrate_switch;
+    std::memset(frame.data, 0xFF, 7);
+    frame.data[7] = modeByte(mode);
+    return DamiaoCodecError::None;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp

@@ -0,0 +1,407 @@
+#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
+
+#include <chrono>
+#include <deque>
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "canbus/abstract_canbus.h"
+
+namespace cmvr::device {
+namespace {
+
+class FakeCanbus final : public AbstractCanbus {
+public:
+    std::string typeName() const override { return "FakeCanbus"; }
+    bool init() override
+    {
+        initialized = true;
+        return init_result;
+    }
+    bool start() override
+    {
+        started = start_result;
+        is_started_ = started;
+        return started;
+    }
+    bool stop() override
+    {
+        stopped = true;
+        started = false;
+        is_started_ = false;
+        return true;
+    }
+
+    msgs::ErrorCode send(const std::vector<CanFrame>& frames,
+                         int32_t* frame_num) override
+    {
+        if (!started || !frame_num ||
+            *frame_num != static_cast<int32_t>(frames.size())) {
+            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+        }
+        if (!send_result) {
+            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+        }
+        sent_batches.push_back(frames);
+        if (!scheduled_replies.empty()) {
+            for (const auto& reply : scheduled_replies.front()) {
+                replies.push_back(reply);
+            }
+            scheduled_replies.pop_front();
+        }
+        return msgs::ErrorCode::OK;
+    }
+
+    msgs::ErrorCode receive(std::vector<CanFrame>* frames,
+                            int32_t* frame_num) override
+    {
+        if (!started || !frames || !frame_num || replies.empty()) {
+            return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+        }
+        frames->clear();
+        frames->push_back(replies.front());
+        replies.pop_front();
+        *frame_num = 1;
+        return msgs::ErrorCode::OK;
+    }
+
+    bool discardPendingFrames() override
+    {
+        ++drain_calls;
+        if (drain_delay > std::chrono::microseconds::zero()) {
+            std::this_thread::sleep_for(drain_delay);
+        }
+        replies.clear();
+        return drain_result;
+    }
+
+    std::string getErrorString(int32_t) override { return {}; }
+
+    void enqueueReplies(std::vector<CanFrame> batch)
+    {
+        scheduled_replies.push_back(std::move(batch));
+    }
+
+    bool init_result{true};
+    bool start_result{true};
+    bool initialized{false};
+    bool started{false};
+    bool stopped{false};
+    bool drain_result{true};
+    bool send_result{true};
+    std::size_t drain_calls{0};
+    std::chrono::microseconds drain_delay{0};
+    std::vector<std::vector<CanFrame>> sent_batches;
+    std::deque<CanFrame> replies;
+    std::deque<std::vector<CanFrame>> scheduled_replies;
+};
+
+DamiaoJointSpec joint(std::string name,
+                      std::uint32_t command_id,
+                      std::uint32_t feedback_id,
+                      std::uint8_t reported_id,
+                      int direction = 1)
+{
+    DamiaoJointSpec spec;
+    spec.joint_name = std::move(name);
+    spec.command_id = command_id;
+    spec.feedback_id = feedback_id;
+    spec.reported_motor_id = reported_id;
+    spec.model = DamiaoMotorModel::DM4310;
+    spec.direction = direction;
+    spec.zero_offset_rad = direction == 1 ? 0.1 : -0.2;
+    spec.joint_lower_rad = -2.0;
+    spec.joint_upper_rad = 2.0;
+    spec.max_velocity_rad_s = 3.0;
+    spec.max_torque_nm = 2.0;
+    spec.healthy_status_mask = 1U << 0U;
+    spec.max_driver_temperature_raw = 80U;
+    spec.max_motor_temperature_raw = 90U;
+    return spec;
+}
+
+CanFrame feedback(std::uint32_t id,
+                  std::uint8_t reported_id,
+                  std::uint8_t status = 0U)
+{
+    CanFrame frame;
+    frame.id = id;
+    frame.len = 8;
+    frame.is_fd = true;
+    frame.bitrate_switch = true;
+    frame.rx_monotonic_ns = 100;
+    frame.data[0] =
+        static_cast<std::uint8_t>((status << 4U) | reported_id);
+    frame.data[1] = 0x80;
+    frame.data[2] = 0x00;
+    frame.data[3] = 0x80;
+    frame.data[4] = 0x08;
+    frame.data[5] = 0x00;
+    frame.data[6] = 30U;
+    frame.data[7] = 35U;
+    return frame;
+}
+
+std::chrono::steady_clock::time_point soon()
+{
+    return std::chrono::steady_clock::now() +
+           std::chrono::milliseconds(20);
+}
+
+std::size_t countLifecycleByte(
+    const std::vector<std::vector<CanFrame>>& batches,
+    const std::uint8_t value)
+{
+    std::size_t count = 0;
+    for (const auto& batch : batches) {
+        for (const auto& frame : batch) {
+            if (frame.len == 8 &&
+                frame.data[0] == 0xFF &&
+                frame.data[7] == value) {
+                ++count;
+            }
+        }
+    }
+    return count;
+}
+
+TEST(DamiaoCanFdChainTest, PassiveOpenNeverEnablesHardware)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus, {joint("J1", 1, 0x11, 1)},
+        DamiaoChainOptions{true, true, false});
+
+    ASSERT_TRUE(chain.init().ok());
+    ASSERT_TRUE(chain.openPassive().ok());
+    EXPECT_EQ(chain.state(), DamiaoChainState::Passive);
+    EXPECT_TRUE(bus->sent_batches.empty());
+
+    const auto arm_result = chain.arm(soon());
+    EXPECT_FALSE(arm_result.ok());
+    EXPECT_EQ(arm_result.code, ArmErrorCode::CommandRejected);
+    EXPECT_TRUE(bus->sent_batches.empty());
+}
+
+TEST(DamiaoCanFdChainTest, ExplicitArmAndExchangeUseUniqueConfiguredFeedback)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus,
+        {joint("J1", 1, 0x11, 1),
+         joint("J2", 2, 0x12, 2, -1)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(chain.init().ok());
+    ASSERT_TRUE(chain.openPassive().ok());
+
+    // A stale invalid frame is already queued before this request. The drain
+    // must remove it; only replies generated by the subsequent send may be
+    // accepted.
+    bus->replies.push_back(feedback(0x11, 1, 2));
+    bus->enqueueReplies({
+        feedback(0x12, 2),
+        feedback(0x11, 1),
+    });
+    ASSERT_TRUE(chain.arm(soon()).ok());
+    EXPECT_EQ(chain.state(), DamiaoChainState::Armed);
+    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 2U);
+
+    bus->enqueueReplies({
+        feedback(0x11, 1),
+        feedback(0x12, 2),
+    });
+    DamiaoMitCommand commands[2]{};
+    commands[0].tau_ff_nm = 1.0;
+    commands[1].tau_ff_nm = -1.0;
+    DamiaoJointFeedback states[2]{};
+    ASSERT_TRUE(chain.exchange(
+        commands, 2, states, 2, soon()).ok());
+    EXPECT_EQ(chain.state(), DamiaoChainState::Active);
+    EXPECT_TRUE(states[0].valid);
+    EXPECT_TRUE(states[1].valid);
+    // J2 has direction=-1 and offset=-0.2.
+    EXPECT_NEAR(states[1].q_rad, -0.2003814697265625, 1e-12);
+    EXPECT_NEAR(states[1].dq_rad_s, -0.0146484375, 1e-12);
+    EXPECT_NEAR(states[1].tau_nm, -0.0048828125, 1e-12);
+}
+
+TEST(DamiaoCanFdChainTest, MissedFeedbackLatchesFaultAndNeverReenables)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus, {joint("J1", 1, 0x11, 1)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(chain.init().ok());
+    ASSERT_TRUE(chain.openPassive().ok());
+    bus->enqueueReplies({feedback(0x11, 1)});
+    ASSERT_TRUE(chain.arm(soon()).ok());
+
+    DamiaoMitCommand command;
+    DamiaoJointFeedback state;
+    const auto result = chain.exchange(
+        &command, 1, &state, 1,
+        std::chrono::steady_clock::now() +
+            std::chrono::milliseconds(1));
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, ArmErrorCode::Timeout);
+    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
+    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U);
+    EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U);
+
+    // Clearing the fault is explicit and leaves the chain passive.
+    bus->enqueueReplies({feedback(0x11, 1)});
+    ASSERT_TRUE(chain.clearFault(soon()).ok());
+    EXPECT_EQ(chain.state(), DamiaoChainState::Passive);
+    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U);
+}
+
+TEST(DamiaoCanFdChainTest, DuplicateFeedbackCannotSatisfyAGroupCycle)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus,
+        {joint("J1", 1, 0x11, 1),
+         joint("J2", 2, 0x12, 2)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(chain.init().ok());
+    ASSERT_TRUE(chain.openPassive().ok());
+    bus->enqueueReplies({
+        feedback(0x11, 1),
+        feedback(0x12, 2),
+    });
+    ASSERT_TRUE(chain.arm(soon()).ok());
+
+    bus->enqueueReplies({
+        feedback(0x11, 1),
+        feedback(0x11, 1),
+    });
+    DamiaoMitCommand commands[2]{};
+    DamiaoJointFeedback states[2]{};
+    EXPECT_FALSE(chain.exchange(
+        commands, 2, states, 2,
+        std::chrono::steady_clock::now() +
+            std::chrono::milliseconds(1)).ok());
+    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
+    EXPECT_EQ(chain.statistics().duplicate_feedback, 1U);
+}
+
+TEST(DamiaoCanFdChainTest, RejectsUnreviewedStatusAndClassicFrame)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus, {joint("J1", 1, 0x11, 1)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(chain.init().ok());
+    ASSERT_TRUE(chain.openPassive().ok());
+    bus->enqueueReplies({feedback(0x11, 1)});
+    ASSERT_TRUE(chain.arm(soon()).ok());
+
+    bus->enqueueReplies({feedback(0x11, 1, 2)});
+    DamiaoMitCommand command;
+    DamiaoJointFeedback state;
+    EXPECT_FALSE(chain.exchange(
+        &command, 1, &state, 1, soon()).ok());
+    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
+
+    auto second_bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain second(
+        second_bus, {joint("J1", 1, 0x11, 1)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(second.init().ok());
+    ASSERT_TRUE(second.openPassive().ok());
+    auto classic = feedback(0x11, 1);
+    classic.is_fd = false;
+    classic.bitrate_switch = false;
+    second_bus->enqueueReplies({classic});
+    EXPECT_FALSE(second.arm(soon()).ok());
+    EXPECT_EQ(second.state(), DamiaoChainState::FaultLatched);
+
+    auto third_bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain third(
+        third_bus, {joint("J1", 1, 0x11, 1)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(third.init().ok());
+    ASSERT_TRUE(third.openPassive().ok());
+    auto error_passive = feedback(0x11, 1);
+    error_passive.error_state_indicator = true;
+    third_bus->enqueueReplies({error_passive});
+    EXPECT_FALSE(third.arm(soon()).ok());
+    EXPECT_EQ(third.state(), DamiaoChainState::FaultLatched);
+}
+
+TEST(DamiaoCanFdChainTest, HardwareEnableRequiresReviewedHealthContract)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    auto unreviewed = joint("J1", 1, 0x11, 1);
+    unreviewed.healthy_status_mask = 0U;
+    DamiaoCanFdChain chain(
+        bus, {unreviewed},
+        DamiaoChainOptions{true, true, true});
+
+    const auto result = chain.init();
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument);
+    EXPECT_FALSE(bus->initialized);
+}
+
+TEST(DamiaoCanFdChainTest, DisableReportsUnconfirmedSafetyFrames)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus, {joint("J1", 1, 0x11, 1)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(chain.init().ok());
+    ASSERT_TRUE(chain.openPassive().ok());
+    bus->enqueueReplies({feedback(0x11, 1)});
+    ASSERT_TRUE(chain.arm(soon()).ok());
+
+    bus->send_result = false;
+    const auto result = chain.disable();
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, ArmErrorCode::CommandFailed);
+    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
+}
+
+TEST(DamiaoCanFdChainTest, ExpiredDeadlineAfterDrainNeverCommitsEnable)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus, {joint("J1", 1, 0x11, 1)},
+        DamiaoChainOptions{true, true, true});
+    ASSERT_TRUE(chain.init().ok());
+    ASSERT_TRUE(chain.openPassive().ok());
+
+    bus->drain_delay = std::chrono::milliseconds(3);
+    bus->enqueueReplies({feedback(0x11, 1)});
+    const auto result = chain.arm(
+        std::chrono::steady_clock::now() +
+        std::chrono::milliseconds(1));
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, ArmErrorCode::Timeout);
+    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
+    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 0U);
+    EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U);
+}
+
+TEST(DamiaoCanFdChainTest, ConfigurationRejectsAmbiguousMappings)
+{
+    auto bus = std::make_shared<FakeCanbus>();
+    DamiaoCanFdChain chain(
+        bus,
+        {joint("J1", 1, 0x11, 1),
+         joint("J1", 2, 0x12, 2)},
+        DamiaoChainOptions{true, true, false});
+    const auto result = chain.init();
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument);
+    EXPECT_FALSE(bus->initialized);
+}
+
+} // namespace
+} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp

@@ -0,0 +1,150 @@
+#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
+
+#include <array>
+#include <cmath>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::device {
+namespace {
+
+void expectPayload(const CanFrame& frame,
+                   const std::array<std::uint8_t, 8>& expected)
+{
+    ASSERT_EQ(frame.len, expected.size());
+    for (std::size_t i = 0; i < expected.size(); ++i) {
+        EXPECT_EQ(frame.data[i], expected[i]) << "byte " << i;
+    }
+}
+
+TEST(DamiaoMitCodecTest, MatchesLegacyPythonGoldenVectors)
+{
+    CanFrame frame;
+    DamiaoMitCommand zero;
+    auto result = DamiaoMitCodec::encodeMit(
+        1, DamiaoMotorModel::DM4310, zero, true, true, frame);
+    ASSERT_TRUE(result);
+    EXPECT_EQ(result.saturation_mask, DAMIAO_SATURATION_NONE);
+    EXPECT_TRUE(frame.is_fd);
+    EXPECT_TRUE(frame.bitrate_switch);
+    expectPayload(frame, {0x7F, 0xFF, 0x7F, 0xF0,
+                          0x00, 0x00, 0x07, 0xFF});
+
+    DamiaoMitCommand nontrivial;
+    nontrivial.kp = 100.0;
+    nontrivial.kd = 1.0;
+    nontrivial.q_rad = 1.25;
+    nontrivial.dq_rad_s = -2.5;
+    nontrivial.tau_ff_nm = 3.0;
+    result = DamiaoMitCodec::encodeMit(
+        1, DamiaoMotorModel::DM4310, nontrivial, false, false, frame);
+    ASSERT_TRUE(result);
+    expectPayload(frame, {0x8C, 0xCC, 0x75, 0x43,
+                          0x33, 0x33, 0x3A, 0x65});
+}
+
+TEST(DamiaoMitCodecTest, ReportsProtocolSaturationWithoutHidingIt)
+{
+    DamiaoMitCommand command;
+    command.q_rad = 100.0;
+    command.dq_rad_s = -100.0;
+    command.kp = 600.0;
+    command.kd = -1.0;
+    command.tau_ff_nm = 100.0;
+    CanFrame frame;
+    const auto result = DamiaoMitCodec::encodeMit(
+        2, DamiaoMotorModel::DM4310, command, false, false, frame);
+    ASSERT_TRUE(result);
+    EXPECT_EQ(
+        result.saturation_mask,
+        DAMIAO_SATURATION_Q | DAMIAO_SATURATION_DQ |
+            DAMIAO_SATURATION_KP | DAMIAO_SATURATION_KD |
+            DAMIAO_SATURATION_TAU);
+}
+
+TEST(DamiaoMitCodecTest, RejectsNonFiniteInput)
+{
+    DamiaoMitCommand command;
+    command.tau_ff_nm = std::numeric_limits<double>::quiet_NaN();
+    CanFrame frame;
+    const auto result = DamiaoMitCodec::encodeMit(
+        1, DamiaoMotorModel::DM4310, command, false, false, frame);
+    EXPECT_FALSE(result);
+    EXPECT_EQ(result.error, DamiaoCodecError::NonFiniteInput);
+}
+
+TEST(DamiaoMitCodecTest, EncodesLifecycleFramesWithoutEnablingImplicitly)
+{
+    CanFrame frame;
+    ASSERT_EQ(DamiaoMitCodec::encodeMode(
+                  3, DamiaoMode::Enable, true, true, frame),
+              DamiaoCodecError::None);
+    expectPayload(frame, {0xFF, 0xFF, 0xFF, 0xFF,
+                          0xFF, 0xFF, 0xFF, 0xFC});
+
+    ASSERT_EQ(DamiaoMitCodec::encodeMode(
+                  3, DamiaoMode::Disable, true, true, frame),
+              DamiaoCodecError::None);
+    EXPECT_EQ(frame.data[7], 0xFD);
+    ASSERT_EQ(DamiaoMitCodec::encodeMode(
+                  3, DamiaoMode::SetZero, true, true, frame),
+              DamiaoCodecError::None);
+    EXPECT_EQ(frame.data[7], 0xFE);
+    ASSERT_EQ(DamiaoMitCodec::encodeMode(
+                  3, DamiaoMode::ClearFault, true, true, frame),
+              DamiaoCodecError::None);
+    EXPECT_EQ(frame.data[7], 0xFB);
+}
+
+TEST(DamiaoMitCodecTest, DecodesLegacyFeedbackAndRequiresConfiguredId)
+{
+    CanFrame frame;
+    frame.id = 0x11;
+    frame.len = 8;
+    frame.is_fd = true;
+    frame.bitrate_switch = true;
+    frame.rx_monotonic_ns = 1234567;
+    frame.data[0] = 0xA1;
+    frame.data[1] = 0x80;
+    frame.data[2] = 0x00;
+    frame.data[3] = 0x80;
+    frame.data[4] = 0x08;
+    frame.data[5] = 0x00;
+    frame.data[6] = 40;
+    frame.data[7] = 41;
+
+    DamiaoJointFeedback feedback;
+    EXPECT_EQ(DamiaoMitCodec::decodeFeedback(
+                  frame, 0x12, DamiaoMotorModel::DM4310, feedback),
+              DamiaoCodecError::UnexpectedFeedbackId);
+    EXPECT_FALSE(feedback.valid);
+
+    ASSERT_EQ(DamiaoMitCodec::decodeFeedback(
+                  frame, 0x11, DamiaoMotorModel::DM4310, feedback),
+              DamiaoCodecError::None);
+    EXPECT_TRUE(feedback.valid);
+    EXPECT_EQ(feedback.reported_motor_id, 1);
+    EXPECT_EQ(feedback.status, 0x0A);
+    EXPECT_EQ(feedback.driver_temperature_raw, 40);
+    EXPECT_EQ(feedback.motor_temperature_raw, 41);
+    EXPECT_EQ(feedback.rx_monotonic_ns, 1234567);
+    EXPECT_NEAR(feedback.q_rad, 0.0003814697265625, 1e-12);
+    EXPECT_NEAR(feedback.dq_rad_s, 0.0146484375, 1e-12);
+    EXPECT_NEAR(feedback.tau_nm, 0.0048828125, 1e-12);
+}
+
+TEST(DamiaoMitCodecTest, ContainsAllLegacyMotorRanges)
+{
+    EXPECT_DOUBLE_EQ(
+        DamiaoMitCodec::limitsFor(DamiaoMotorModel::DM8009).tau_max_nm,
+        54.0);
+    EXPECT_DOUBLE_EQ(
+        DamiaoMitCodec::limitsFor(DamiaoMotorModel::DMH3510).dq_max_rad_s,
+        280.0);
+    EXPECT_FALSE(
+        DamiaoMitCodec::limitsFor(DamiaoMotorModel::Unknown).valid());
+}
+
+} // namespace
+} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp

@@ -0,0 +1,338 @@
+#include "arm/ume_robot_arm/include/ume_robot_arm.h"
+
+#include <chrono>
+#include <cstdint>
+#include <deque>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "canbus/abstract_canbus.h"
+#include "common/io/proto_file_io.h"
+
+#ifndef CMVR_UME_ARM_CONFIG_PATH
+#define CMVR_UME_ARM_CONFIG_PATH ""
+#endif
+
+namespace cmvr::device {
+namespace {
+
+class LoopbackDamiaoBus final : public AbstractCanbus {
+public:
+    std::string typeName() const override { return "LoopbackDamiaoBus"; }
+    bool init() override
+    {
+        std::lock_guard lock(mutex);
+        initialized = true;
+        return true;
+    }
+    bool start() override
+    {
+        std::lock_guard lock(mutex);
+        started = true;
+        is_started_ = true;
+        return true;
+    }
+    bool stop() override
+    {
+        std::lock_guard lock(mutex);
+        started = false;
+        is_started_ = false;
+        return true;
+    }
+
+    msgs::ErrorCode send(
+        const std::vector<CanFrame>& frames,
+        int32_t* frame_num) override
+    {
+        std::lock_guard lock(mutex);
+        if (!started || !frame_num ||
+            *frame_num != static_cast<int32_t>(frames.size())) {
+            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+        }
+        if (!send_result) {
+            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+        }
+        sent_batches.push_back(frames);
+        for (const auto& frame : frames) {
+            if (frame.id < 1U || frame.id > 8U) {
+                continue;
+            }
+            CanFrame reply;
+            reply.id = 0x10U + frame.id;
+            reply.len = 8U;
+            reply.is_fd = true;
+            reply.bitrate_switch = true;
+            reply.rx_monotonic_ns =
+                std::chrono::duration_cast<std::chrono::nanoseconds>(
+                    std::chrono::steady_clock::now().time_since_epoch())
+                    .count();
+            reply.data[0] = static_cast<std::uint8_t>(frame.id);
+            reply.data[1] = 0x80U;
+            reply.data[2] = 0x00U;
+            reply.data[3] = 0x80U;
+            reply.data[4] = 0x08U;
+            reply.data[5] = 0x00U;
+            replies.push_back(reply);
+        }
+        return msgs::ErrorCode::OK;
+    }
+
+    msgs::ErrorCode receive(
+        std::vector<CanFrame>* frames,
+        int32_t* frame_num) override
+    {
+        std::lock_guard lock(mutex);
+        if (!started || !frames || !frame_num || replies.empty()) {
+            return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+        }
+        frames->clear();
+        frames->push_back(replies.front());
+        replies.pop_front();
+        *frame_num = 1;
+        return msgs::ErrorCode::OK;
+    }
+
+    bool discardPendingFrames() override
+    {
+        std::lock_guard lock(mutex);
+        replies.clear();
+        return started;
+    }
+
+    std::string getErrorString(int32_t) override { return {}; }
+
+    void setSendResult(const bool result)
+    {
+        std::lock_guard lock(mutex);
+        send_result = result;
+    }
+
+    std::size_t lifecycleCount(const std::uint8_t byte) const
+    {
+        std::lock_guard lock(mutex);
+        std::size_t count = 0;
+        for (const auto& batch : sent_batches) {
+            for (const auto& frame : batch) {
+                if (frame.len == 8U &&
+                    frame.data[0] == 0xFFU &&
+                    frame.data[7] == byte) {
+                    ++count;
+                }
+            }
+        }
+        return count;
+    }
+
+    bool initialized{false};
+    bool started{false};
+    bool send_result{true};
+    std::deque<CanFrame> replies;
+    std::vector<std::vector<CanFrame>> sent_batches;
+    mutable std::mutex mutex;
+};
+
+config::RobotArmConfig configFor(const bool hardware_enabled)
+{
+    config::RobotArmConfig cfg;
+    cfg.set_id("ume_right");
+    auto* ume = cfg.mutable_ume();
+    ume->set_hardware_enabled(hardware_enabled);
+    ume->set_control_frequency_hz(800U);
+    ume->set_cycle_deadline_us(1000U);
+    ume->set_feedback_watchdog_ms(20U);
+    auto* can = ume->mutable_can();
+    can->set_interface_name("fake-can");
+    can->set_enable_fd(true);
+    can->set_bitrate_switch(true);
+    can->set_send_timeout_us(100U);
+    can->set_receive_timeout_us(100U);
+    can->set_receive_own_messages(false);
+    for (std::uint32_t i = 1; i <= 8U; ++i) {
+        auto* joint = ume->add_joints();
+        joint->set_joint_name("RJ" + std::to_string(i));
+        joint->set_command_id(i);
+        joint->set_feedback_id(0x10U + i);
+        joint->set_reported_motor_id(i);
+        joint->set_model(config::DAMIAO_MOTOR_MODEL_DM4310);
+        joint->set_direction(1);
+        joint->set_joint_lower_rad(-2.0);
+        joint->set_joint_upper_rad(2.0);
+        joint->set_max_velocity_rad_s(3.0);
+        joint->set_max_torque_nm(2.0);
+        joint->add_healthy_feedback_status(0U);
+        joint->set_max_driver_temperature_raw(80U);
+        joint->set_max_motor_temperature_raw(90U);
+    }
+    return cfg;
+}
+
+bool waitUntil(
+    const std::function<bool()>& predicate,
+    const std::chrono::milliseconds timeout)
+{
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (std::chrono::steady_clock::now() < deadline) {
+        if (predicate()) {
+            return true;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    return predicate();
+}
+
+TEST(UmeRobotArmTest, LifecycleIsPassiveUntilExplicitFreshTorqueCommand)
+{
+    auto bus = std::make_shared<LoopbackDamiaoBus>();
+    UmeRobotArm arm(configFor(true), bus);
+
+    ASSERT_TRUE(arm.init());
+    EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U);
+    ASSERT_TRUE(arm.start());
+    EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U);
+
+    TorqueServoOptions options;
+    options.period = 0.00125;
+    options.command_watchdog_ms = 100U;
+    ASSERT_TRUE(arm.startTorqueMode(options).ok());
+    JointTorqueCommand command;
+    command.torque.assign(8U, 0.0);
+    ASSERT_TRUE(arm.servoTorque(command).ok());
+    ASSERT_TRUE(arm.torqueOn().ok());
+
+    ASSERT_TRUE(waitUntil(
+        [&arm] { return arm.getJointState().sequence > 0U; },
+        std::chrono::milliseconds(30)));
+    const auto state = arm.getJointState();
+    EXPECT_TRUE(state.position_valid);
+    EXPECT_TRUE(state.velocity_valid);
+    EXPECT_TRUE(state.effort_valid);
+    EXPECT_EQ(state.position.size(), 8U);
+    EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U);
+    EXPECT_EQ(arm.getControlMode(), ControlMode::Torque);
+
+    EXPECT_TRUE(arm.stop());
+    EXPECT_GE(bus->lifecycleCount(0xFDU), 8U);
+}
+
+TEST(UmeRobotArmTest, StaleCommandLatchesFaultAndNeverReenables)
+{
+    auto bus = std::make_shared<LoopbackDamiaoBus>();
+    UmeRobotArm arm(configFor(true), bus);
+    ASSERT_TRUE(arm.init());
+    ASSERT_TRUE(arm.start());
+
+    TorqueServoOptions options;
+    options.period = 0.001;
+    options.command_watchdog_ms = 2U;
+    ASSERT_TRUE(arm.startTorqueMode(options).ok());
+    JointTorqueCommand command;
+    command.torque.assign(8U, 0.0);
+    ASSERT_TRUE(arm.servoTorque(command).ok());
+    ASSERT_TRUE(arm.torqueOn().ok());
+    ASSERT_TRUE(waitUntil(
+        [&arm] { return arm.isFault(); },
+        std::chrono::milliseconds(50)));
+
+    EXPECT_FALSE(arm.busy());
+    EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U);
+    EXPECT_GE(bus->lifecycleCount(0xFDU), 8U);
+    EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault);
+}
+
+TEST(UmeRobotArmTest, HardwareGateRejectsEnableWithoutWritingIt)
+{
+    auto bus = std::make_shared<LoopbackDamiaoBus>();
+    UmeRobotArm arm(configFor(false), bus);
+    ASSERT_TRUE(arm.init());
+    ASSERT_TRUE(arm.start());
+    ASSERT_TRUE(arm.startTorqueMode(TorqueServoOptions{}).ok());
+    JointTorqueCommand command;
+    command.torque.assign(8U, 0.0);
+    ASSERT_TRUE(arm.servoTorque(command).ok());
+
+    const auto result = arm.torqueOn();
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, ArmErrorCode::CommandRejected);
+    EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U);
+}
+
+TEST(UmeRobotArmTest, PositionServoIsExplicitlyUnsupported)
+{
+    auto bus = std::make_shared<LoopbackDamiaoBus>();
+    UmeRobotArm arm(configFor(false), bus);
+    JointPositionCommand command;
+    command.position.assign(8U, 0.0);
+    const auto result = arm.servoJ(command);
+    EXPECT_EQ(result.code, ArmErrorCode::UnsupportedCommand);
+}
+
+TEST(UmeRobotArmTest, RejectsCycleDeadlineLongerThanControlPeriod)
+{
+    auto cfg = configFor(false);
+    cfg.mutable_ume()->set_cycle_deadline_us(2000U);
+    auto bus = std::make_shared<LoopbackDamiaoBus>();
+    UmeRobotArm arm(cfg, bus);
+
+    EXPECT_FALSE(arm.init());
+    EXPECT_FALSE(bus->initialized);
+    EXPECT_EQ(
+        arm.healthSnapshot().state,
+        DeviceHealthState::Fault);
+}
+
+TEST(UmeRobotArmTest, RejectsReportedMotorIdBeforeNarrowingConversion)
+{
+    auto cfg = configFor(false);
+    cfg.mutable_ume()->mutable_joints(0)->set_reported_motor_id(257U);
+    auto bus = std::make_shared<LoopbackDamiaoBus>();
+    UmeRobotArm arm(cfg, bus);
+
+    EXPECT_FALSE(arm.init());
+    EXPECT_FALSE(bus->initialized);
+    EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault);
+}
+
+TEST(UmeRobotArmTest, EmergencyStopReportsUnconfirmedDisable)
+{
+    auto bus = std::make_shared<LoopbackDamiaoBus>();
+    UmeRobotArm arm(configFor(true), bus);
+    ASSERT_TRUE(arm.init());
+    ASSERT_TRUE(arm.start());
+
+    bus->setSendResult(false);
+    const auto result = arm.emergencyStop();
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, ArmErrorCode::CommandFailed);
+    EXPECT_TRUE(arm.isEmergencyStopped());
+    EXPECT_TRUE(arm.isFault());
+    EXPECT_NE(
+        arm.healthSnapshot().error_message.find("zero/disable failed"),
+        std::string::npos);
+}
+
+TEST(UmeRobotArmTest, CheckedInDualArmConfigParsesAndKeepsHardwareDisabled)
+{
+    config::ArmRootConfig root;
+    ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+        CMVR_UME_ARM_CONFIG_PATH, &root));
+    ASSERT_EQ(root.arm().robot_arms_size(), 2);
+    for (const auto& arm : root.arm().robot_arms()) {
+        ASSERT_TRUE(arm.has_ume());
+        EXPECT_EQ(arm.ume().joints_size(), 8);
+        EXPECT_FALSE(arm.ume().hardware_enabled());
+        EXPECT_TRUE(arm.ume().can().enable_fd());
+        EXPECT_TRUE(arm.ume().can().bitrate_switch());
+        EXPECT_GT(arm.ume().can().send_timeout_us(), 0U);
+        EXPECT_FALSE(arm.ume().can().receive_own_messages());
+    }
+}
+
+} // namespace
+} // namespace cmvr::device

cmvr-es/devices/canbus/CMakeLists.txt

@@ -31,6 +31,21 @@ target_link_libraries(socket_can_client_raw_test
         glog
         cmvr_es::proto
 )
+add_test(
+        NAME socket_can_client_raw_test
+        COMMAND socket_can_client_raw_test
+)
+set(_socket_can_client_raw_test_environment
+        "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+)
+if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+    list(APPEND _socket_can_client_raw_test_environment
+            "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+endif()
+set_tests_properties(socket_can_client_raw_test PROPERTIES
+        TIMEOUT 10
+        ENVIRONMENT "${_socket_can_client_raw_test_environment}"
+)
 
 
 add_executable(protocol_data_test
@@ -93,4 +108,3 @@ target_link_libraries(can_receiver_test
         glog
         cmvr_es::proto
 )
-

cmvr-es/devices/canbus/abstract_canbus.h

@@ -3,6 +3,14 @@
 //
 
 #pragma once
+#include <algorithm>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <sstream>
+#include <string>
+#include <sys/time.h>
+
 #include "../abstract_device.h"
 #include "cmvr/msgs/error_code.pb.h"
 #include "canbus/common/byte.h"
@@ -14,20 +22,26 @@ namespace cmvr::device {
     */
     struct CanFrame {
         /// Message id
-        uint32_t id;
+        uint32_t id{0};
         /// Message length
-        uint8_t len;
+        uint8_t len{0};
-        /// Message content
+        /// Message content. Classic CAN uses at most the first 8 bytes.
-        uint8_t data[8];
+        uint8_t data[64]{};
-        /// Time stamp
+        bool is_extended_id{false};
-        struct timeval timestamp;
+        bool is_remote_frame{false};
+        bool is_error_frame{false};
+        bool is_fd{false};
+        bool bitrate_switch{false};
+        bool error_state_indicator{false};
+        /// Local host receive time used for freshness and watchdog checks.
+        int64_t rx_monotonic_ns{0};
+        /// Legacy wall-clock field retained for source compatibility.
+        struct timeval timestamp{0, 0};
 
         /**
          * @brief Constructor
          */
-        CanFrame() : id(0), len(0), timestamp{0} {
+        CanFrame() = default;
-            std::memset(data, 0, sizeof(data));
-        }
 
         /**
          * @brief CanFrame string including essential information about the message.
@@ -37,10 +51,15 @@ namespace cmvr::device {
             std::stringstream output_stream("");
             output_stream << "id:0x" << Byte::byte_to_hex(id)
                     << ",len:" << static_cast<int>(len) << ",data:";
-            for (uint8_t i = 0; i < len; ++i) {
+            const auto printable_len =
+                std::min<std::size_t>(len, sizeof(data));
+            for (std::size_t i = 0; i < printable_len; ++i) {
                 output_stream << Byte::byte_to_hex(data[i]);
             }
-            output_stream << ",";
+            output_stream << ",fd:" << is_fd
+                          << ",brs:" << bitrate_switch
+                          << ",extended:" << is_extended_id
+                          << ",error:" << is_error_frame << ",";
             return output_stream.str();
         }
     };
@@ -67,6 +86,28 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode send(const std::vector<CanFrame> &frames,
                                                int32_t *const frame_num) = 0;
 
+        /**
+         * @brief Send messages without starting a batch after an absolute
+         *        local deadline.
+         *
+         * Deadline-aware transports should override this method so their
+         * internal blocking budget is also capped by @p deadline. The default
+         * preserves source compatibility and at least rejects an already
+         * expired request before calling send().
+         */
+        virtual cmvr::msgs::ErrorCode sendUntil(
+            const std::vector<CanFrame>& frames,
+            int32_t* const frame_num,
+            const std::chrono::steady_clock::time_point deadline) {
+            if (std::chrono::steady_clock::now() >= deadline) {
+                if (frame_num) {
+                    *frame_num = 0;
+                }
+                return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+            }
+            return send(frames, frame_num);
+        }
+
         /**
          * @brief Send a single message.
          * @param frames A single-element vector containing only one message.
@@ -75,7 +116,9 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode sendSingleFrame(
             const std::vector<CanFrame> &frames) {
             if (frames.size() != 1U) {
-                CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size();
+                CMVR_LOG(ERROR) << "frames size not equal to 1, actual frame size: "
+                                << frames.size();
+                return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
             }
             int32_t n = 1;
             return send(frames, &n);
@@ -91,6 +134,17 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode receive(std::vector<CanFrame> *const frames,
                                                   int32_t *const frame_num) = 0;
 
+        /**
+         * @brief Discard frames already queued by the transport.
+         *
+         * Command/response protocols without a sequence field can use this
+         * immediately before sending a new request to reduce the risk that a
+         * response from an older cycle is accepted as fresh. Implementations
+         * must keep this call bounded. The conservative default reports that
+         * the transport cannot provide this guarantee.
+         */
+        virtual bool discardPendingFrames() { return false; }
+
         /**
          * @brief Get the error string.
          * @param status The status to get the error string.

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc

@@ -12,9 +12,13 @@
 #include "socket_can_client_raw.h"
 #include "absl/strings/str_cat.h"
 
+#include <cerrno>
+#include <chrono>
+#include <limits>
+#include <poll.h>
+
 namespace cmvr {
     namespace device {
-#define CAN_ID_MASK 0x1FFFF800U  // can_filter mask
 #define CAN_STANDARD_MAX_ID 0x7FFU
 
         using cmvr::msgs::ErrorCode;
@@ -24,8 +28,25 @@ namespace cmvr {
             auto channel_id = cfg.channel_id();
             port_ = static_cast<CANCardParameter::CANChannelId>(channel_id);
             interface_ = CANCardParameter::NATIVE;
-
+            interface_name_ =
-            enable_can_err_check_ = false;
+                cfg.has_interface_name() && !cfg.interface_name().empty()
+                    ? cfg.interface_name()
+                    : cfg.dev_id();
+            enable_fd_ = cfg.has_enable_fd() && cfg.enable_fd();
+            default_bitrate_switch_ =
+                cfg.has_bitrate_switch() && cfg.bitrate_switch();
+            receive_own_messages_ =
+                cfg.has_receive_own_messages() && cfg.receive_own_messages();
+            receive_timeout_us_ =
+                cfg.has_receive_timeout_us() && cfg.receive_timeout_us() > 0
+                    ? cfg.receive_timeout_us()
+                    : 100000U;
+            send_timeout_us_ =
+                cfg.has_send_timeout_us() && cfg.send_timeout_us() > 0
+                    ? cfg.send_timeout_us()
+                    : 100000U;
+            enable_can_err_check_ =
+                cfg.has_enable_error_frames() && cfg.enable_error_frames();
         }
 
 
@@ -49,7 +70,7 @@ namespace cmvr {
         }
 
         SocketCanClientRaw::~SocketCanClientRaw() {
-            if (dev_handler_) {
+            if (dev_handler_ >= 0) {
                 stop();
             }
         }
@@ -59,8 +80,8 @@ namespace cmvr {
                 status_ = ErrorCode::OK;
                 return true;
             }
-            struct sockaddr_can addr;
+            struct sockaddr_can addr {};
-            struct ifreq ifr;
+            struct ifreq ifr {};
 
             // open device
             // guss net is the device minor number, if one card is 0,1
@@ -91,17 +112,71 @@ namespace cmvr {
                 if (ret < 0) {
                     CMVR_LOG(ERROR) << "add receive msg id filter error code: " << ret;
                     status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                    stop();
                     return false;
                 }
             }
 
-            // 2. enable reception of can frames.
+            // 2. Explicitly opt into CAN-FD only when configured. This socket
-            int enable = 1;
+            // option does not configure the physical link bitrate or state.
-            ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable,
+            if (enable_fd_) {
-                               sizeof(enable));
+                int enable = 1;
-            if (ret < 0) {
+                ret = ::setsockopt(dev_handler_, SOL_CAN_RAW,
-                CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret;
+                                   CAN_RAW_FD_FRAMES, &enable, sizeof(enable));
+                if (ret < 0) {
+                    CMVR_LOG(ERROR) << "enable CAN-FD frames failed: "
+                                    << std::strerror(errno);
+                    status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                    stop();
+                    return false;
+                }
+            }
+
+            const int receive_own = receive_own_messages_ ? 1 : 0;
+            if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
+                             &receive_own, sizeof(receive_own)) < 0) {
+                CMVR_LOG(ERROR) << "configure receive-own-messages failed: "
+                                << std::strerror(errno);
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
+                return false;
+            }
+
+            if (enable_can_err_check_) {
+                const can_err_mask_t error_mask = CAN_ERR_MASK;
+                if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
+                                 &error_mask, sizeof(error_mask)) < 0) {
+                    CMVR_LOG(ERROR) << "configure CAN error filter failed: "
+                                    << std::strerror(errno);
+                    status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                    stop();
+                    return false;
+                }
+            }
+
+            struct timeval receive_timeout {
+                static_cast<time_t>(receive_timeout_us_ / 1000000U),
+                static_cast<suseconds_t>(receive_timeout_us_ % 1000000U)
+            };
+            if (::setsockopt(dev_handler_, SOL_SOCKET, SO_RCVTIMEO,
+                             &receive_timeout, sizeof(receive_timeout)) < 0) {
+                CMVR_LOG(ERROR) << "configure CAN receive timeout failed: "
+                                << std::strerror(errno);
+                status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
+                return false;
+            }
+
+            struct timeval send_timeout {
+                static_cast<time_t>(send_timeout_us_ / 1000000U),
+                static_cast<suseconds_t>(send_timeout_us_ % 1000000U)
+            };
+            if (::setsockopt(dev_handler_, SOL_SOCKET, SO_SNDTIMEO,
+                             &send_timeout, sizeof(send_timeout)) < 0) {
+                CMVR_LOG(ERROR) << "configure CAN send timeout failed: "
+                                << std::strerror(errno);
+                status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
                 return false;
             }
 
@@ -115,13 +190,39 @@ namespace cmvr {
                 interface_prefix = "can";
             }
 
-            const std::string can_name = absl::StrCat(interface_prefix, port_);
+            const std::string can_name =
-            std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ);
+                interface_name_.empty()
-            if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) {
+                    ? absl::StrCat(interface_prefix, port_)
-                CMVR_LOG(ERROR) << "ioctl error";
+                    : interface_name_;
+            if (can_name.size() >= IFNAMSIZ) {
+                CMVR_LOG(ERROR) << "CAN interface name is too long: " << can_name;
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
                 return false;
             }
+            std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ);
+            ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+            if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) {
+                CMVR_LOG(ERROR) << "CAN interface not found: " << can_name
+                                << ", error=" << std::strerror(errno);
+                status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
+                return false;
+            }
+
+            if (enable_fd_) {
+                struct ifreq mtu_request {};
+                std::strncpy(mtu_request.ifr_name, can_name.c_str(), IFNAMSIZ);
+                mtu_request.ifr_name[IFNAMSIZ - 1] = '\0';
+                if (::ioctl(dev_handler_, SIOCGIFMTU, &mtu_request) < 0 ||
+                    mtu_request.ifr_mtu != CANFD_MTU) {
+                    CMVR_LOG(ERROR) << "CAN-FD requested but interface MTU is not CANFD_MTU: "
+                                    << can_name;
+                    status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                    stop();
+                    return false;
+                }
+            }
 
             // bind socket to network interface
 
@@ -131,8 +232,10 @@ namespace cmvr {
                          sizeof(addr));
 
             if (ret < 0) {
-                CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret;
+                CMVR_LOG(ERROR) << "bind socket to CAN interface failed: "
+                                << std::strerror(errno);
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
                 return false;
             }
 
@@ -142,10 +245,11 @@ namespace cmvr {
         }
 
         bool SocketCanClientRaw::stop() {
-            if (is_started_) {
+            is_started_ = false;
-                is_started_ = false;
+            if (dev_handler_ >= 0) {
-
+                const int fd = dev_handler_;
-                int ret = close(dev_handler_);
+                dev_handler_ = -1;
+                int ret = close(fd);
                 if (ret < 0) {
                     CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret);
                     return false;
@@ -159,48 +263,190 @@ namespace cmvr {
         // Synchronous transmission of CAN messages
         ErrorCode SocketCanClientRaw::send(const std::vector<CanFrame> &frames,
                                            int32_t *const frame_num) {
+            return sendWithDeadline_(
+                frames, frame_num,
+                std::chrono::steady_clock::now() +
+                    std::chrono::microseconds(send_timeout_us_));
+        }
+
+        ErrorCode SocketCanClientRaw::sendUntil(
+            const std::vector<CanFrame>& frames,
+            int32_t* const frame_num,
+            const std::chrono::steady_clock::time_point deadline) {
+            return sendWithDeadline_(
+                frames, frame_num,
+                std::min(
+                    deadline,
+                    std::chrono::steady_clock::now() +
+                        std::chrono::microseconds(send_timeout_us_)));
+        }
+
+        ErrorCode SocketCanClientRaw::sendWithDeadline_(
+            const std::vector<CanFrame>& frames,
+            int32_t* const frame_num,
+            const std::chrono::steady_clock::time_point send_deadline) {
             if (frame_num == nullptr) {
-                CMVR_LOG(FATAL) << "frame_num is null";
+                CMVR_LOG(ERROR) << "frame_num is null";
+                return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
             }
-            if (frames.size() != static_cast<size_t>(*frame_num)) {
+            if (*frame_num < 0 ||
-                CMVR_LOG(FATAL) << "frames size does not match frame_num";
+                frames.size() != static_cast<size_t>(*frame_num) ||
+                frames.size() > static_cast<std::size_t>(MAX_CAN_SEND_FRAME_LEN)) {
+                CMVR_LOG(ERROR) << "frames size does not match a valid frame_num";
+                return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM;
             }
 
             if (!is_started_) {
                 CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!";
                 return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
             }
-            for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) {
+            if (std::chrono::steady_clock::now() >= send_deadline) {
-                if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) {
+                *frame_num = 0;
-                    CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len
+                return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                            << ", which is not equal to can message data length ("
+            }
-                            << CANBUS_MESSAGE_LENGTH << ").";
-                    return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                }
-                if (frames[i].id > CAN_STANDARD_MAX_ID) {
-                    send_frames_[i].can_id = (frames[i].id & CAN_EFF_MASK) | CAN_EFF_FLAG;
-                } else {
-                    send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK);
-                }
-                // CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id;
-                send_frames_[i].can_dlc = frames[i].len;
-                std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len);
 
-                // Synchronous transmission of CAN messages
+            // Validate the complete batch before committing its first frame.
-                int ret = static_cast<int>(
+            // This prevents a malformed later element from causing a valid
-                    write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i])));
+            // prefix of a cyclic command batch to reach the bus.
-                if (ret <= 0) {
+            for (size_t i = 0; i < frames.size(); ++i) {
-                    CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret;
+                const auto& source = frames[i];
-                    return ErrorCode::CAN_CLIENT_ERROR_BASE;
+                const auto max_length =
+                    source.is_fd ? CANFD_MESSAGE_LENGTH
+                                 : CANBUS_MESSAGE_LENGTH;
+                if (source.len > max_length ||
+                    (source.is_remote_frame && source.is_fd) ||
+                    (source.is_fd && !enable_fd_)) {
+                    *frame_num = 0;
+                    CMVR_LOG(ERROR) << "invalid CAN frame at index " << i
+                                    << ", len=" << static_cast<int>(source.len)
+                                    << ", fd=" << source.is_fd;
+                    return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
                 }
             }
 
+            int32_t sent_count = 0;
+            for (size_t i = 0; i < frames.size(); ++i) {
+                const auto& source = frames[i];
+                if (std::chrono::steady_clock::now() >= send_deadline) {
+                    *frame_num = sent_count;
+                    CMVR_LOG(ERROR)
+                        << "can " << port_
+                        << " send batch timed out before frame " << i;
+                    return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                }
+
+                canid_t can_id = source.is_extended_id ||
+                                         source.id > CAN_STANDARD_MAX_ID
+                                     ? (source.id & CAN_EFF_MASK) | CAN_EFF_FLAG
+                                     : (source.id & CAN_SFF_MASK);
+                if (source.is_remote_frame) {
+                    can_id |= CAN_RTR_FLAG;
+                }
+                if (source.is_error_frame) {
+                    can_id = (source.id & CAN_ERR_MASK) | CAN_ERR_FLAG;
+                }
+
+                const void* payload = nullptr;
+                std::size_t expected = 0;
+                struct canfd_frame fd_frame {};
+                struct can_frame classic_frame {};
+                if (source.is_fd) {
+                    fd_frame.can_id = can_id;
+                    fd_frame.len = source.len;
+                    if (source.bitrate_switch || default_bitrate_switch_) {
+                        fd_frame.flags |= CANFD_BRS;
+                    }
+                    if (source.error_state_indicator) {
+                        fd_frame.flags |= CANFD_ESI;
+                    }
+                    std::memcpy(fd_frame.data, source.data, source.len);
+                    expected = CANFD_MTU;
+                    payload = &fd_frame;
+                } else {
+                    classic_frame.can_id = can_id;
+                    classic_frame.can_dlc = source.len;
+                    std::memcpy(
+                        classic_frame.data, source.data, source.len);
+                    expected = CAN_MTU;
+                    payload = &classic_frame;
+                }
+
+                while (true) {
+                    const auto written = ::send(
+                        dev_handler_, payload, expected,
+                        MSG_DONTWAIT | MSG_NOSIGNAL);
+                    if (written == static_cast<ssize_t>(expected)) {
+                        ++sent_count;
+                        break;
+                    }
+                    if (written >= 0) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR)
+                            << "can " << port_
+                            << " sent a partial frame";
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+                    if (errno == EINTR) {
+                        continue;
+                    }
+                    if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR) << "can " << port_
+                                        << " send message failed: "
+                                        << std::strerror(errno);
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+
+                    const auto now = std::chrono::steady_clock::now();
+                    if (now >= send_deadline) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR)
+                            << "can " << port_
+                            << " send batch timed out";
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+                    const auto remaining =
+                        std::chrono::duration_cast<std::chrono::nanoseconds>(
+                            send_deadline - now);
+                    struct timespec timeout {
+                        static_cast<time_t>(
+                            remaining.count() / 1000000000LL),
+                        static_cast<long>(
+                            remaining.count() % 1000000000LL)
+                    };
+                    struct pollfd writable {
+                        dev_handler_, POLLOUT, 0
+                    };
+                    const int ready =
+                        ::ppoll(&writable, 1, &timeout, nullptr);
+                    if (ready == 0) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR)
+                            << "can " << port_
+                            << " send batch timed out";
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+                    if (ready < 0 && errno != EINTR) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR)
+                            << "can " << port_
+                            << " send poll failed: "
+                            << std::strerror(errno);
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+                }
+            }
+
+            *frame_num = sent_count;
             return ErrorCode::OK;
         }
 
         // buf size must be 8 bytes, every time, we receive only one frame
         ErrorCode SocketCanClientRaw::receive(std::vector<CanFrame> *const frames,
                                               int32_t *const frame_num) {
+            if (frames == nullptr || frame_num == nullptr) {
+                return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+            }
             if (!is_started_) {
                 CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!";
                 return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
@@ -213,39 +459,109 @@ namespace cmvr {
                 return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM;
             }
 
-            for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
+            frames->clear();
+            const int32_t requested = *frame_num;
+            *frame_num = 0;
+            for (int32_t i = 0; i < requested && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
                 CanFrame cf;
-                auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i]));
+                struct canfd_frame raw {};
+                const auto ret = ::read(dev_handler_, &raw, CANFD_MTU);
 
                 if (ret < 0) {
-                    CMVR_LOG(ERROR) << "receive message failed, error code: " << ret;
+                    if (errno == EAGAIN || errno == EWOULDBLOCK ||
-                    return ErrorCode::CAN_CLIENT_ERROR_BASE;
+                        errno == EINTR) {
-                }
+                        return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
-                if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH ||
+                    }
-                    recv_frames_[i].can_dlc < 0) {
+                    CMVR_LOG(ERROR) << "receive CAN message failed: "
-                    CMVR_LOG(ERROR) << "recv_frames_[" << i
+                                    << std::strerror(errno);
-                            << "].can_dlc = " << recv_frames_[i].can_dlc
-                            << ", which is not equal to can message data length ("
-                            << CANBUS_MESSAGE_LENGTH << ").";
                     return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
                 }
-                if (recv_frames_[i].can_id > CAN_STANDARD_MAX_ID) {
+                if (ret != CAN_MTU && ret != CANFD_MTU) {
-                    cf.id = enable_can_err_check_
+                    CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret;
-                                ? recv_frames_[i].can_id & CAN_EFF_MASK | CAN_ERR_FLAG
+                    return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
-                                : recv_frames_[i].can_id & CAN_EFF_MASK;
-                } else {
-                    cf.id = (recv_frames_[i].can_id & CAN_SFF_MASK);
                 }
-                // CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id;
+
-                cf.len = recv_frames_[i].can_dlc;
+                const canid_t raw_id = raw.can_id;
-                std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc);
+                cf.is_extended_id = (raw_id & CAN_EFF_FLAG) != 0;
+                cf.is_remote_frame = (raw_id & CAN_RTR_FLAG) != 0;
+                cf.is_error_frame = (raw_id & CAN_ERR_FLAG) != 0;
+                if (cf.is_error_frame) {
+                    cf.id = raw_id & CAN_ERR_MASK;
+                } else if (cf.is_extended_id) {
+                    cf.id = raw_id & CAN_EFF_MASK;
+                } else {
+                    cf.id = raw_id & CAN_SFF_MASK;
+                }
+
+                cf.is_fd = ret == CANFD_MTU;
+                if (cf.is_fd) {
+                    cf.len = raw.len;
+                    cf.bitrate_switch = (raw.flags & CANFD_BRS) != 0;
+                    cf.error_state_indicator = (raw.flags & CANFD_ESI) != 0;
+                } else {
+                    const auto* classic =
+                        reinterpret_cast<const struct can_frame*>(&raw);
+                    cf.len = classic->can_dlc;
+                }
+                const auto max_length =
+                    cf.is_fd ? CANFD_MESSAGE_LENGTH : CANBUS_MESSAGE_LENGTH;
+                if (cf.len > max_length) {
+                    return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+                }
+                std::memcpy(cf.data, raw.data, cf.len);
+
+                struct timespec monotonic {};
+                if (::clock_gettime(CLOCK_MONOTONIC, &monotonic) == 0) {
+                    cf.rx_monotonic_ns =
+                        static_cast<int64_t>(monotonic.tv_sec) * 1000000000LL +
+                        monotonic.tv_nsec;
+                }
+                ::gettimeofday(&cf.timestamp, nullptr);
                 frames->push_back(cf);
+                ++(*frame_num);
             }
             return ErrorCode::OK;
         }
 
-        std::string SocketCanClientRaw::getErrorString(const int32_t /*status*/) {
+        bool SocketCanClientRaw::discardPendingFrames() {
-            return "";
+            if (!is_started_ || dev_handler_ < 0) {
+                return false;
+            }
+
+            constexpr std::size_t kMaximumDrainFrames = 4096;
+            const auto deadline =
+                std::chrono::steady_clock::now() +
+                std::chrono::microseconds(send_timeout_us_);
+            std::size_t count = 0;
+            while (count < kMaximumDrainFrames &&
+                   std::chrono::steady_clock::now() < deadline) {
+                struct canfd_frame raw {};
+                const auto received = ::recv(
+                    dev_handler_, &raw, CANFD_MTU, MSG_DONTWAIT);
+                if (received == CAN_MTU || received == CANFD_MTU) {
+                    ++count;
+                    continue;
+                }
+                if (received < 0 &&
+                    (errno == EAGAIN || errno == EWOULDBLOCK)) {
+                    return true;
+                }
+                if (received < 0 && errno == EINTR) {
+                    continue;
+                }
+                CMVR_LOG(ERROR)
+                    << "failed while draining pending CAN frames: "
+                    << (received < 0 ? std::strerror(errno)
+                                     : "unexpected MTU");
+                return false;
+            }
+            CMVR_LOG(ERROR)
+                << "CAN receive queue did not drain within its bound";
+            return false;
+        }
+
+        std::string SocketCanClientRaw::getErrorString(const int32_t status) {
+            return std::strerror(status < 0 ? -status : status);
         }
     }
 }

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h

@@ -12,11 +12,13 @@
 #include <sys/types.h>
 
 #include <linux/can.h>
+#include <linux/can/error.h>
 #include <linux/can/raw.h>
 
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
+#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -51,6 +53,10 @@ namespace cmvr {
              */
             cmvr::msgs::ErrorCode send(const std::vector<CanFrame> &frames,
                                            int32_t *const frame_num) override;
+            cmvr::msgs::ErrorCode sendUntil(
+                const std::vector<CanFrame>& frames,
+                int32_t* const frame_num,
+                std::chrono::steady_clock::time_point deadline) override;
 
             /**
              * @brief Receive messages
@@ -60,6 +66,7 @@ namespace cmvr {
              */
             cmvr::msgs::ErrorCode receive(std::vector<CanFrame> *const frames,
                                               int32_t *const frame_num) override;
+            bool discardPendingFrames() override;
 
             /**
              * @brief Get the error string.
@@ -67,14 +74,23 @@ namespace cmvr {
              */
             std::string getErrorString(const int32_t status) override;
         private:
-            int dev_handler_ = 0;
+            int dev_handler_{-1};
             cmvr::msgs::CANCardParameter::CANChannelId port_;
             cmvr::msgs::CANCardParameter::CANInterface interface_;
-            can_frame send_frames_[MAX_CAN_SEND_FRAME_LEN];
+            std::string interface_name_;
-            can_frame recv_frames_[MAX_CAN_RECV_FRAME_LEN];
+            bool enable_fd_{false};
+            bool default_bitrate_switch_{false};
+            bool receive_own_messages_{false};
+            uint32_t receive_timeout_us_{100000};
+            uint32_t send_timeout_us_{100000};
 
             //
             bool enable_can_err_check_{false};
+
+            cmvr::msgs::ErrorCode sendWithDeadline_(
+                const std::vector<CanFrame>& frames,
+                int32_t* frame_num,
+                std::chrono::steady_clock::time_point deadline);
         };
     }
 }

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc

@@ -1,44 +1,226 @@
-#include "common/base/logging/logger.h"
-//
-// Created by lgv on 2025/7/16.
-//
-#include "cmvr/msgs/error_code.pb.h"
-#include "cmvr/msgs/can_card_parameter.pb.h"
 #include "canbus/can_client/socket/socket_can_client_raw.h"
-#include "gtest/gtest.h"
-namespace cmvr {
-namespace device {
-    using cmvr::msgs::ErrorCode;
-    using cmvr::msgs::CANCardParameter;
 
-    TEST(SocketCanClientRawTest, simple_test) {
+#include <algorithm>
-        CANCardParameter param;
+#include <chrono>
-        param.set_brand(CANCardParameter::SOCKET_CAN_RAW);
+#include <filesystem>
-        param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO);
+#include <iterator>
+#include <string>
+#include <thread>
+#include <vector>
 
-        cmvr::config::SocketCanConfig cfg;
+#include <gtest/gtest.h>
-        cfg.set_channel_id(0);
-        SocketCanClientRaw socket_can_client(cfg);
 
-        // EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE);
+namespace cmvr::device {
-        socket_can_client.start();
+namespace {
-        std::vector<CanFrame> frames;
+
-        int32_t num = 0;
+std::size_t openFileDescriptorCount()
-        EXPECT_EQ(socket_can_client.send(frames, &num),
+{
-                  ErrorCode::OK);
+    std::error_code error;
-        ++num;
+    std::size_t count = 0;
-        EXPECT_EQ(socket_can_client.receive(&frames, &num),
+    for (std::filesystem::directory_iterator iterator(
-                  ErrorCode::OK);
+             "/proc/self/fd", error);
-        CMVR_LOG(INFO) << frames.at(0).CanFrameString();
+         !error && iterator != std::filesystem::directory_iterator();
-        CanFrame can_frame;
+         iterator.increment(error)) {
-        can_frame.id = 0x123;
+        ++count;
-        can_frame.len = 8;
+    }
-        memset(can_frame.data, 0xA3, sizeof(can_frame.data));
+    return error ? 0U : count;
-        frames.clear();
+}
-        frames.push_back(can_frame);
+
-        EXPECT_EQ(socket_can_client.sendSingleFrame(frames),
+config::SocketCanConfig vcanConfig(const bool enable_fd)
-                  ErrorCode::OK);
+{
-        socket_can_client.stop();
+    config::SocketCanConfig config;
+    config.set_interface_name("vcan0");
+    config.set_enable_fd(enable_fd);
+    config.set_bitrate_switch(enable_fd);
+    config.set_receive_own_messages(false);
+    config.set_receive_timeout_us(2000U);
+    config.set_send_timeout_us(2000U);
+    return config;
+}
+
+bool vcanAvailable()
+{
+    return ::if_nametoindex("vcan0") != 0U;
+}
+
+TEST(SocketCanClientRawTest, MissingClassicInterfaceFailsWithoutLeakingFd)
+{
+    config::SocketCanConfig config;
+    config.set_interface_name("cmvr_no_such_can");
+    config.set_enable_fd(false);
+    config.set_receive_timeout_us(100U);
+    config.set_send_timeout_us(100U);
+    SocketCanClientRaw client(config);
+
+    const auto before = openFileDescriptorCount();
+    ASSERT_GT(before, 0U);
+    for (int attempt = 0; attempt < 32; ++attempt) {
+        EXPECT_FALSE(client.start());
+        EXPECT_TRUE(client.stop());
+    }
+    const auto after = openFileDescriptorCount();
+    EXPECT_LE(after, before + 1U);
+}
+
+TEST(SocketCanClientRawTest, ClosedClientRejectsClassicSendAndReceive)
+{
+    config::SocketCanConfig config;
+    config.set_interface_name("cmvr_no_such_can");
+    config.set_enable_fd(false);
+    SocketCanClientRaw client(config);
+
+    CanFrame frame;
+    frame.id = 0x123U;
+    frame.len = 8U;
+    frame.is_fd = false;
+    std::fill(std::begin(frame.data), std::end(frame.data), 0xA3U);
+    std::vector<CanFrame> frames{frame};
+    int32_t count = 1;
+    EXPECT_EQ(
+        client.send(frames, &count),
+        msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED);
+
+    count = 1;
+    EXPECT_EQ(
+        client.receive(&frames, &count),
+        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
+    EXPECT_NE(frame.CanFrameString().find("fd:0"), std::string::npos);
+}
+
+TEST(SocketCanClientRawTest, VcanTransmitsClassicAndCanFdBatches)
+{
+    if (!vcanAvailable()) {
+        GTEST_SKIP() << "vcan0 is not available in this network namespace";
+    }
+
+    SocketCanClientRaw classic_tx(vcanConfig(false));
+    SocketCanClientRaw fd_rx(vcanConfig(true));
+    ASSERT_TRUE(classic_tx.start());
+    ASSERT_TRUE(fd_rx.start());
+
+    CanFrame first;
+    first.id = 0x123U;
+    first.len = 8U;
+    first.data[0] = 0xA1U;
+    CanFrame second;
+    second.id = 0x456U;
+    second.len = 3U;
+    second.data[0] = 0xB2U;
+    std::vector<CanFrame> classic_frames{first, second};
+    int32_t count = 2;
+    ASSERT_EQ(
+        classic_tx.send(classic_frames, &count),
+        msgs::ErrorCode::OK);
+    ASSERT_EQ(count, 2);
+
+    for (const auto& expected : classic_frames) {
+        std::vector<CanFrame> received;
+        int32_t receive_count = 1;
+        ASSERT_EQ(
+            fd_rx.receive(&received, &receive_count),
+            msgs::ErrorCode::OK);
+        ASSERT_EQ(receive_count, 1);
+        ASSERT_EQ(received.size(), 1U);
+        EXPECT_FALSE(received.front().is_fd);
+        EXPECT_EQ(received.front().id, expected.id);
+        EXPECT_EQ(received.front().len, expected.len);
+        EXPECT_EQ(received.front().data[0], expected.data[0]);
+    }
+    ASSERT_TRUE(classic_tx.stop());
+    ASSERT_TRUE(fd_rx.stop());
+
+    SocketCanClientRaw fd_tx(vcanConfig(true));
+    SocketCanClientRaw second_fd_rx(vcanConfig(true));
+    ASSERT_TRUE(fd_tx.start());
+    ASSERT_TRUE(second_fd_rx.start());
+    CanFrame fd_first;
+    fd_first.id = 0x201U;
+    fd_first.len = 12U;
+    fd_first.is_fd = true;
+    fd_first.bitrate_switch = true;
+    fd_first.data[11] = 0xC3U;
+    CanFrame fd_second;
+    fd_second.id = 0x202U;
+    fd_second.len = 64U;
+    fd_second.is_fd = true;
+    fd_second.bitrate_switch = true;
+    fd_second.data[63] = 0xD4U;
+    std::vector<CanFrame> fd_frames{fd_first, fd_second};
+    count = 2;
+    ASSERT_EQ(fd_tx.send(fd_frames, &count), msgs::ErrorCode::OK);
+    ASSERT_EQ(count, 2);
+
+    for (const auto& expected : fd_frames) {
+        std::vector<CanFrame> received;
+        int32_t receive_count = 1;
+        ASSERT_EQ(
+            second_fd_rx.receive(&received, &receive_count),
+            msgs::ErrorCode::OK);
+        ASSERT_EQ(received.size(), 1U);
+        EXPECT_TRUE(received.front().is_fd);
+        EXPECT_TRUE(received.front().bitrate_switch);
+        EXPECT_EQ(received.front().id, expected.id);
+        EXPECT_EQ(received.front().len, expected.len);
+        EXPECT_EQ(
+            received.front().data[expected.len - 1U],
+            expected.data[expected.len - 1U]);
     }
 }
+
+TEST(SocketCanClientRawTest, VcanDrainAndBatchValidationAreFailClosed)
+{
+    if (!vcanAvailable()) {
+        GTEST_SKIP() << "vcan0 is not available in this network namespace";
+    }
+
+    SocketCanClientRaw tx(vcanConfig(false));
+    SocketCanClientRaw rx(vcanConfig(false));
+    ASSERT_TRUE(tx.start());
+    ASSERT_TRUE(rx.start());
+
+    CanFrame valid;
+    valid.id = 0x321U;
+    valid.len = 8U;
+    valid.data[0] = 0x5AU;
+    std::vector<CanFrame> one{valid};
+    int32_t count = 1;
+    ASSERT_EQ(tx.send(one, &count), msgs::ErrorCode::OK);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    ASSERT_TRUE(rx.discardPendingFrames());
+
+    std::vector<CanFrame> received;
+    int32_t receive_count = 1;
+    EXPECT_EQ(
+        rx.receive(&received, &receive_count),
+        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
+
+    CanFrame invalid = valid;
+    invalid.id = 0x322U;
+    invalid.len = 9U;
+    std::vector<CanFrame> invalid_batch{valid, invalid};
+    count = 2;
+    EXPECT_EQ(
+        tx.send(invalid_batch, &count),
+        msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED);
+    EXPECT_EQ(count, 0);
+    receive_count = 1;
+    EXPECT_EQ(
+        rx.receive(&received, &receive_count),
+        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
+
+    count = 1;
+    EXPECT_EQ(
+        tx.sendUntil(
+            one, &count,
+            std::chrono::steady_clock::now() -
+                std::chrono::microseconds(1)),
+        msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED);
+    EXPECT_EQ(count, 0);
+    receive_count = 1;
+    EXPECT_EQ(
+        rx.receive(&received, &receive_count),
+        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
 }
+
+} // namespace
+} // namespace cmvr::device

cmvr-es/devices/canbus/common/canbus_consts.h

@@ -26,9 +26,14 @@
 namespace cmvr {
     namespace device {
         const int32_t CAN_FRAME_SIZE = 8;
-        const int32_t MAX_CAN_SEND_FRAME_LEN = 1;
+        const int32_t CAN_FD_FRAME_SIZE = 64;
+        // One UME cycle may submit a complete arm worth of frames. The receive
+        // API intentionally remains one-frame-at-a-time so a caller never
+        // blocks waiting to fill an artificial batch.
+        const int32_t MAX_CAN_SEND_FRAME_LEN = 64;
         const int32_t MAX_CAN_RECV_FRAME_LEN = 1;     // 这个暂时改为 1 ,大量数据的时候改为 10
 
         const int32_t CANBUS_MESSAGE_LENGTH = 8; // according to ISO-11891-1
+        const int32_t CANFD_MESSAGE_LENGTH = 64;
     }
 }

cmvr-es/devices/motor/CMakeLists.txt

@@ -17,4 +17,52 @@ add_subdirectory(drivers/ti5_canopen)
 add_subdirectory(drivers/mujoco)
 add_subdirectory(bus_runtime)
 add_subdirectory(drivers/ethercat_motor)
+add_subdirectory(drivers/modbus_plc_motor)
+
+if(BUILD_TESTING)
+    enable_testing()
+    set(CMVR_MOTOR_LIBMODBUS_ROOT
+            ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11)
+    add_executable(modbus_tcp_motor_bus_runtime_test
+            bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp
+    )
+    target_include_directories(modbus_tcp_motor_bus_runtime_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+            ${CMVR_MOTOR_LIBMODBUS_ROOT}/include
+    )
+    target_link_directories(modbus_tcp_motor_bus_runtime_test
+            PRIVATE
+            ${CMVR_MOTOR_LIBMODBUS_ROOT}/lib
+    )
+    target_link_libraries(modbus_tcp_motor_bus_runtime_test
+            PRIVATE
+            cmvr_es::device::motor_bus_runtime
+            cmvr_es::device::modbus_plc_motor_driver
+            cmvr_es::proto
+            modbus
+            gtest
+            gtest_main
+            pthread
+            glog
+    )
+    target_compile_definitions(modbus_tcp_motor_bus_runtime_test
+            PRIVATE
+            CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH="${CMAKE_SOURCE_DIR}/cmvr-es/config/devices/motor/plc_motors.pb.txt"
+    )
+    add_test(NAME modbus_tcp_motor_bus_runtime_test
+             COMMAND modbus_tcp_motor_bus_runtime_test)
+    set(_modbus_motor_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _modbus_motor_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(modbus_tcp_motor_bus_runtime_test PROPERTIES
+            TIMEOUT 15
+            ENVIRONMENT "${_modbus_motor_test_environment}"
+    )
+endif()
+
 add_subdirectory(manager)

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

@@ -2,20 +2,29 @@ add_library(motor_bus_runtime SHARED
         can/src/can_motor_bus_runtime.cpp
         mujoco/src/mujoco_motor_bus_runtime.cpp
         ethercat/src/ethercat_motor_bus_runtime.cpp
+        modbus_tcp/src/modbus_tcp_client.cpp
+        modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp
 )
 
 set(IGH_ETHERCAT_ROOT
         ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0
 )
+set(CMVR_LIBMODBUS_ROOT
+        ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11
+)
 
 target_include_directories(motor_bus_runtime
         PUBLIC
         ${CMAKE_CURRENT_SOURCE_DIR}
         PRIVATE
         ${IGH_ETHERCAT_ROOT}/include
+        ${CMVR_LIBMODBUS_ROOT}/include
 )
 
-target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib)
+target_link_directories(motor_bus_runtime PRIVATE
+        ${IGH_ETHERCAT_ROOT}/lib
+        ${CMVR_LIBMODBUS_ROOT}/lib
+)
 
 target_link_libraries(motor_bus_runtime
         PUBLIC
@@ -24,6 +33,7 @@ target_link_libraries(motor_bus_runtime
         cmvr_es::mujoco_world
         PRIVATE
         ethercat
+        modbus
         cmvr_es::device::canbus
         glog
 )

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

@@ -68,16 +68,18 @@ bool CanMotorBusRuntime::start()
         return false;
     }
 
-    auto ret = sender_->Start();
+    // Start the receiver first so a protocol response cannot arrive before the
+    // receive path is ready.
+    auto ret = receiver_->Start();
     if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
         stop();
         return false;
     }
 
-    ret = receiver_->Start();
+    ret = sender_->Start();
     if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
         stop();
         return false;
     }

cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h

@@ -0,0 +1,209 @@
+#ifndef CMVR_ES_CMVR_PLC_REGISTER_MAP_H
+#define CMVR_ES_CMVR_PLC_REGISTER_MAP_H
+
+#include <array>
+#include <cstddef>
+#include <cstdint>
+
+namespace cmvr::device::cmvr_plc {
+
+constexpr std::uint16_t kMagicCm = 0x434d;
+constexpr std::uint16_t kMagicVr = 0x5652;
+constexpr std::uint16_t kProtocolMajor = 1;
+constexpr std::uint16_t kProtocolMinor = 0;
+constexpr double kPositionScale = 1000000.0;
+constexpr double kVelocityScale = 1000000.0;
+constexpr double kAccelerationScale = 1000000.0;
+constexpr double kTorqueScale = 1000.0;
+
+constexpr int kGlobalRegisterCount = 32;
+constexpr int kMagicCmOffset = 0;
+constexpr int kMagicVrOffset = 1;
+constexpr int kProtocolMajorOffset = 2;
+constexpr int kProtocolMinorOffset = 3;
+constexpr int kAxisCountOffset = 4;
+constexpr int kPlcGlobalStateOffset = 5;
+constexpr int kPlcBootIdOffset = 6;
+constexpr int kCmvrSessionIdOffset = 8;
+constexpr int kCmvrHeartbeatOffset = 10;
+constexpr int kPlcHeartbeatOffset = 12;
+constexpr int kCommunicationWatchdogOffset = 14;
+constexpr int kGlobalErrorOffset = 16;
+constexpr int kOwnerStateOffset = 17;
+constexpr int kOwnerSessionIdOffset = 18;
+
+constexpr int kAxisFirstOffset = 100;
+constexpr int kAxisRegisterStride = 128;
+constexpr int kAxisControlRegisterCount = 64;
+constexpr int kAxisStatusRelativeOffset = 64;
+constexpr int kAxisStatusRegisterCount = 64;
+
+constexpr int axisBase(const std::uint32_t axis_index)
+{
+    return kAxisFirstOffset + static_cast<int>(axis_index) * kAxisRegisterStride;
+}
+
+enum class CommandCode : std::uint16_t {
+    Nop = 0,
+    SetZero = 1,
+    MoveToZero = 2,
+    ProfilePosition = 3,
+    ProfileVelocity = 4,
+    OpenCyclicPosition = 5,
+    CyclicPositionSample = 6,
+    OpenCyclicVelocity = 7,
+    CyclicVelocitySample = 8,
+    CloseCyclicStream = 9,
+    QuickStop = 10,
+    Enable = 11,
+    Disable = 12,
+};
+
+enum class CommandState : std::uint16_t {
+    Idle = 0,
+    Received = 1,
+    Validating = 2,
+    Accepted = 3,
+    Running = 4,
+    TargetReached = 5,
+    Completed = 6,
+    Rejected = 7,
+    Failed = 8,
+    TimedOut = 9,
+    QuickStopped = 10,
+    CommunicationLost = 11,
+};
+
+enum class ResultCode : std::uint16_t {
+    Ok = 0,
+    InvalidCommand = 1,
+    InvalidParameter = 2,
+    AxisNotReady = 3,
+    AxisBusy = 4,
+    NotEnabled = 5,
+    PositionLimit = 6,
+    VelocityLimit = 7,
+    AccelerationLimit = 8,
+    ZeroNotValid = 9,
+    DriveFault = 10,
+    CommandTimeout = 11,
+    SequenceError = 12,
+    SessionMismatch = 13,
+    CommunicationWatchdog = 14,
+    CyclicWatchdog = 15,
+    Unsupported = 16,
+    InternalError = 17,
+};
+
+enum StatusFlag : std::uint16_t {
+    Enabled = 1U << 0U,
+    Moving = 1U << 1U,
+    TargetReached = 1U << 2U,
+    Fault = 1U << 3U,
+    QuickStopActive = 1U << 4U,
+    CommunicationWatchdogExpired = 1U << 5U,
+    CyclicWatchdogExpired = 1U << 6U,
+    ZeroValid = 1U << 7U,
+    StreamActive = 1U << 8U,
+    CommandBusy = 1U << 9U,
+};
+
+constexpr std::size_t kCommandPayloadRegisterCount = 62;
+constexpr int kCommitSequenceRelativeOffset = 62;
+
+constexpr std::size_t kPayloadSequence = 0;
+constexpr std::size_t kCommandCode = 2;
+constexpr std::size_t kCommandFlags = 3;
+constexpr std::size_t kTargetPosition = 4;
+constexpr std::size_t kTargetVelocity = 6;
+constexpr std::size_t kAcceleration = 8;
+constexpr std::size_t kTargetTorque = 10;
+constexpr std::size_t kPositionTolerance = 12;
+constexpr std::size_t kVelocityTolerance = 14;
+constexpr std::size_t kCommandTimeout = 16;
+constexpr std::size_t kStreamWatchdog = 18;
+constexpr std::size_t kCyclicSampleSequence = 20;
+constexpr std::size_t kClientMonotonicTime = 22;
+constexpr std::size_t kExpectedZeroEpoch = 24;
+constexpr std::size_t kDisconnectAction = 26;
+constexpr std::size_t kCommandSessionId = 27;
+constexpr std::size_t kPayloadSequenceMirror = 60;
+
+constexpr std::size_t kAckSequence = 0;
+constexpr std::size_t kActiveSequence = 2;
+constexpr std::size_t kCommandState = 4;
+constexpr std::size_t kResultCode = 5;
+constexpr std::size_t kAxisState = 6;
+constexpr std::size_t kCurrentMode = 7;
+constexpr std::size_t kActualPosition = 8;
+constexpr std::size_t kActualVelocity = 10;
+constexpr std::size_t kActualTorque = 12;
+constexpr std::size_t kTargetPositionStatus = 14;
+constexpr std::size_t kTargetVelocityStatus = 16;
+constexpr std::size_t kStatusFlags = 18;
+constexpr std::size_t kDriveStatusword = 19;
+constexpr std::size_t kFaultCode = 20;
+constexpr std::size_t kZeroEpoch = 22;
+constexpr std::size_t kLastAppliedCyclicSequence = 24;
+constexpr std::size_t kStateSequence = 26;
+constexpr std::size_t kPlcMonotonicTime = 28;
+constexpr std::size_t kHeartbeatAge = 30;
+constexpr std::size_t kAckSessionId = 32;
+constexpr std::size_t kStateSequenceMirror = 62;
+
+enum class OwnerState : std::uint16_t {
+    None = 0,
+    Accepting = 1,
+    Accepted = 2,
+    Rejected = 3,
+};
+
+inline void encodeUint32(std::uint16_t* registers,
+                         const std::size_t offset,
+                         const std::uint32_t value)
+{
+    registers[offset] = static_cast<std::uint16_t>(value >> 16U);
+    registers[offset + 1] = static_cast<std::uint16_t>(value & 0xffffU);
+}
+
+inline void encodeInt32(std::uint16_t* registers,
+                        const std::size_t offset,
+                        const std::int32_t value)
+{
+    encodeUint32(registers, offset, static_cast<std::uint32_t>(value));
+}
+
+inline std::uint32_t decodeUint32(const std::uint16_t* registers,
+                                  const std::size_t offset)
+{
+    return (static_cast<std::uint32_t>(registers[offset]) << 16U) |
+           static_cast<std::uint32_t>(registers[offset + 1]);
+}
+
+inline std::int32_t decodeInt32(const std::uint16_t* registers,
+                               const std::size_t offset)
+{
+    return static_cast<std::int32_t>(decodeUint32(registers, offset));
+}
+
+inline bool isTerminal(const CommandState state)
+{
+    return state == CommandState::Completed ||
+           state == CommandState::Rejected ||
+           state == CommandState::Failed ||
+           state == CommandState::TimedOut ||
+           state == CommandState::QuickStopped ||
+           state == CommandState::CommunicationLost;
+}
+
+inline bool isFailure(const CommandState state)
+{
+    return state == CommandState::Rejected ||
+           state == CommandState::Failed ||
+           state == CommandState::TimedOut ||
+           state == CommandState::CommunicationLost;
+}
+
+} // namespace cmvr::device::cmvr_plc
+
+#endif // CMVR_ES_CMVR_PLC_REGISTER_MAP_H

cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h

@@ -0,0 +1,47 @@
+#ifndef CMVR_ES_MODBUS_TCP_CLIENT_H
+#define CMVR_ES_MODBUS_TCP_CLIENT_H
+
+#include <cstdint>
+#include <atomic>
+#include <string>
+#include <vector>
+
+struct _modbus;
+using modbus_t = struct _modbus;
+
+namespace cmvr::device {
+
+class ModbusTcpClient {
+public:
+    ModbusTcpClient() = default;
+    ~ModbusTcpClient();
+
+    ModbusTcpClient(const ModbusTcpClient&) = delete;
+    ModbusTcpClient& operator=(const ModbusTcpClient&) = delete;
+
+    bool open(const std::string& host,
+              std::uint16_t port,
+              std::uint8_t unit_id,
+              std::uint32_t connect_timeout_ms,
+              std::uint32_t response_timeout_ms);
+    void close();
+    void shutdown();
+    bool connected() const { return context_ != nullptr; }
+
+    bool readHoldingRegisters(int address, int count, std::vector<std::uint16_t>& values);
+    bool writeHoldingRegisters(int address, const std::uint16_t* values, int count);
+    bool writeHoldingRegisters(int address, const std::vector<std::uint16_t>& values);
+
+    const std::string& lastError() const { return last_error_; }
+
+private:
+    void setLastErrnoError_(const char* operation);
+
+    modbus_t* context_{nullptr};
+    std::atomic<int> socket_fd_{-1};
+    std::string last_error_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_MODBUS_TCP_CLIENT_H

cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h

@@ -0,0 +1,173 @@
+#ifndef CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H
+#define CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H
+
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+
+#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h"
+#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h"
+
+namespace cmvr::device {
+
+struct ModbusTcpMotorBusRuntimeTestAccess;
+
+struct CmvrPlcAxisCommand {
+    cmvr_plc::CommandCode code{cmvr_plc::CommandCode::Nop};
+    std::uint16_t flags{0};
+    std::int32_t target_position{0};
+    std::int32_t target_velocity{0};
+    std::int32_t acceleration{0};
+    std::int32_t target_torque{0};
+    std::int32_t position_tolerance{0};
+    std::int32_t velocity_tolerance{0};
+    std::uint32_t command_timeout_ms{0};
+    std::uint32_t stream_watchdog_ms{0};
+    std::uint32_t cyclic_sample_sequence{0};
+    std::uint32_t client_monotonic_time_ms{0};
+    std::uint32_t expected_zero_epoch{0};
+    std::uint16_t disconnect_action{0};
+};
+
+struct CmvrPlcAxisStatus {
+    std::uint32_t ack_sequence{0};
+    std::uint32_t active_sequence{0};
+    cmvr_plc::CommandState command_state{cmvr_plc::CommandState::Idle};
+    cmvr_plc::ResultCode result_code{cmvr_plc::ResultCode::Ok};
+    std::uint16_t axis_state{0};
+    std::uint16_t current_mode{0};
+    std::int32_t actual_position{0};
+    std::int32_t actual_velocity{0};
+    std::int32_t actual_torque{0};
+    std::int32_t target_position{0};
+    std::int32_t target_velocity{0};
+    std::uint16_t status_flags{0};
+    std::uint16_t drive_statusword{0};
+    std::uint32_t fault_code{0};
+    std::uint32_t zero_epoch{0};
+    std::uint32_t last_applied_cyclic_sequence{0};
+    std::uint32_t state_sequence{0};
+    std::uint32_t plc_monotonic_time_ms{0};
+    std::uint32_t heartbeat_age_ms{0};
+    std::uint32_t ack_session_id{0};
+};
+
+class ModbusTcpMotorBusRuntime final : public AbstractMotorBusRuntime {
+public:
+    ModbusTcpMotorBusRuntime();
+    ~ModbusTcpMotorBusRuntime() override;
+
+    bool init(const config::MotorGroupConfig& group_cfg) override;
+    bool start() override;
+    void stop() override;
+    config::MotorBusType busType() const override { return config::MOTOR_BUS_MODBUS_TCP; }
+
+    bool hasMotor(std::uint8_t motor_id) const;
+    bool axisForMotor(std::uint8_t motor_id, std::uint32_t& axis_index) const;
+    bool connected() const { return connected_.load(); }
+    std::uint32_t sessionId() const { return session_id_.load(); }
+    std::uint32_t plcBootId() const { return plc_boot_id_.load(); }
+    std::uint64_t connectionEpoch() const { return connection_epoch_.load(); }
+    std::uint32_t streamWatchdogMs() const { return stream_watchdog_ms_; }
+    std::uint32_t commandAckTimeoutMs() const { return command_ack_timeout_ms_; }
+    const std::string& id() const { return id_; }
+    std::string lastError() const;
+
+    bool readAxisStatus(std::uint8_t motor_id, CmvrPlcAxisStatus& status);
+    bool submitAxisCommand(std::uint8_t motor_id,
+                           const CmvrPlcAxisCommand& command,
+                           bool wait_for_terminal_state = false,
+                           std::optional<std::uint64_t>
+                               expected_connection_epoch = std::nullopt);
+    bool submitAxisSafetyCommand(std::uint8_t motor_id,
+                                 const CmvrPlcAxisCommand& command);
+
+private:
+    friend struct ModbusTcpMotorBusRuntimeTestAccess;
+
+    bool connectAndHandshakeLocked_();
+    void markDisconnectedLocked_(const std::string& error);
+    bool readRegistersLocked_(int address, int count, std::vector<std::uint16_t>& values);
+    bool writeRegistersLocked_(int address, const std::uint16_t* values, int count);
+    bool writeHeartbeatLocked_();
+    bool readAxisStatusByIndex_(std::uint32_t axis_index, CmvrPlcAxisStatus& status);
+    bool waitForCommand_(std::uint32_t axis_index,
+                         std::uint32_t command_sequence,
+                         std::uint32_t command_session_id,
+                         std::uint64_t connection_epoch,
+                         std::uint64_t cancel_generation,
+                         bool cancel_on_safety_preemption,
+                         const CmvrPlcAxisCommand& command,
+                         bool wait_for_terminal_state,
+                         const CmvrPlcAxisStatus& initial_status);
+    bool submitAxisCommandImpl_(std::uint8_t motor_id,
+                                const CmvrPlcAxisCommand& command,
+                                bool wait_for_terminal_state,
+                                bool safety_priority,
+                                std::optional<std::uint64_t>
+                                    expected_connection_epoch);
+    bool writeClientHeartbeatLocked_();
+    std::shared_ptr<std::mutex> axisMutex_(std::uint8_t motor_id) const;
+    std::shared_ptr<std::mutex> safetyMutex_(std::uint8_t motor_id) const;
+    void workerLoop_();
+    static std::uint32_t randomNonZeroSessionId_();
+    static std::uint32_t monotonicMilliseconds_();
+
+    std::string id_;
+    config::ModbusTcpConfig config_;
+    std::unordered_map<std::uint8_t, std::uint32_t> motor_axes_;
+    mutable std::unordered_map<std::uint8_t, std::shared_ptr<std::mutex>> axis_mutexes_;
+    mutable std::unordered_map<std::uint8_t, std::shared_ptr<std::mutex>> safety_mutexes_;
+    std::unordered_map<std::uint8_t, std::uint32_t> command_sequences_;
+    std::unordered_map<std::uint8_t, std::uint64_t> cancel_generations_;
+    // Monotonic admission counter protected by state_mutex_. Besides being
+    // useful when diagnosing queueing, it gives concurrency tests an exact
+    // synchronization point after an invocation has captured its safety
+    // generation and connection epoch, but before it waits on a command
+    // serialization mutex.
+    std::uint64_t command_admission_count_{0};
+
+    mutable std::mutex io_mutex_;
+    mutable std::mutex state_mutex_;
+    mutable std::mutex lifecycle_mutex_;
+    ModbusTcpClient client_;
+    std::string last_error_;
+    std::thread worker_;
+    std::mutex worker_wait_mutex_;
+    std::condition_variable worker_wait_cv_;
+    std::atomic<bool> running_{false};
+    std::atomic<bool> connected_{false};
+    std::atomic<std::uint32_t> plc_boot_id_{0};
+    std::atomic<std::uint64_t> connection_epoch_{0};
+    std::atomic<std::uint32_t> session_id_{0};
+    std::uint32_t last_session_id_{0};
+    std::uint32_t heartbeat_counter_{0};
+    std::chrono::steady_clock::time_point last_client_heartbeat_write_at_{};
+    std::uint32_t plc_heartbeat_counter_{0};
+    std::chrono::steady_clock::time_point plc_heartbeat_changed_at_{};
+    std::uint32_t connect_timeout_ms_{500};
+    std::uint32_t io_timeout_ms_{100};
+    std::uint32_t heartbeat_period_ms_{100};
+    std::uint32_t communication_watchdog_ms_{500};
+    std::uint32_t status_poll_period_ms_{20};
+    std::uint32_t reconnect_min_ms_{100};
+    std::uint32_t reconnect_max_ms_{2000};
+    std::uint32_t command_ack_timeout_ms_{500};
+    std::uint32_t stream_watchdog_ms_{500};
+    std::uint16_t protocol_major_{cmvr_plc::kProtocolMajor};
+    std::uint16_t protocol_minor_{cmvr_plc::kProtocolMinor};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H

cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp

@@ -0,0 +1,188 @@
+#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h"
+
+#include <cerrno>
+#include <limits>
+#include <fcntl.h>
+#include <netdb.h>
+#include <poll.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <modbus/modbus.h>
+
+namespace cmvr::device {
+
+ModbusTcpClient::~ModbusTcpClient()
+{
+    close();
+}
+
+bool ModbusTcpClient::open(const std::string& host,
+                           const std::uint16_t port,
+                           const std::uint8_t unit_id,
+                           const std::uint32_t connect_timeout_ms,
+                           const std::uint32_t response_timeout_ms)
+{
+    close();
+    context_ = modbus_new_tcp(host.c_str(), static_cast<int>(port));
+    if (!context_) {
+        last_error_ = "modbus_new_tcp failed";
+        return false;
+    }
+    if (modbus_set_slave(context_, static_cast<int>(unit_id)) == -1) {
+        setLastErrnoError_("modbus_set_slave");
+        close();
+        return false;
+    }
+
+    const auto timeout = response_timeout_ms == 0 ? 200U : response_timeout_ms;
+    if (modbus_set_response_timeout(context_, timeout / 1000U,
+                                    (timeout % 1000U) * 1000U) == -1 ||
+        modbus_set_byte_timeout(context_, timeout / 1000U,
+                                (timeout % 1000U) * 1000U) == -1) {
+        setLastErrnoError_("modbus_set_timeout");
+        close();
+        return false;
+    }
+    addrinfo hints{};
+    hints.ai_family = AF_INET;
+    hints.ai_socktype = SOCK_STREAM;
+    hints.ai_flags = AI_NUMERICHOST;
+    addrinfo* addresses = nullptr;
+    const auto service = std::to_string(port);
+    const auto resolve_result =
+        getaddrinfo(host.c_str(), service.c_str(), &hints, &addresses);
+    if (resolve_result != 0) {
+        last_error_ = std::string("host must be a numeric IPv4 address: ") +
+                      gai_strerror(resolve_result);
+        close();
+        return false;
+    }
+    int connected_fd = -1;
+    for (auto* address = addresses; address; address = address->ai_next) {
+        const int fd = ::socket(address->ai_family, address->ai_socktype, address->ai_protocol);
+        if (fd < 0) {
+            continue;
+        }
+        const int old_flags = fcntl(fd, F_GETFL, 0);
+        if (old_flags < 0 || fcntl(fd, F_SETFL, old_flags | O_NONBLOCK) < 0) {
+            ::close(fd);
+            continue;
+        }
+        socket_fd_.store(fd);
+        const int rc = ::connect(fd, address->ai_addr, address->ai_addrlen);
+        if (rc == 0 || errno == EINPROGRESS) {
+            pollfd descriptor{fd, POLLOUT, 0};
+            const auto timeout = static_cast<int>(
+                connect_timeout_ms == 0 ? 1000U : connect_timeout_ms);
+            if (rc == 0 || poll(&descriptor, 1, timeout) > 0) {
+                int socket_error = 0;
+                socklen_t error_size = sizeof(socket_error);
+                if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &socket_error, &error_size) == 0 &&
+                    socket_error == 0) {
+                    if (fcntl(fd, F_SETFL, old_flags) == 0) {
+                        connected_fd = fd;
+                        break;
+                    }
+                }
+            }
+        }
+        socket_fd_.store(-1);
+        ::close(fd);
+    }
+    freeaddrinfo(addresses);
+    if (connected_fd < 0 || modbus_set_socket(context_, connected_fd) == -1) {
+        if (connected_fd >= 0) {
+            ::close(connected_fd);
+        }
+        last_error_ = "Modbus TCP connect timed out or failed";
+        close();
+        return false;
+    }
+    socket_fd_.store(connected_fd);
+    last_error_.clear();
+    return true;
+}
+
+void ModbusTcpClient::close()
+{
+    socket_fd_.store(-1);
+    if (context_) {
+        modbus_close(context_);
+        modbus_free(context_);
+        context_ = nullptr;
+    }
+}
+
+void ModbusTcpClient::shutdown()
+{
+    const auto fd = socket_fd_.load();
+    if (fd >= 0) {
+        ::shutdown(fd, SHUT_RDWR);
+    }
+}
+
+bool ModbusTcpClient::readHoldingRegisters(
+    const int address,
+    const int count,
+    std::vector<std::uint16_t>& values)
+{
+    if (!context_) {
+        last_error_ = "Modbus TCP connection is closed";
+        return false;
+    }
+    if (address < 0 || count <= 0 || count > MODBUS_MAX_READ_REGISTERS) {
+        last_error_ = "invalid holding-register read range";
+        return false;
+    }
+
+    values.assign(static_cast<std::size_t>(count), 0);
+    const auto rc = modbus_read_registers(context_, address, count, values.data());
+    if (rc != count) {
+        setLastErrnoError_("modbus_read_registers");
+        return false;
+    }
+    last_error_.clear();
+    return true;
+}
+
+bool ModbusTcpClient::writeHoldingRegisters(
+    const int address,
+    const std::uint16_t* values,
+    const int count)
+{
+    if (!context_) {
+        last_error_ = "Modbus TCP connection is closed";
+        return false;
+    }
+    if (address < 0 || !values || count <= 0 || count > MODBUS_MAX_WRITE_REGISTERS) {
+        last_error_ = "invalid holding-register write range";
+        return false;
+    }
+
+    const auto rc = modbus_write_registers(context_, address, count, values);
+    if (rc != count) {
+        setLastErrnoError_("modbus_write_registers");
+        return false;
+    }
+    last_error_.clear();
+    return true;
+}
+
+bool ModbusTcpClient::writeHoldingRegisters(
+    const int address,
+    const std::vector<std::uint16_t>& values)
+{
+    if (values.size() > static_cast<std::size_t>(std::numeric_limits<int>::max())) {
+        last_error_ = "holding-register write is too large";
+        return false;
+    }
+    return writeHoldingRegisters(address, values.data(), static_cast<int>(values.size()));
+}
+
+void ModbusTcpClient::setLastErrnoError_(const char* operation)
+{
+    last_error_ = std::string(operation) + ": " + modbus_strerror(errno);
+}
+
+} // namespace cmvr::device

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

@@ -0,0 +1,21 @@
+add_library(modbus_plc_motor_driver SHARED
+        src/cmvr_plc_motor_protocol.cpp
+        src/modbus_plc_motor.cpp
+)
+
+target_include_directories(modbus_plc_motor_driver
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+target_link_libraries(modbus_plc_motor_driver
+        PUBLIC
+        cmvr_es::device::motor_core
+        cmvr_es::device::motor_bus_runtime
+        PRIVATE
+        cmvr_es::proto
+        glog
+)
+
+add_library(cmvr_es::device::modbus_plc_motor_driver ALIAS modbus_plc_motor_driver)
+install(TARGETS modbus_plc_motor_driver LIBRARY DESTINATION lib)

cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h

@@ -0,0 +1,89 @@
+#ifndef CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H
+#define CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H
+
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <unordered_map>
+
+#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h"
+#include "devices/motor/motor_protocol_interface.h"
+
+namespace cmvr::device {
+
+class CmvrPlcMotorProtocol final : public MotorProtocolInterface {
+public:
+    explicit CmvrPlcMotorProtocol(std::shared_ptr<ModbusTcpMotorBusRuntime> bus_runtime);
+    ~CmvrPlcMotorProtocol() override = default;
+
+    bool initNode(std::uint8_t node_id) override;
+    void 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 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;
+
+private:
+    struct NodeState {
+        msgs::RunMode requested_mode{msgs::RUN_MODE_UNSPECIFIED};
+        double limit_q_lb{0.0};
+        double limit_q_ub{0.0};
+        double limit_qd{0.0};
+        double limit_qdd{0.0};
+        bool cyclic_stream_open{false};
+        bool cyclic_reconnect_latched{false};
+        std::uint32_t cyclic_sequence{0};
+        std::uint64_t cyclic_generation{0};
+        std::uint64_t cyclic_connection_epoch{0};
+        bool profile_feedback_bound{false};
+        std::uint64_t profile_connection_epoch{0};
+    };
+
+    bool submitSimple_(std::uint8_t node_id,
+                       cmvr_plc::CommandCode code,
+                       bool wait_for_terminal,
+                       std::uint32_t timeout_ms);
+    bool ensureCyclicOpen_(std::uint8_t node_id,
+                           NodeState& state,
+                           msgs::RunMode mode);
+    static std::optional<std::int32_t> toMicroUnits_(double value);
+    static double fromMicroUnits_(std::int32_t value);
+    static bool isSupportedMode_(msgs::RunMode mode);
+    bool validatePosition_(const NodeState& state, double position) const;
+    bool validateVelocity_(const NodeState& state, double velocity) const;
+    bool validateAcceleration_(const NodeState& state, double acceleration) const;
+    bool readMotionStatus_(std::uint8_t node_id, CmvrPlcAxisStatus& status);
+    NodeState& nodeStateLocked_(std::uint8_t node_id);
+
+    std::shared_ptr<ModbusTcpMotorBusRuntime> bus_runtime_;
+    std::mutex nodes_mutex_;
+    std::unordered_map<std::uint8_t, NodeState> nodes_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H

cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h

@@ -0,0 +1,21 @@
+#ifndef CMVR_ES_MODBUS_PLC_MOTOR_H
+#define CMVR_ES_MODBUS_PLC_MOTOR_H
+
+#include "cmvr/config/motor_config/motor_config.pb.h"
+#include "devices/motor/abstract_motor.h"
+
+namespace cmvr::device {
+
+class ModbusPlcMotor final : public AbstractMotor {
+public:
+    explicit ModbusPlcMotor(const config::MotorConfigItem& config);
+
+    std::string typeName() const override { return "ModbusPlcMotor"; }
+    bool init() override;
+    bool torqueOff() override;
+    bool quickStop() override;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_MODBUS_PLC_MOTOR_H

cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp

@@ -0,0 +1,582 @@
+#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+namespace {
+
+// MotorService accepts waits up to 10 minutes. The PLC timeout is a ceiling;
+// shorter RPC timeouts still actively QuickStop from the service layer.
+constexpr std::uint32_t kProfileCommandTimeoutCeilingMs = 600000;
+constexpr std::uint32_t kSafetyCommandTimeoutMs = 5000;
+
+bool statusAllowsMotionFeedback(const CmvrPlcAxisStatus& status)
+{
+    constexpr std::uint16_t kFatalFlags =
+        cmvr_plc::StatusFlag::Fault |
+        cmvr_plc::StatusFlag::CommunicationWatchdogExpired |
+        cmvr_plc::StatusFlag::CyclicWatchdogExpired;
+    return (status.status_flags & kFatalFlags) == 0U &&
+           status.result_code == cmvr_plc::ResultCode::Ok &&
+           static_cast<std::uint16_t>(status.command_state) <=
+               static_cast<std::uint16_t>(
+                   cmvr_plc::CommandState::CommunicationLost) &&
+           !cmvr_plc::isFailure(status.command_state);
+}
+
+} // namespace
+
+CmvrPlcMotorProtocol::CmvrPlcMotorProtocol(
+    std::shared_ptr<ModbusTcpMotorBusRuntime> bus_runtime)
+    : bus_runtime_(std::move(bus_runtime))
+{
+    comm_proto = CommProto::CUSTOM;
+}
+
+bool CmvrPlcMotorProtocol::initNode(const std::uint8_t node_id)
+{
+    if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) {
+        CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] missing PLC axis mapping for motor "
+                        << static_cast<int>(node_id);
+        return false;
+    }
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    auto& state = nodeStateLocked_(node_id);
+    state.cyclic_connection_epoch = bus_runtime_->connectionEpoch();
+    state.cyclic_stream_open = false;
+    state.cyclic_reconnect_latched = false;
+    state.cyclic_sequence = 0;
+    state.cyclic_generation = 0;
+    state.profile_feedback_bound = false;
+    state.profile_connection_epoch = 0;
+    return true;
+}
+
+void CmvrPlcMotorProtocol::setMode(
+    const std::uint8_t node_id,
+    const msgs::RunMode mode)
+{
+    if (!isSupportedMode_(mode)) {
+        CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] unsupported mode "
+                        << msgs::RunMode_Name(mode);
+        return;
+    }
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    auto& state = nodeStateLocked_(node_id);
+    const bool cyclic_mode =
+        mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION ||
+        mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
+    if (cyclic_mode) {
+        // Every explicit cyclic setMode call is a new upper-layer stream
+        // generation, even when the mode value itself is unchanged. This is
+        // the only transition allowed to clear a reconnect latch.
+        state.requested_mode = mode;
+        state.cyclic_connection_epoch =
+            bus_runtime_ ? bus_runtime_->connectionEpoch() : 0U;
+        state.cyclic_stream_open = false;
+        state.cyclic_reconnect_latched = false;
+        state.cyclic_sequence = 0;
+        if (++state.cyclic_generation == 0U) {
+            ++state.cyclic_generation;
+        }
+        return;
+    }
+    if (state.requested_mode != mode) {
+        state.cyclic_stream_open = false;
+    }
+    state.requested_mode = mode;
+}
+
+msgs::RunMode CmvrPlcMotorProtocol::getMode(const std::uint8_t node_id)
+{
+    CmvrPlcAxisStatus status;
+    if (bus_runtime_ && bus_runtime_->readAxisStatus(node_id, status)) {
+        const auto mode = static_cast<msgs::RunMode>(status.current_mode);
+        if (isSupportedMode_(mode)) {
+            return mode;
+        }
+    }
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    return nodeStateLocked_(node_id).requested_mode;
+}
+
+void CmvrPlcMotorProtocol::setLimitQdd(
+    const std::uint8_t node_id,
+    const double u_qdd,
+    const double l_qdd)
+{
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    nodeStateLocked_(node_id).limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd));
+}
+
+void CmvrPlcMotorProtocol::setLimitQd(
+    const std::uint8_t node_id,
+    const double qd)
+{
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    nodeStateLocked_(node_id).limit_qd = std::abs(qd);
+}
+
+void CmvrPlcMotorProtocol::setLimitQ(
+    const std::uint8_t node_id,
+    const double ub,
+    const double lb)
+{
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    auto& state = nodeStateLocked_(node_id);
+    state.limit_q_ub = ub;
+    state.limit_q_lb = lb;
+}
+
+bool CmvrPlcMotorProtocol::calibrateZeroQ(const std::uint8_t node_id)
+{
+    return submitSimple_(node_id, cmvr_plc::CommandCode::SetZero, true,
+                         kSafetyCommandTimeoutMs);
+}
+
+bool CmvrPlcMotorProtocol::reachedTargetQ(const std::uint8_t node_id)
+{
+    CmvrPlcAxisStatus status;
+    return readMotionStatus_(node_id, status) &&
+           statusAllowsMotionFeedback(status) &&
+           (status.status_flags & cmvr_plc::StatusFlag::TargetReached) != 0U;
+}
+
+bool CmvrPlcMotorProtocol::commandProfilePosition(
+    const std::uint8_t node_id,
+    const double target_q,
+    const double max_qd,
+    const double max_qdd)
+{
+    NodeState state;
+    {
+        std::lock_guard<std::mutex> lock(nodes_mutex_);
+        auto& stored = nodeStateLocked_(node_id);
+        stored.requested_mode = msgs::RUN_MODE_PROFILE_POSITION;
+        stored.cyclic_stream_open = false;
+        state = stored;
+    }
+    if (!validatePosition_(state, target_q) ||
+        !validateVelocity_(state, max_qd) ||
+        !validateAcceleration_(state, max_qdd)) {
+        CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-position command";
+        return false;
+    }
+    const auto position = toMicroUnits_(target_q);
+    const auto velocity = toMicroUnits_(std::abs(max_qd));
+    const auto acceleration = toMicroUnits_(std::abs(max_qdd));
+    if (!position || !velocity || !acceleration) {
+        return false;
+    }
+
+    CmvrPlcAxisCommand command;
+    command.code = cmvr_plc::CommandCode::ProfilePosition;
+    command.target_position = *position;
+    command.target_velocity = *velocity;
+    command.acceleration = *acceleration;
+    command.command_timeout_ms = kProfileCommandTimeoutCeilingMs;
+    if (!bus_runtime_) {
+        return false;
+    }
+    const auto submission_epoch = bus_runtime_->connectionEpoch();
+    const auto submitted =
+        bus_runtime_->submitAxisCommand(
+            node_id, command, false, submission_epoch);
+    const auto completed_epoch = bus_runtime_->connectionEpoch();
+    if (!submitted && completed_epoch == submission_epoch) {
+        return false;
+    }
+    {
+        std::lock_guard<std::mutex> lock(nodes_mutex_);
+        auto& stored = nodeStateLocked_(node_id);
+        // Bind on every cross-epoch outcome, including an ambiguous failed
+        // submit, so feedback remains invalid until safety cleanup.
+        stored.profile_feedback_bound = true;
+        stored.profile_connection_epoch = submission_epoch;
+    }
+    return submitted && completed_epoch == submission_epoch;
+}
+
+bool CmvrPlcMotorProtocol::commandProfileVelocity(
+    const std::uint8_t node_id,
+    const double target_qd,
+    const double max_qdd)
+{
+    NodeState state;
+    {
+        std::lock_guard<std::mutex> lock(nodes_mutex_);
+        auto& stored = nodeStateLocked_(node_id);
+        stored.requested_mode = msgs::RUN_MODE_PROFILE_VELOCITY;
+        stored.cyclic_stream_open = false;
+        state = stored;
+    }
+    if (!validateVelocity_(state, target_qd) ||
+        !validateAcceleration_(state, max_qdd)) {
+        CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-velocity command";
+        return false;
+    }
+    const auto velocity = toMicroUnits_(target_qd);
+    const auto acceleration = toMicroUnits_(std::abs(max_qdd));
+    if (!velocity || !acceleration) {
+        return false;
+    }
+
+    CmvrPlcAxisCommand command;
+    command.code = cmvr_plc::CommandCode::ProfileVelocity;
+    command.target_velocity = *velocity;
+    command.acceleration = *acceleration;
+    command.command_timeout_ms = kProfileCommandTimeoutCeilingMs;
+    if (!bus_runtime_) {
+        return false;
+    }
+    const auto submission_epoch = bus_runtime_->connectionEpoch();
+    const auto submitted =
+        bus_runtime_->submitAxisCommand(
+            node_id, command, false, submission_epoch);
+    const auto completed_epoch = bus_runtime_->connectionEpoch();
+    if (!submitted && completed_epoch == submission_epoch) {
+        return false;
+    }
+    {
+        std::lock_guard<std::mutex> lock(nodes_mutex_);
+        auto& stored = nodeStateLocked_(node_id);
+        stored.profile_feedback_bound = true;
+        stored.profile_connection_epoch = submission_epoch;
+    }
+    return submitted && completed_epoch == submission_epoch;
+}
+
+bool CmvrPlcMotorProtocol::commandCyclicPosition(
+    const std::uint8_t node_id,
+    const double target_q,
+    const double target_qd)
+{
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    auto& state = nodeStateLocked_(node_id);
+    if (!validatePosition_(state, target_q) ||
+        !validateVelocity_(state, target_qd) ||
+        !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+        return false;
+    }
+    const auto position = toMicroUnits_(target_q);
+    const auto velocity = toMicroUnits_(target_qd);
+    if (!position || !velocity) {
+        return false;
+    }
+
+    CmvrPlcAxisCommand command;
+    command.code = cmvr_plc::CommandCode::CyclicPositionSample;
+    command.target_position = *position;
+    command.target_velocity = *velocity;
+    command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs();
+    if (++state.cyclic_sequence == 0) {
+        ++state.cyclic_sequence;
+    }
+    command.cyclic_sample_sequence = state.cyclic_sequence;
+    const auto submitted =
+        bus_runtime_->submitAxisCommand(
+            node_id, command, false, state.cyclic_connection_epoch);
+    if (submitted) {
+        state.profile_feedback_bound = false;
+        state.profile_connection_epoch = 0;
+    } else if (
+        bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) {
+        state.cyclic_reconnect_latched = true;
+        state.cyclic_stream_open = false;
+        state.cyclic_sequence = 0;
+        state.cyclic_connection_epoch = bus_runtime_->connectionEpoch();
+    }
+    return submitted;
+}
+
+bool CmvrPlcMotorProtocol::commandCyclicVelocity(
+    const std::uint8_t node_id,
+    const double target_qd)
+{
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    auto& state = nodeStateLocked_(node_id);
+    if (!validateVelocity_(state, target_qd) ||
+        !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) {
+        return false;
+    }
+    const auto velocity = toMicroUnits_(target_qd);
+    if (!velocity) {
+        return false;
+    }
+
+    CmvrPlcAxisCommand command;
+    command.code = cmvr_plc::CommandCode::CyclicVelocitySample;
+    command.target_velocity = *velocity;
+    command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs();
+    if (++state.cyclic_sequence == 0) {
+        ++state.cyclic_sequence;
+    }
+    command.cyclic_sample_sequence = state.cyclic_sequence;
+    const auto submitted =
+        bus_runtime_->submitAxisCommand(
+            node_id, command, false, state.cyclic_connection_epoch);
+    if (submitted) {
+        state.profile_feedback_bound = false;
+        state.profile_connection_epoch = 0;
+    } else if (
+        bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) {
+        state.cyclic_reconnect_latched = true;
+        state.cyclic_stream_open = false;
+        state.cyclic_sequence = 0;
+        state.cyclic_connection_epoch = bus_runtime_->connectionEpoch();
+    }
+    return submitted;
+}
+
+bool CmvrPlcMotorProtocol::commandCyclicTorque(
+    const std::uint8_t node_id,
+    const double target_tau)
+{
+    (void)target_tau;
+    CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] cyclic torque is unsupported, motor="
+                    << static_cast<int>(node_id);
+    return false;
+}
+
+void CmvrPlcMotorProtocol::setMotorConversion(
+    const std::uint8_t node_id,
+    const double encoder_counts_per_rev,
+    const double gear_ratio)
+{
+    (void)node_id;
+    (void)encoder_counts_per_rev;
+    (void)gear_ratio;
+    // CMVR PLC v1 exchanges SI quantities in fixed-point micro-units.
+}
+
+bool CmvrPlcMotorProtocol::torqueOn(const std::uint8_t node_id)
+{
+    return submitSimple_(node_id, cmvr_plc::CommandCode::Enable, true,
+                         kSafetyCommandTimeoutMs);
+}
+
+bool CmvrPlcMotorProtocol::torqueOff(const std::uint8_t node_id)
+{
+    CmvrPlcAxisCommand command;
+    command.code = cmvr_plc::CommandCode::Disable;
+    command.command_timeout_ms = kSafetyCommandTimeoutMs;
+    const auto result =
+        bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command);
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    auto& state = nodeStateLocked_(node_id);
+    state.cyclic_stream_open = false;
+    if (result) {
+        state.profile_feedback_bound = false;
+        state.profile_connection_epoch = 0;
+    }
+    return result;
+}
+
+bool CmvrPlcMotorProtocol::brakeRelease(const std::uint8_t node_id)
+{
+    CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] brake release is unsupported, motor="
+                    << static_cast<int>(node_id);
+    return false;
+}
+
+bool CmvrPlcMotorProtocol::quickStop(const std::uint8_t node_id)
+{
+    CmvrPlcAxisCommand command;
+    command.code = cmvr_plc::CommandCode::QuickStop;
+    command.command_timeout_ms = kSafetyCommandTimeoutMs;
+    const auto result =
+        bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command);
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    auto& state = nodeStateLocked_(node_id);
+    state.cyclic_stream_open = false;
+    if (result) {
+        state.profile_feedback_bound = false;
+        state.profile_connection_epoch = 0;
+    }
+    return result;
+}
+
+double CmvrPlcMotorProtocol::getQ(const std::uint8_t node_id)
+{
+    CmvrPlcAxisStatus status;
+    if (!readMotionStatus_(node_id, status) ||
+        !statusAllowsMotionFeedback(status)) {
+        return std::numeric_limits<double>::quiet_NaN();
+    }
+    return fromMicroUnits_(status.actual_position);
+}
+
+double CmvrPlcMotorProtocol::getQd(const std::uint8_t node_id)
+{
+    CmvrPlcAxisStatus status;
+    if (!readMotionStatus_(node_id, status) ||
+        !statusAllowsMotionFeedback(status)) {
+        return std::numeric_limits<double>::quiet_NaN();
+    }
+    return fromMicroUnits_(status.actual_velocity);
+}
+
+bool CmvrPlcMotorProtocol::readMotionStatus_(
+    const std::uint8_t node_id,
+    CmvrPlcAxisStatus& status)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    std::lock_guard<std::mutex> lock(nodes_mutex_);
+    const auto& state = nodeStateLocked_(node_id);
+    const auto read_epoch = bus_runtime_->connectionEpoch();
+    if (state.profile_feedback_bound &&
+        state.profile_connection_epoch != read_epoch) {
+        return false;
+    }
+    if (!bus_runtime_->readAxisStatus(node_id, status)) {
+        return false;
+    }
+    // Reject a status transaction that straddled a disconnect/reconnect even
+    // when no profile binding was active at the first check.
+    if (bus_runtime_->connectionEpoch() != read_epoch) {
+        return false;
+    }
+    return !state.profile_feedback_bound ||
+           state.profile_connection_epoch == read_epoch;
+}
+
+bool CmvrPlcMotorProtocol::submitSimple_(
+    const std::uint8_t node_id,
+    const cmvr_plc::CommandCode code,
+    const bool wait_for_terminal,
+    const std::uint32_t timeout_ms)
+{
+    CmvrPlcAxisCommand command;
+    command.code = code;
+    command.command_timeout_ms = timeout_ms;
+    return bus_runtime_ &&
+           bus_runtime_->submitAxisCommand(node_id, command, wait_for_terminal);
+}
+
+bool CmvrPlcMotorProtocol::ensureCyclicOpen_(
+    const std::uint8_t node_id,
+    NodeState& state,
+    const msgs::RunMode mode)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    const auto connection_epoch = bus_runtime_->connectionEpoch();
+    if (state.cyclic_connection_epoch != connection_epoch) {
+        // A cyclic generation is bound to the PLC ownership epoch in which it
+        // was created. Never reinterpret a setpoint from that generation as
+        // the first setpoint of a freshly reconnected PLC stream.
+        if (state.cyclic_generation != 0U) {
+            state.cyclic_reconnect_latched = true;
+        }
+        state.cyclic_connection_epoch = connection_epoch;
+        state.cyclic_stream_open = false;
+        state.cyclic_sequence = 0;
+    }
+    if (state.cyclic_reconnect_latched) {
+        CMVR_LOG(WARNING)
+            << "[CmvrPlcMotorProtocol] cyclic stream crossed a PLC session; "
+               "explicit setMode from a new upper-layer stream is required, motor="
+            << static_cast<int>(node_id);
+        return false;
+    }
+    if (state.cyclic_stream_open && state.requested_mode == mode) {
+        return true;
+    }
+
+    // Preserve direct protocol use that does not call setMode explicitly:
+    // its first successful Open still establishes a generation. Once that
+    // generation has observed a reconnect, only explicit setMode can recover.
+    if (state.cyclic_generation == 0U) {
+        state.cyclic_generation = 1U;
+        state.cyclic_connection_epoch = connection_epoch;
+    }
+
+    CmvrPlcAxisCommand command;
+    command.code = mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION
+        ? cmvr_plc::CommandCode::OpenCyclicPosition
+        : cmvr_plc::CommandCode::OpenCyclicVelocity;
+    command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs();
+    command.command_timeout_ms = kSafetyCommandTimeoutMs;
+    if (!bus_runtime_->submitAxisCommand(
+            node_id, command, false, state.cyclic_connection_epoch)) {
+        if (bus_runtime_->connectionEpoch() !=
+            state.cyclic_connection_epoch) {
+            state.cyclic_reconnect_latched = true;
+            state.cyclic_stream_open = false;
+            state.cyclic_sequence = 0;
+            state.cyclic_connection_epoch =
+                bus_runtime_->connectionEpoch();
+        }
+        return false;
+    }
+    state.requested_mode = mode;
+    state.cyclic_stream_open = true;
+    state.cyclic_sequence = 0;
+    return true;
+}
+
+std::optional<std::int32_t> CmvrPlcMotorProtocol::toMicroUnits_(const double value)
+{
+    if (!std::isfinite(value)) {
+        return std::nullopt;
+    }
+    const auto scaled = std::round(value * cmvr_plc::kPositionScale);
+    if (scaled < static_cast<double>(std::numeric_limits<std::int32_t>::min()) ||
+        scaled > static_cast<double>(std::numeric_limits<std::int32_t>::max())) {
+        return std::nullopt;
+    }
+    return static_cast<std::int32_t>(scaled);
+}
+
+double CmvrPlcMotorProtocol::fromMicroUnits_(const std::int32_t value)
+{
+    return static_cast<double>(value) / cmvr_plc::kPositionScale;
+}
+
+bool CmvrPlcMotorProtocol::isSupportedMode_(const msgs::RunMode mode)
+{
+    return mode == msgs::RUN_MODE_PROFILE_POSITION ||
+           mode == msgs::RUN_MODE_PROFILE_VELOCITY ||
+           mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION ||
+           mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
+}
+
+bool CmvrPlcMotorProtocol::validatePosition_(
+    const NodeState& state,
+    const double position) const
+{
+    return std::isfinite(position) &&
+           (state.limit_q_ub <= state.limit_q_lb ||
+            (position >= state.limit_q_lb && position <= state.limit_q_ub));
+}
+
+bool CmvrPlcMotorProtocol::validateVelocity_(
+    const NodeState& state,
+    const double velocity) const
+{
+    return std::isfinite(velocity) &&
+           (state.limit_qd <= 0.0 || std::abs(velocity) <= state.limit_qd);
+}
+
+bool CmvrPlcMotorProtocol::validateAcceleration_(
+    const NodeState& state,
+    const double acceleration) const
+{
+    return std::isfinite(acceleration) && acceleration >= 0.0 &&
+           (state.limit_qdd <= 0.0 || acceleration <= state.limit_qdd);
+}
+
+CmvrPlcMotorProtocol::NodeState& CmvrPlcMotorProtocol::nodeStateLocked_(
+    const std::uint8_t node_id)
+{
+    return nodes_[node_id];
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp

@@ -0,0 +1,55 @@
+#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h"
+
+#include "common/base/logging/logger.h"
+#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
+#include <cmath>
+
+namespace cmvr::device {
+
+ModbusPlcMotor::ModbusPlcMotor(const config::MotorConfigItem& config)
+{
+    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();
+    node_id_ = static_cast<std::uint8_t>(config.id());
+}
+
+bool ModbusPlcMotor::init()
+{
+    if (!protocol_ ||
+        !std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(protocol_)) {
+        CMVR_LOG(ERROR) << "[ModbusPlcMotor] invalid CMVR PLC protocol for "
+                        << info_.joint_name;
+        return false;
+    }
+    if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) ||
+        !std::isfinite(info_.limit_qd) || !std::isfinite(info_.limit_qdd) ||
+        info_.limit_q_ub <= info_.limit_q_lb ||
+        info_.limit_qd <= 0.0 || info_.limit_qdd <= 0.0) {
+        CMVR_LOG(ERROR) << "[ModbusPlcMotor] finite position/velocity/acceleration "
+                           "limits are required for "
+                        << info_.joint_name;
+        return false;
+    }
+    setLimitQ(info_.limit_q_ub, info_.limit_q_lb);
+    setLimitQd(info_.limit_qd);
+    setLimitQdd(info_.limit_qdd, -info_.limit_qdd);
+    return true;
+}
+
+bool ModbusPlcMotor::torqueOff()
+{
+    auto protocol = std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(protocol_);
+    return protocol && protocol->torqueOff(node_id_);
+}
+
+bool ModbusPlcMotor::quickStop()
+{
+    auto protocol = std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(protocol_);
+    return protocol && protocol->quickStop(node_id_);
+}
+
+} // namespace cmvr::device

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

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

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

@@ -78,6 +78,10 @@ private:
         const config::MotorGroupConfig& group_cfg,
         const std::vector<config::MotorConfigItem>& motor_cfgs,
         const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
+    std::vector<std::shared_ptr<AbstractMotor>> createModbusTcpMotors_(
+        const config::MotorGroupConfig& group_cfg,
+        const std::vector<config::MotorConfigItem>& motor_cfgs,
+        const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
 
 private:
     config::MotorConfig cfg_;

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

@@ -15,11 +15,14 @@
 #include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
 #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
 #include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.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"
 #include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
+#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
+#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h"
 #include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
 #include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 
@@ -415,6 +418,16 @@ std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
             return std::make_shared<CanMotorBusRuntime>();
         case config::MOTOR_BUS_MUJOCO:
             return std::make_shared<MujocoMotorBusRuntime>();
+        case config::MOTOR_BUS_MODBUS_TCP:
+            if (group_cfg.vendor() == config::MOTOR_VENDOR_PLC_GENERIC &&
+                group_cfg.protocol() == config::MOTOR_PROTOCOL_CMVR_PLC_V1) {
+                return std::make_shared<ModbusTcpMotorBusRuntime>();
+            }
+            CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP motor: vendor="
+                            << config::MotorVendor_Name(group_cfg.vendor())
+                            << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+                            << ", group=" << group_cfg.id();
+            return nullptr;
         case config::MOTOR_BUS_ETHERCAT: {
             auto runtime = std::make_shared<EthercatMotorBusRuntime>();
             if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
@@ -454,6 +467,8 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createMotors_(
             return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime);
         case config::MOTOR_BUS_ETHERCAT:
             return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime);
+        case config::MOTOR_BUS_MODBUS_TCP:
+            return createModbusTcpMotors_(group_cfg, motor_cfgs, bus_runtime);
         default:
             CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
                             << config::MotorBusType_Name(group_cfg.bus_type())
@@ -596,4 +611,50 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
     return motors;
 }
 
+std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createModbusTcpMotors_(
+    const config::MotorGroupConfig& group_cfg,
+    const std::vector<config::MotorConfigItem>& motor_cfgs,
+    const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const
+{
+    auto modbus_runtime =
+        std::dynamic_pointer_cast<ModbusTcpMotorBusRuntime>(bus_runtime);
+    if (!modbus_runtime || !group_cfg.has_modbus_tcp()) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing Modbus TCP runtime/config: "
+                        << group_cfg.id();
+        return {};
+    }
+    if (group_cfg.vendor() != config::MOTOR_VENDOR_PLC_GENERIC ||
+        group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) {
+        CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP PLC motor: vendor="
+                        << config::MotorVendor_Name(group_cfg.vendor())
+                        << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol());
+        return {};
+    }
+    for (const auto& motor_cfg : motor_cfgs) {
+        if (motor_cfg.id() < 0 || motor_cfg.id() > 255 ||
+            !modbus_runtime->hasMotor(static_cast<std::uint8_t>(motor_cfg.id()))) {
+            CMVR_LOG(ERROR) << "[MotorManager] missing PLC axis mapping for motor id "
+                            << motor_cfg.id() << " in group: " << group_cfg.id();
+            return {};
+        }
+    }
+
+    auto protocol = std::make_shared<CmvrPlcMotorProtocol>(modbus_runtime);
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(motor_cfgs.size());
+    for (const auto& cfg : motor_cfgs) {
+        auto motor = std::make_shared<ModbusPlcMotor>(cfg);
+        motor->setProtocol(protocol);
+        // initNode() and ModbusPlcMotor::init() only establish local mappings and
+        // limits. The shared TCP connection starts after all motors are created.
+        if (!motor->init()) {
+            CMVR_LOG(ERROR) << "[MotorManager] failed to init Modbus PLC motor: "
+                            << cfg.joint_name();
+            return {};
+        }
+        motors.push_back(std::move(motor));
+    }
+    return motors;
+}
+
 } // namespace cmvr::device

cmvr-es/main.cpp

@@ -1,5 +1,9 @@
 #include <csignal>
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
 #include <pthread.h>
+#include <string>
 
 #include "common/base/logging/logger.h"
 #include "runtime/include/cmvr_runtime.h"
@@ -14,20 +18,91 @@ bool blockShutdownSignals(sigset_t& shutdown_signals)
     return pthread_sigmask(SIG_BLOCK, &shutdown_signals, nullptr) == 0;
 }
 
+struct CommandLineOptions {
+    std::string config_path;
+    double control_period_s{0.001};
+    bool show_help{false};
+};
+
+void printUsage(const char* program)
+{
+    std::cout
+        << "Usage: " << program
+        << " [--config PATH] [--control-period-s SECONDS]\n"
+        << "\n"
+        << "With no --config argument, cmvr_es loads config/cmvr_es.pb.txt "
+           "beside the executable.\n";
+}
+
+bool parseCommandLine(
+    const int argc,
+    char* argv[],
+    CommandLineOptions& options)
+{
+    for (int index = 1; index < argc; ++index) {
+        const std::string argument = argv[index];
+        if (argument == "--help" || argument == "-h") {
+            options.show_help = true;
+            return true;
+        }
+        if (argument == "--config") {
+            if (++index >= argc || argv[index][0] == '\0') {
+                std::cerr << "--config requires a path\n";
+                return false;
+            }
+            options.config_path = argv[index];
+            continue;
+        }
+        if (argument == "--control-period-s") {
+            if (++index >= argc) {
+                std::cerr
+                    << "--control-period-s requires a numeric value\n";
+                return false;
+            }
+            char* end = nullptr;
+            const double value = std::strtod(argv[index], &end);
+            if (!end || *end != '\0' || !std::isfinite(value) ||
+                value <= 0.0 || value > 1.0) {
+                std::cerr
+                    << "--control-period-s must be in (0, 1]\n";
+                return false;
+            }
+            options.control_period_s = value;
+            continue;
+        }
+        std::cerr << "unknown argument: " << argument << '\n';
+        return false;
+    }
+    return true;
+}
+
 } // namespace
 
-int main()
+int main(int argc, char* argv[])
 {
+    CommandLineOptions options;
+    if (!parseCommandLine(argc, argv, options)) {
+        printUsage(argv[0]);
+        return 2;
+    }
+    if (options.show_help) {
+        printUsage(argv[0]);
+        return 0;
+    }
+
     sigset_t shutdown_signals;
     if (!blockShutdownSignals(shutdown_signals)) {
         return 1;
     }
 
     cmvr::Runtime runtime;
-    if (!runtime.init()) {
+    const bool initialized = options.config_path.empty()
+                                 ? runtime.init()
+                                 : runtime.init(options.config_path);
+    if (!initialized) {
         return 1;
     }
-    if (!runtime.startTasks()) {
+    if (!runtime.startTasks(options.control_period_s)) {
         return 1;
     }
 

cmvr-es/manager/control_authority/CMakeLists.txt

@@ -0,0 +1,34 @@
+add_library(control_authority STATIC
+        src/control_authority_manager.cpp
+)
+target_compile_features(control_authority PUBLIC cxx_std_17)
+target_include_directories(control_authority
+        PUBLIC
+        ${PROJECT_SOURCE_DIR}/cmvr-es
+)
+
+add_library(
+        cmvr_es::control_authority
+        ALIAS control_authority
+)
+install(TARGETS control_authority ARCHIVE DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(control_authority_manager_test
+            tests/control_authority_manager_test.cpp
+    )
+    target_link_libraries(control_authority_manager_test
+            PRIVATE
+            cmvr_es::control_authority
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME control_authority_manager_test
+            COMMAND control_authority_manager_test
+    )
+    set_tests_properties(control_authority_manager_test PROPERTIES
+            TIMEOUT 10
+    )
+endif()

cmvr-es/manager/control_authority/include/control_authority_manager.h

@@ -0,0 +1,74 @@
+#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
+#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
+
+#include <chrono>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+
+namespace cmvr::control {
+
+struct ControlLeaseToken {
+    std::string resource_id;
+    std::string owner_id;
+    std::uint64_t generation{0};
+
+    bool valid() const noexcept
+    {
+        return !resource_id.empty() &&
+               !owner_id.empty() &&
+               generation != 0U;
+    }
+};
+
+struct ControlAcquireResult {
+    bool acquired{false};
+    ControlLeaseToken token;
+    std::string detail;
+};
+
+// Process-wide, transport-independent control ownership. The generation in a
+// token prevents a delayed release from an old network session from releasing
+// a newer lease on the same arm.
+class ControlAuthorityManager {
+public:
+    using Duration = std::chrono::milliseconds;
+
+    static ControlAuthorityManager& instance();
+
+    ControlAcquireResult tryAcquire(
+        const std::string& resource_id,
+        const std::string& owner_id,
+        Duration ttl);
+    bool renew(const ControlLeaseToken& token, Duration ttl);
+    bool validate(const ControlLeaseToken& token);
+    void release(const ControlLeaseToken& token) noexcept;
+
+    // Safety/control paths which do not possess a lease use this query to
+    // reject mutating commands. Read-only state and stop/torque-off commands
+    // are intentionally allowed by their callers.
+    bool isLeased(const std::string& resource_id);
+    void revoke(const std::string& resource_id) noexcept;
+
+    // Test/process teardown hook. Runtime code should release/revoke exact
+    // resources instead of clearing unrelated ownership.
+    void clear() noexcept;
+
+private:
+    struct Entry {
+        std::string owner_id;
+        std::uint64_t generation{0};
+        std::chrono::steady_clock::time_point deadline;
+    };
+
+    bool expired_(const Entry& entry) const noexcept;
+
+    std::mutex mutex_;
+    std::unordered_map<std::string, Entry> entries_;
+    std::uint64_t next_generation_{0};
+};
+
+} // namespace cmvr::control
+
+#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H

cmvr-es/manager/control_authority/src/control_authority_manager.cpp

@@ -0,0 +1,152 @@
+#include "manager/control_authority/include/control_authority_manager.h"
+
+#include <utility>
+
+namespace cmvr::control {
+
+ControlAuthorityManager& ControlAuthorityManager::instance()
+{
+    static ControlAuthorityManager manager;
+    return manager;
+}
+
+ControlAcquireResult ControlAuthorityManager::tryAcquire(
+    const std::string& resource_id,
+    const std::string& owner_id,
+    const Duration ttl)
+{
+    if (resource_id.empty() || owner_id.empty() ||
+        ttl <= Duration::zero()) {
+        return {false, {}, "invalid control lease request"};
+    }
+
+    std::lock_guard lock(mutex_);
+    const auto existing = entries_.find(resource_id);
+    if (existing != entries_.end()) {
+        if (!expired_(existing->second)) {
+            return {
+                false,
+                {},
+                "control resource is already leased by " +
+                    existing->second.owner_id};
+        }
+        entries_.erase(existing);
+    }
+
+    ControlLeaseToken token;
+    token.resource_id = resource_id;
+    token.owner_id = owner_id;
+    token.generation = ++next_generation_;
+    entries_.emplace(
+        resource_id,
+        Entry{
+            owner_id,
+            token.generation,
+            std::chrono::steady_clock::now() + ttl});
+    return {true, std::move(token), {}};
+}
+
+bool ControlAuthorityManager::renew(
+    const ControlLeaseToken& token,
+    const Duration ttl)
+{
+    if (!token.valid() || ttl <= Duration::zero()) {
+        return false;
+    }
+    std::lock_guard lock(mutex_);
+    const auto found = entries_.find(token.resource_id);
+    if (found == entries_.end() ||
+        expired_(found->second) ||
+        found->second.owner_id != token.owner_id ||
+        found->second.generation != token.generation) {
+        if (found != entries_.end() && expired_(found->second)) {
+            entries_.erase(found);
+        }
+        return false;
+    }
+    found->second.deadline =
+        std::chrono::steady_clock::now() + ttl;
+    return true;
+}
+
+bool ControlAuthorityManager::validate(
+    const ControlLeaseToken& token)
+{
+    if (!token.valid()) {
+        return false;
+    }
+    std::lock_guard lock(mutex_);
+    const auto found = entries_.find(token.resource_id);
+    if (found == entries_.end()) {
+        return false;
+    }
+    if (expired_(found->second)) {
+        entries_.erase(found);
+        return false;
+    }
+    return found->second.owner_id == token.owner_id &&
+           found->second.generation == token.generation;
+}
+
+void ControlAuthorityManager::release(
+    const ControlLeaseToken& token) noexcept
+{
+    if (!token.valid()) {
+        return;
+    }
+    try {
+        std::lock_guard lock(mutex_);
+        const auto found = entries_.find(token.resource_id);
+        if (found != entries_.end() &&
+            found->second.owner_id == token.owner_id &&
+            found->second.generation == token.generation) {
+            entries_.erase(found);
+        }
+    } catch (...) {
+    }
+}
+
+bool ControlAuthorityManager::isLeased(
+    const std::string& resource_id)
+{
+    if (resource_id.empty()) {
+        return false;
+    }
+    std::lock_guard lock(mutex_);
+    const auto found = entries_.find(resource_id);
+    if (found == entries_.end()) {
+        return false;
+    }
+    if (expired_(found->second)) {
+        entries_.erase(found);
+        return false;
+    }
+    return true;
+}
+
+void ControlAuthorityManager::revoke(
+    const std::string& resource_id) noexcept
+{
+    try {
+        std::lock_guard lock(mutex_);
+        entries_.erase(resource_id);
+    } catch (...) {
+    }
+}
+
+void ControlAuthorityManager::clear() noexcept
+{
+    try {
+        std::lock_guard lock(mutex_);
+        entries_.clear();
+    } catch (...) {
+    }
+}
+
+bool ControlAuthorityManager::expired_(
+    const Entry& entry) const noexcept
+{
+    return std::chrono::steady_clock::now() >= entry.deadline;
+}
+
+} // namespace cmvr::control

cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp

@@ -0,0 +1,91 @@
+#include "manager/control_authority/include/control_authority_manager.h"
+
+#include <chrono>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::control {
+namespace {
+
+using namespace std::chrono_literals;
+
+class ControlAuthorityManagerTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        ControlAuthorityManager::instance().clear();
+    }
+
+    void TearDown() override
+    {
+        ControlAuthorityManager::instance().clear();
+    }
+};
+
+TEST_F(ControlAuthorityManagerTest, LeaseIsExclusiveAndExactReleaseRestoresAccess)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto first =
+        manager.tryAcquire("right_arm", "session-a", 100ms);
+    ASSERT_TRUE(first.acquired);
+    EXPECT_TRUE(manager.validate(first.token));
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+
+    const auto conflict =
+        manager.tryAcquire("right_arm", "session-b", 100ms);
+    EXPECT_FALSE(conflict.acquired);
+
+    manager.release(first.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+    EXPECT_TRUE(
+        manager.tryAcquire("right_arm", "session-b", 100ms)
+            .acquired);
+}
+
+TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto old =
+        manager.tryAcquire("right_arm", "session-a", 100ms);
+    ASSERT_TRUE(old.acquired);
+    manager.release(old.token);
+    const auto current =
+        manager.tryAcquire("right_arm", "session-a", 100ms);
+    ASSERT_TRUE(current.acquired);
+    ASSERT_NE(
+        old.token.generation,
+        current.token.generation);
+
+    manager.release(old.token);
+    EXPECT_TRUE(manager.validate(current.token));
+}
+
+TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto lease =
+        manager.tryAcquire("right_arm", "session-a", 20ms);
+    ASSERT_TRUE(lease.acquired);
+    std::this_thread::sleep_for(10ms);
+    ASSERT_TRUE(manager.renew(lease.token, 30ms));
+    std::this_thread::sleep_for(20ms);
+    EXPECT_TRUE(manager.validate(lease.token));
+    std::this_thread::sleep_for(20ms);
+    EXPECT_FALSE(manager.validate(lease.token));
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest, DifferentArmsCanBeLeasedIndependently)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    EXPECT_TRUE(
+        manager.tryAcquire("right_arm", "session-a", 100ms)
+            .acquired);
+    EXPECT_TRUE(
+        manager.tryAcquire("left_arm", "session-b", 100ms)
+            .acquired);
+}
+
+} // namespace
+} // namespace cmvr::control

cmvr-es/manager/device_manager/CMakeLists.txt

@@ -42,9 +42,39 @@ if(BUILD_TESTING)
                 "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime")
         list(JOIN _device_manager_test_library_dirs ":"
                 _device_manager_test_library_path)
+        set(_device_manager_snapshot_test_environment
+                "LD_LIBRARY_PATH=${_device_manager_test_library_path}")
+        if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+            list(APPEND _device_manager_snapshot_test_environment
+                    "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+        endif()
         set_tests_properties(device_manager_snapshot_test PROPERTIES
                 ENVIRONMENT
-                "LD_LIBRARY_PATH=${_device_manager_test_library_path}"
+                "${_device_manager_snapshot_test_environment}"
         )
     endif()
+
+    add_executable(device_manager_lifecycle_test
+            tests/device_manager_lifecycle_test.cpp
+    )
+    target_link_libraries(device_manager_lifecycle_test PRIVATE
+            cmvr_es::device_manager
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME device_manager_lifecycle_test
+            COMMAND device_manager_lifecycle_test
+    )
+    set(_device_manager_lifecycle_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _device_manager_lifecycle_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(device_manager_lifecycle_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_device_manager_lifecycle_test_environment}"
+    )
 endif()

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

@@ -27,9 +27,10 @@ namespace cmvr::device {
         static DeviceManager& getInstance();
         static void destroyInstance();
 
-        void start();
+        bool start();
-        void restart();
+        bool restart();
         void stop();
+        bool initialized() const noexcept { return initialized_; }
 
         void getDeviceList(std::list<std::pair<std::string, std::string>> &device_list);
         void registerDevice(const std::shared_ptr<AbstractDevice>& device);
@@ -54,14 +55,19 @@ namespace cmvr::device {
         std::unordered_map<std::string, DeviceRecord> devices_;
         std::unordered_map<std::string, ManagedDeviceSnapshot> device_statuses_;
         std::unique_ptr<DeviceFactory> dev_factory_;
+        bool initialized_{false};
 
         explicit DeviceManager(const config::DeviceManagerConfig &cfg);
         void log_device_plan_() const;
-        void pre_scan_robot_arm_dependencies_() const;
+        bool pre_scan_robot_arm_dependencies_() const;
-        void init_devices_();
+        bool init_devices_();
         void configure_mujoco_viewer_pip_();
-        void start_devices_();
+        void initialize_device_statuses_();
-        void stop_devices_();
+        void mark_initializing_statuses_error_(const std::string& error_message);
+        void update_device_status_(const std::string& device_id,
+                                   ManagedDeviceState state,
+                                   const std::string& error_message = {});
+        void stop_devices_(bool update_status = true);
     };
 } // cmvr
 

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

@@ -6,7 +6,10 @@
 #include "../include/device_manager.h"
 
 #include <algorithm>
+#include <chrono>
 #include <exception>
+#include <utility>
+#include <vector>
 
 #include "devices/agv/abstract_agv.h"
 #include "devices/arm/robot_arm.h"
@@ -31,6 +34,51 @@ namespace {
 
 using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
 using MotorJointSelections = std::unordered_map<std::string, GroupJointSelection>;
+constexpr std::size_t kMaxDeviceErrorLength = 512;
+
+std::uint64_t unixTimeMs() noexcept
+{
+    const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::system_clock::now().time_since_epoch());
+    return elapsed.count() > 0
+               ? static_cast<std::uint64_t>(elapsed.count())
+               : 1U;
+}
+
+std::string truncateDeviceError(const std::string& message)
+{
+    return message.substr(0, kMaxDeviceErrorLength);
+}
+
+DeviceKind deviceTypeToKind(
+    const cmvr::config::DeviceConfigEntry::DeviceType type)
+{
+    switch (type) {
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT:
+            return DeviceKind::BioHead;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM:
+            return DeviceKind::MotorSystem;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM:
+            return DeviceKind::Arm;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA:
+            return DeviceKind::Camera;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND:
+            return DeviceKind::DexHand;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MICROPHONE:
+            return DeviceKind::Microphone;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_SPEAKER:
+            return DeviceKind::Speaker;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_AGV:
+            return DeviceKind::AGV;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD:
+            return DeviceKind::MujocoWorld;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER:
+            return DeviceKind::MujocoViewer;
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN:
+        default:
+            return DeviceKind::Unknown;
+    }
+}
 
 void logSection(const char* title)
 {
@@ -126,12 +174,24 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
     cfg_ = cfg;
 
     dev_factory_ = std::make_unique<DeviceFactory>();
+    initialize_device_statuses_();
     logSection("Device Plan");
     log_device_plan_();
-    pre_scan_robot_arm_dependencies_();
+    const bool dependencies_valid = pre_scan_robot_arm_dependencies_();
     logSection("Initialize Devices");
-    init_devices_();
+    if (!dependencies_valid) {
-    configure_mujoco_viewer_pip_();
+        mark_initializing_statuses_error_(
+            "device dependency validation failed");
+    }
+    const bool devices_initialized =
+        dependencies_valid ? init_devices_() : false;
+    initialized_ = dependencies_valid && devices_initialized;
+    if (initialized_) {
+        configure_mujoco_viewer_pip_();
+    } else {
+        CMVR_LOG(ERROR) << "[DeviceManager]: Initialization failed for at "
+                           "least one enabled device";
+    }
 }
 
 DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) {
@@ -156,34 +216,133 @@ void DeviceManager::destroyInstance() {
     MotorManager::clearActiveJoints();
 }
 
-void DeviceManager::start(){
+bool DeviceManager::start(){
-    for (auto& [id, record] : devices_) {
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
-        if (!record.device) {
+    if (!initialized_) {
-            CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id;
+        CMVR_LOG(ERROR) << "[DeviceManager]: Refusing to start because "
-            continue;
+                           "initialization did not complete";
-        }
+        stop_devices_(false);
-        if (record.device->start()) {
+        return false;
-            CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success";
+    }
-        } else {
+
-            CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed";
+    std::vector<std::pair<std::string, std::shared_ptr<AbstractDevice>>>
+        devices;
+    {
+        std::shared_lock lock(devices_mutex_);
+        devices.reserve(devices_.size());
+        for (const auto& [id, record] : devices_) {
+            devices.emplace_back(id, record.device);
         }
     }
+
+    bool all_started = true;
+    for (const auto& [id, device] : devices) {
+        if (!device) {
+            CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id;
+            update_device_status_(
+                id, ManagedDeviceState::Error,
+                "cannot start null device: " + id);
+            all_started = false;
+            continue;
+        }
+        bool started = false;
+        std::string error_message;
+        try {
+            started = device->start();
+            if (!started) {
+                error_message = "device start returned false: " + id;
+            }
+        } catch (const std::exception& error) {
+            error_message =
+                "device start threw for " + id + ": " + error.what();
+            CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id
+                            << " threw: " << error.what();
+        } catch (...) {
+            error_message =
+                "device start threw an unknown exception: " + id;
+            CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id
+                            << " threw an unknown exception";
+        }
+        if (started) {
+            update_device_status_(id, ManagedDeviceState::Running);
+            CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success";
+        } else {
+            update_device_status_(
+                id, ManagedDeviceState::Error, error_message);
+            CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed";
+            all_started = false;
+        }
+    }
+    if (!all_started) {
+        CMVR_LOG(ERROR) << "[DeviceManager]: At least one enabled device failed "
+                           "to start; stopping all devices";
+        // Rollback is a physical cleanup operation. Preserve the start
+        // results in the status table so the failure is diagnosable; an
+        // explicit stop() records Stopped/Error transitions.
+        stop_devices_(false);
+    }
+    return all_started;
 }
 
-void DeviceManager::restart() {
+bool DeviceManager::restart() {
     stop();
-    start();
+    return start();
 }
 
 void DeviceManager::stop() {
-    for (auto& [id, record] : devices_) {
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
-        if (!record.device) {
+    stop_devices_();
+}
+
+void DeviceManager::stop_devices_(const bool update_status) {
+    std::vector<std::pair<std::string, std::shared_ptr<AbstractDevice>>>
+        devices;
+    {
+        std::shared_lock lock(devices_mutex_);
+        devices.reserve(devices_.size());
+        for (const auto& [id, record] : devices_) {
+            devices.emplace_back(id, record.device);
+        }
+    }
+
+    for (const auto& [id, device] : devices) {
+        if (!device) {
             CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id;
+            if (update_status) {
+                update_device_status_(
+                    id, ManagedDeviceState::Error,
+                    "cannot stop null device: " + id);
+            }
             continue;
         }
-        if (record.device->stop()) {
+        bool stopped = false;
+        std::string error_message;
+        try {
+            stopped = device->stop();
+            if (!stopped) {
+                error_message = "device stop returned false: " + id;
+            }
+        } catch (const std::exception& error) {
+            error_message =
+                "device stop threw for " + id + ": " + error.what();
+            CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id
+                            << " threw: " << error.what();
+        } catch (...) {
+            error_message =
+                "device stop threw an unknown exception: " + id;
+            CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id
+                            << " threw an unknown exception";
+        }
+        if (stopped) {
+            if (update_status) {
+                update_device_status_(id, ManagedDeviceState::Stopped);
+            }
             CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success";
         } else {
+            if (update_status) {
+                update_device_status_(
+                    id, ManagedDeviceState::Error, error_message);
+            }
             CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed";
         }
     }
@@ -192,6 +351,7 @@ void DeviceManager::stop() {
 template <class DeviceType>
 std::shared_ptr<DeviceType> DeviceManager::getDevice(const std::string& device_id)
 {
+    std::shared_lock lock(devices_mutex_);
     auto it = devices_.find(device_id);
     if (it == devices_.end()) {
         CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found.";
@@ -219,6 +379,7 @@ std::shared_ptr<AbstractDevice> DeviceManager::getDeviceBase(const std::string&
 
 void DeviceManager::getDeviceList(std::list<std::pair<std::string, std::string>>& device_list){
     device_list.clear();
+    std::shared_lock lock(devices_mutex_);
     for (const auto& [device_id, record] : devices_) {
         device_list.emplace_back(device_id, record.type_name);
     }
@@ -244,22 +405,115 @@ void DeviceManager::registerDevice(const std::string& device_id,
         CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register device with empty id";
         return;
     }
-    if (devices_.count(device_id)) {
-        CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id;
-        return;
-    }
     DeviceRecord record;
     record.id = device_id;
     record.kind = device->kind();
     record.type_name = device->typeName();
     record.device = device;
-    devices_.emplace(record.id, std::move(record));
+    {
+        std::unique_lock lock(devices_mutex_);
+        if (devices_.count(device_id)) {
+            CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id;
+            return;
+        }
+
+        ManagedDeviceSnapshot status;
+        status.id = record.id;
+        status.kind = record.kind;
+        status.type_name = record.type_name;
+        status.enabled = true;
+        status.state = ManagedDeviceState::Registered;
+        status.status_updated_at_unix_ms = unixTimeMs();
+        devices_.emplace(record.id, std::move(record));
+        device_statuses_[device_id] = std::move(status);
+    }
     CMVR_LOG(INFO) << "[DeviceManager]: Register device success"
                    << ", id=" << device_id
                    << ", type=" << device->typeName()
                    << ", kind=" << toString(device->kind());
 }
 
+void DeviceManager::initialize_device_statuses_()
+{
+    std::unique_lock lock(devices_mutex_);
+    for (const auto& entry : cfg_.devices()) {
+        const auto kind = deviceTypeToKind(entry.type());
+        ManagedDeviceSnapshot status;
+        status.id = entry.id();
+        status.kind = kind;
+        status.type_name = toString(kind);
+        status.enabled = entry.enable();
+        status.state = entry.enable()
+                           ? ManagedDeviceState::Initializing
+                           : ManagedDeviceState::Disabled;
+        status.status_updated_at_unix_ms = unixTimeMs();
+
+        if (entry.id().empty()) {
+            status.state = ManagedDeviceState::Error;
+            status.abnormal = true;
+            status.error_message =
+                "configured device id must not be empty";
+        }
+
+        const auto [it, inserted] =
+            device_statuses_.emplace(entry.id(), std::move(status));
+        if (!inserted) {
+            auto& duplicate_status = it->second;
+            duplicate_status.enabled =
+                duplicate_status.enabled || entry.enable();
+            duplicate_status.state = ManagedDeviceState::Error;
+            duplicate_status.abnormal = true;
+            duplicate_status.error_message = truncateDeviceError(
+                "duplicate configured device id: " + entry.id());
+            duplicate_status.status_updated_at_unix_ms = unixTimeMs();
+        }
+    }
+}
+
+void DeviceManager::mark_initializing_statuses_error_(
+    const std::string& error_message)
+{
+    std::unique_lock lock(devices_mutex_);
+    for (auto& [id, status] : device_statuses_) {
+        if (status.state != ManagedDeviceState::Initializing) {
+            continue;
+        }
+        status.state = ManagedDeviceState::Error;
+        status.abnormal = true;
+        status.error_message = truncateDeviceError(
+            error_message + ": " + id);
+        status.status_updated_at_unix_ms = unixTimeMs();
+    }
+}
+
+void DeviceManager::update_device_status_(
+    const std::string& device_id,
+    const ManagedDeviceState state,
+    const std::string& error_message)
+{
+    std::unique_lock lock(devices_mutex_);
+    auto& status = device_statuses_[device_id];
+    if (status.id.empty()) {
+        status.id = device_id;
+    }
+    const auto device_it = devices_.find(device_id);
+    if (device_it != devices_.end()) {
+        status.kind = device_it->second.kind;
+        status.type_name = device_it->second.type_name;
+    }
+    status.enabled = true;
+    status.state = state;
+    status.abnormal = state == ManagedDeviceState::Error;
+    status.error_message =
+        state == ManagedDeviceState::Error
+            ? truncateDeviceError(
+                  error_message.empty()
+                      ? "device lifecycle operation failed: " + device_id
+                      : error_message)
+            : std::string{};
+    status.status_updated_at_unix_ms = unixTimeMs();
+}
+
 DeviceManagerSnapshot DeviceManager::snapshot() const
 {
     struct SnapshotSource {
@@ -270,15 +524,14 @@ DeviceManagerSnapshot DeviceManager::snapshot() const
     std::vector<SnapshotSource> sources;
     {
         std::shared_lock lock(devices_mutex_);
-        sources.reserve(devices_.size());
+        sources.reserve(device_statuses_.size());
-        for (const auto& [id, record] : devices_) {
+        for (const auto& [id, stored_status] : device_statuses_) {
             SnapshotSource source;
-            source.status.id = id;
+            source.status = stored_status;
-            source.status.kind = record.kind;
+            const auto device_it = devices_.find(id);
-            source.status.type_name = record.type_name;
+            if (device_it != devices_.end()) {
-            source.status.enabled = true;
+                source.device = device_it->second.device;
-            source.status.state = ManagedDeviceState::Ready;
+            }
-            source.device = record.device;
             sources.push_back(std::move(source));
         }
     }
@@ -302,10 +555,23 @@ DeviceManagerSnapshot DeviceManager::snapshot() const
                     "device health snapshot threw an unknown exception";
             }
         }
-        source.status.abnormal =
+        source.status.health.error_message =
+            truncateDeviceError(source.status.health.error_message);
+        const bool lifecycle_error =
+            source.status.state == ManagedDeviceState::Error;
+        const bool health_error =
             source.status.health.state == DeviceHealthState::Degraded ||
             source.status.health.state == DeviceHealthState::Fault;
-        source.status.error_message = source.status.health.error_message;
+        source.status.abnormal = lifecycle_error || health_error;
+        if (source.status.error_message.empty()) {
+            source.status.error_message =
+                source.status.health.error_message;
+        }
+        source.status.error_message =
+            truncateDeviceError(source.status.error_message);
+        if (source.status.status_updated_at_unix_ms == 0) {
+            source.status.status_updated_at_unix_ms = unixTimeMs();
+        }
         result.devices.push_back(std::move(source.status));
     }
 
@@ -354,10 +620,11 @@ void DeviceManager::log_device_plan_() const
     CMVR_LOG(INFO) << "[DeviceManager]: Device plan end";
 }
 
-void DeviceManager::pre_scan_robot_arm_dependencies_() const
+bool DeviceManager::pre_scan_robot_arm_dependencies_() const
 {
     MotorJointSelections selections;
     std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
+    MotorManager::clearActiveJoints();
 
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) {
@@ -365,22 +632,22 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
-            return;
+            return false;
         }
         if (entry.config_file().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
-            return;
+            return false;
         }
 
         config::MotorRootConfig root_cfg;
         if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file();
-            return;
+            return false;
         }
         if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
                             << "' does not match config id '" << root_cfg.motor().id() << "'";
-            return;
+            return false;
         }
         motor_roots.emplace(entry.id(), std::move(root_cfg));
     }
@@ -391,17 +658,17 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty";
-            return;
+            return false;
         }
         if (entry.config_file().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id();
-            return;
+            return false;
         }
 
         config::ArmRootConfig root_cfg;
         if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file();
-            return;
+            return false;
         }
 
         const config::RobotArmConfig* arm_cfg = nullptr;
@@ -414,29 +681,30 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         if (!arm_cfg) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id()
                             << "' not found in config: " << entry.config_file();
-            return;
+            return false;
         }
 
-        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) {
+        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor ||
+            arm_cfg->backend_case() == config::RobotArmConfig::kUme) {
             continue;
         }
         if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id();
-            return;
+            return false;
         }
 
         const auto& motor_config = arm_cfg->motor();
         if (motor_config.motor_system_id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id();
-            return;
+            return false;
         }
         if (motor_config.motor_group_ids_size() == 0) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id();
-            return;
+            return false;
         }
         if (motor_config.joint_names_size() == 0) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id();
-            return;
+            return false;
         }
 
         const auto motor_root_it = motor_roots.find(motor_config.motor_system_id());
@@ -444,7 +712,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
                             << "' depends on disabled or missing MotorManager: "
                             << motor_config.motor_system_id();
-            return;
+            return false;
         }
 
         std::unordered_set<std::string> allowed_groups;
@@ -452,7 +720,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         for (const auto& group_id : motor_config.motor_group_ids()) {
             if (group_id.empty()) {
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id();
-                return;
+                return false;
             }
             allowed_groups.insert(group_id);
         }
@@ -461,7 +729,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         for (const auto& joint_name : motor_config.joint_names()) {
             if (joint_name.empty()) {
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id();
-                return;
+                return false;
             }
 
             std::string matched_group;
@@ -480,7 +748,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
                                 << "' joint '" << joint_name
                                 << "' not found in configured motor_group_ids";
-                return;
+                return false;
             }
             group_selection[matched_group].insert(joint_name);
         }
@@ -495,46 +763,132 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
     }
 
-    MotorManager::clearActiveJoints();
     for (auto& [motor_system_id, group_selection] : selections) {
         MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
     }
+    return true;
 }
 
-void DeviceManager::init_devices_() {
+bool DeviceManager::init_devices_() {
+    bool all_initialized = true;
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable()) {
             continue;
         }
 
+        {
+            std::shared_lock lock(devices_mutex_);
+            const auto status_it = device_statuses_.find(entry.id());
+            if (status_it != device_statuses_.end() &&
+                status_it->second.state == ManagedDeviceState::Error) {
+                all_initialized = false;
+                continue;
+            }
+        }
+
         CMVR_LOG(INFO) << "[DeviceManager]: Initialize device begin"
                        << ", id=" << entry.id()
                        << ", type=" << deviceTypeToString(entry.type())
                        << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file());
 
-        DeviceRecord record = dev_factory_->create(entry);
+        DeviceRecord record;
+        try {
+            record = dev_factory_->create(entry);
+        } catch (const std::exception& error) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "device creation threw for " + entry.id() + ": " +
+                    error.what());
+            CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw for "
+                            << entry.id() << ": " << error.what();
+            all_initialized = false;
+            continue;
+        } catch (...) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "device creation threw an unknown exception: " +
+                    entry.id());
+            CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw an "
+                               "unknown exception for " << entry.id();
+            all_initialized = false;
+            continue;
+        }
         if (!record.device || record.id.empty()) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "failed to create configured device: " + entry.id());
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to create device for entry id=" << entry.id();
+            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Create device object success"
                        << ", id=" << record.id
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind);
-        if (devices_.count(record.id)) {
+        bool duplicate_device = false;
+        {
+            std::shared_lock lock(devices_mutex_);
+            duplicate_device = devices_.count(record.id) != 0;
+        }
+        if (duplicate_device) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "duplicate configured device id: " + record.id);
             CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate " << record.type_name << " Device ID " << record.id;
+            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Init device object begin"
                        << ", id=" << record.id
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind);
-        if (!record.device->init()) {
+        bool device_initialized = false;
+        std::string init_error_message;
+        try {
+            device_initialized = record.device->init();
+            if (!device_initialized) {
+                init_error_message =
+                    "device init returned false: " + record.id;
+            }
+        } catch (const std::exception& error) {
+            init_error_message =
+                "device init threw for " + record.id + ": " +
+                error.what();
+            CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw"
+                            << ", id=" << record.id
+                            << ", error=" << error.what();
+        } catch (...) {
+            init_error_message =
+                "device init threw an unknown exception: " + record.id;
+            CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw an "
+                               "unknown exception, id=" << record.id;
+        }
+        if (!device_initialized) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                init_error_message);
             CMVR_LOG(ERROR) << "[DeviceManager]: Init device object failed"
                             << ", id=" << record.id
                             << ", type=" << record.type_name
                             << ", kind=" << toString(record.kind)
                             << ", config_file=" << entry.config_file();
+            try {
+                if (!record.device->stop()) {
+                    CMVR_LOG(ERROR)
+                        << "[DeviceManager]: Cleanup after failed init "
+                           "returned false, id=" << record.id;
+                }
+            } catch (const std::exception& error) {
+                CMVR_LOG(ERROR)
+                    << "[DeviceManager]: Cleanup after failed init threw"
+                    << ", id=" << record.id
+                    << ", error=" << error.what();
+            } catch (...) {
+                CMVR_LOG(ERROR)
+                    << "[DeviceManager]: Cleanup after failed init threw an "
+                       "unknown exception, id=" << record.id;
+            }
+            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Init device object success"
@@ -542,8 +896,36 @@ void DeviceManager::init_devices_() {
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind)
                        << ", config_file=" << entry.config_file();
-        devices_.emplace(record.id, std::move(record));
+        {
+            std::unique_lock lock(devices_mutex_);
+            const auto id = record.id;
+            const auto kind = record.kind;
+            const auto type_name = record.type_name;
+            const auto [device_it, inserted] =
+                devices_.emplace(id, std::move(record));
+            if (!inserted) {
+                auto& status = device_statuses_[entry.id()];
+                status.state = ManagedDeviceState::Error;
+                status.abnormal = true;
+                status.error_message = truncateDeviceError(
+                    "duplicate configured device id: " + id);
+                status.status_updated_at_unix_ms = unixTimeMs();
+                all_initialized = false;
+                continue;
+            }
+
+            auto& status = device_statuses_[id];
+            status.id = id;
+            status.kind = kind;
+            status.type_name = type_name;
+            status.enabled = true;
+            status.state = ManagedDeviceState::Ready;
+            status.abnormal = false;
+            status.error_message.clear();
+            status.status_updated_at_unix_ms = unixTimeMs();
+        }
     }
+    return all_initialized;
 }
 
 void DeviceManager::configure_mujoco_viewer_pip_()

cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp

@@ -0,0 +1,124 @@
+#include "manager/device_manager/include/device_manager.h"
+
+#include <stdexcept>
+
+#include <gtest/gtest.h>
+
+namespace {
+
+class LifecycleDevice final : public cmvr::device::AbstractDevice {
+public:
+    explicit LifecycleDevice(const std::string& id)
+        : AbstractDevice(id)
+    {
+    }
+
+    cmvr::device::DeviceKind kind() const noexcept override
+    {
+        return cmvr::device::DeviceKind::Camera;
+    }
+
+    std::string typeName() const override { return "LifecycleDevice"; }
+
+    bool start() override
+    {
+        ++start_calls;
+        if (throw_on_start) {
+            throw std::runtime_error("start failure");
+        }
+        return start_result;
+    }
+
+    bool stop() override
+    {
+        ++stop_calls;
+        return true;
+    }
+
+    bool start_result{true};
+    bool throw_on_start{false};
+    int start_calls{0};
+    int stop_calls{0};
+};
+
+class DeviceManagerLifecycleTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        cmvr::device::DeviceManager::destroyInstance();
+    }
+
+    void TearDown() override
+    {
+        cmvr::device::DeviceManager::destroyInstance();
+    }
+};
+
+TEST_F(DeviceManagerLifecycleTest,
+       EnabledDeviceCreationFailureMarksInitializationFailed)
+{
+    cmvr::config::DeviceManagerConfig config;
+    auto* entry = config.add_devices();
+    entry->set_id("unsupported");
+    entry->set_type(
+        cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN);
+    entry->set_enable(true);
+
+    auto& manager =
+        cmvr::device::DeviceManager::getInstance(config);
+    EXPECT_FALSE(manager.initialized());
+    EXPECT_FALSE(manager.start());
+}
+
+TEST_F(DeviceManagerLifecycleTest, DisabledInvalidDeviceIsIgnored)
+{
+    cmvr::config::DeviceManagerConfig config;
+    auto* entry = config.add_devices();
+    entry->set_id("disabled");
+    entry->set_type(
+        cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN);
+    entry->set_enable(false);
+
+    auto& manager =
+        cmvr::device::DeviceManager::getInstance(config);
+    EXPECT_TRUE(manager.initialized());
+    EXPECT_TRUE(manager.start());
+}
+
+TEST_F(DeviceManagerLifecycleTest,
+       DeviceStartFailureIsReturnedAndTriggersStop)
+{
+    cmvr::config::DeviceManagerConfig config;
+    auto& manager =
+        cmvr::device::DeviceManager::getInstance(config);
+    ASSERT_TRUE(manager.initialized());
+
+    auto device =
+        std::make_shared<LifecycleDevice>("start_failure");
+    device->start_result = false;
+    manager.registerDevice(device);
+
+    EXPECT_FALSE(manager.start());
+    EXPECT_EQ(device->start_calls, 1);
+    EXPECT_EQ(device->stop_calls, 1);
+}
+
+TEST_F(DeviceManagerLifecycleTest,
+       DeviceStartExceptionIsReturnedAndTriggersStop)
+{
+    cmvr::config::DeviceManagerConfig config;
+    auto& manager =
+        cmvr::device::DeviceManager::getInstance(config);
+    ASSERT_TRUE(manager.initialized());
+
+    auto device =
+        std::make_shared<LifecycleDevice>("start_exception");
+    device->throw_on_start = true;
+    manager.registerDevice(device);
+
+    EXPECT_FALSE(manager.start());
+    EXPECT_EQ(device->start_calls, 1);
+    EXPECT_EQ(device->stop_calls, 1);
+}
+
+} // namespace

cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp

@@ -253,6 +253,13 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(duplicate_status->error_message ==
                "duplicate configured device id: duplicate_device");
 
+    // Configuration failures deliberately make this manager ineligible for
+    // start(). Use a fresh, valid manager for dynamic registration and
+    // lifecycle transitions so the test does not weaken fail-closed startup.
+    DeviceManager::destroyInstance();
+    cmvr::config::DeviceManagerConfig dynamic_config;
+    auto& dynamic_manager = DeviceManager::getInstance(dynamic_config);
+
     auto healthy = std::make_shared<FakeDevice>("z_healthy");
     auto degraded = std::make_shared<FakeDevice>("a_degraded");
     degraded->health = {
@@ -264,18 +271,18 @@ bool testConfiguredAndDynamicSnapshots()
     auto health_throw = std::make_shared<FakeDevice>("b_health_throw");
     health_throw->throw_on_health = true;
 
-    manager.registerDevice(healthy);
+    dynamic_manager.registerDevice(healthy);
-    manager.registerDevice(degraded);
+    dynamic_manager.registerDevice(degraded);
-    manager.registerDevice(start_fail);
+    dynamic_manager.registerDevice(start_fail);
-    manager.registerDevice(stop_fail);
+    dynamic_manager.registerDevice(stop_fail);
-    manager.registerDevice(health_throw);
+    dynamic_manager.registerDevice(health_throw);
 
     // Duplicate registration must retain the original object and status.
-    manager.registerDevice(
+    dynamic_manager.registerDevice(
         std::make_shared<FakeDevice>("z_healthy", DeviceKind::Speaker));
-    CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy);
+    CHECK_TRUE(dynamic_manager.getDeviceBase("z_healthy") == healthy);
 
-    const auto registered = manager.snapshot();
+    const auto registered = dynamic_manager.snapshot();
     CHECK_TRUE(isSorted(registered));
     const auto* healthy_registered =
         findDevice(registered, "z_healthy");
@@ -304,8 +311,8 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(thrown_health->health.error_message.size() <= 512);
     CHECK_TRUE(thrown_health->error_message.size() <= 512);
 
-    manager.start();
+    CHECK_TRUE(!dynamic_manager.start());
-    const auto running = manager.snapshot();
+    const auto running = dynamic_manager.snapshot();
     CHECK_TRUE(findDevice(running, "z_healthy")->state ==
                ManagedDeviceState::Running);
     CHECK_TRUE(findDevice(running, "m_start_fail")->state ==
@@ -319,14 +326,16 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(healthy_registered->state ==
                ManagedDeviceState::Registered);
 
-    manager.stop();
+    dynamic_manager.stop();
-    const auto stopped = manager.snapshot();
+    const auto stopped = dynamic_manager.snapshot();
     CHECK_TRUE(findDevice(stopped, "z_healthy")->state ==
                ManagedDeviceState::Stopped);
     CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state ==
                ManagedDeviceState::Error);
     CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal);
-    CHECK_TRUE(healthy->stop_calls.load() == 1);
+    // Failed start rolls back every device once; explicit stop performs the
+    // second best-effort stop.
+    CHECK_TRUE(healthy->stop_calls.load() == 2);
     return true;
 }
 

cmvr-es/manager/task_manager/CMakeLists.txt

@@ -15,3 +15,29 @@ target_link_libraries(task_manager
 
 add_library(cmvr_es::task_manager ALIAS task_manager)
 install(TARGETS task_manager LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(task_manager_lifecycle_test
+            tests/task_manager_lifecycle_test.cpp
+    )
+    target_link_libraries(task_manager_lifecycle_test PRIVATE
+            cmvr_es::task_manager
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME task_manager_lifecycle_test
+            COMMAND task_manager_lifecycle_test
+    )
+    set(_task_manager_lifecycle_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _task_manager_lifecycle_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(task_manager_lifecycle_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_task_manager_lifecycle_test_environment}"
+    )
+endif()

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

@@ -26,9 +26,10 @@ namespace cmvr::task {
 
         ~TaskManager();
 
-        void startRunTask(double control_period_s = 0.001);
+        bool startRunTask(double control_period_s = 0.001);
         void stopRunTask();
         bool running() const { return running_.load(); }
+        bool initialized() const noexcept { return initialized_; }
 
         std::shared_ptr<Task> getTask(const std::string& task_id) const;
         std::shared_ptr<TouchScreenTask> getTouchScreenTask(const std::string& task_id = "touch_screen") const;
@@ -37,7 +38,7 @@ namespace cmvr::task {
         explicit TaskManager(const config::TaskManagerConfig& cfg);
 
         void logTaskPlan() const;
-        void initTasks();
+        bool initTasks();
         void runTaskLoop(double control_period_s);
         static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode);
 
@@ -49,8 +50,10 @@ namespace cmvr::task {
         std::unordered_map<std::string, double> task_period_s_;
         std::unordered_map<std::string, std::chrono::steady_clock::time_point> next_step_time_;
         mutable std::mutex tasks_mutex_;
+        std::mutex lifecycle_mutex_;
         std::atomic<bool> running_{false};
         std::thread run_thread_;
+        bool initialized_{false};
     };
 
 } // namespace cmvr::task

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

@@ -40,6 +40,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type)
             return "TASK_TYPE_SELF_COLLISION";
         case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE:
             return "TASK_TYPE_QUIC_EDGE";
+        case config::TaskConfigEntry::TASK_TYPE_UME_TELEOP:
+            return "TASK_TYPE_UME_TELEOP";
         case config::TaskConfigEntry::TASK_TYPE_UNKNOWN:
         default:
             return "TASK_TYPE_UNKNOWN";
@@ -59,6 +61,21 @@ const char* taskConfigRunModeToString(const config::TaskConfigEntry::TaskRunMode
     }
 }
 
+void stopTaskNoThrow(const std::shared_ptr<Task>& task)
+{
+    if (!task) {
+        return;
+    }
+    try {
+        task->stop();
+    } catch (const std::exception& error) {
+        CMVR_LOG(ERROR) << "[TaskManager] task stop threw: "
+                        << error.what();
+    } catch (...) {
+        CMVR_LOG(ERROR) << "[TaskManager] task stop threw an unknown exception";
+    }
+}
+
 } // namespace
 
 std::shared_ptr<TaskManager> TaskManager::instance_ = nullptr;
@@ -70,7 +87,11 @@ TaskManager::TaskManager(const config::TaskManagerConfig& cfg)
     logSection("Task Plan");
     logTaskPlan();
     logSection("Initialize Tasks");
-    initTasks();
+    initialized_ = initTasks();
+    if (!initialized_) {
+        CMVR_LOG(ERROR) << "[TaskManager] Initialization failed for at least "
+                           "one enabled task";
+    }
 }
 
 TaskManager::~TaskManager()
@@ -126,16 +147,20 @@ std::shared_ptr<Task> TaskManager::getTask(const std::string& task_id) const
     return it->second;
 }
 
-void TaskManager::startRunTask(const double control_period_s)
+bool TaskManager::startRunTask(const double control_period_s)
 {
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    if (!initialized_) {
+        CMVR_LOG(ERROR) << "[TaskManager] refusing to start because "
+                           "initialization did not complete";
+        return false;
+    }
     if (!std::isfinite(control_period_s) || control_period_s <= 0.0) {
         CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s";
-        return;
+        return false;
     }
-
+    if (running_.load()) {
-    bool expected = false;
+        return true;
-    if (!running_.compare_exchange_strong(expected, true)) {
-        return;
     }
 
     std::vector<std::shared_ptr<Task>> tasks;
@@ -151,39 +176,57 @@ void TaskManager::startRunTask(const double control_period_s)
 
     std::vector<std::shared_ptr<Task>> started_tasks;
     for (const auto& task : tasks) {
-        if (!task->start()) {
+        bool started = false;
+        try {
+            started = task->start();
+        } catch (const std::exception& error) {
+            CMVR_LOG(ERROR) << "[TaskManager] task start threw: "
+                            << task->id() << ", error=" << error.what();
+        } catch (...) {
+            CMVR_LOG(ERROR) << "[TaskManager] task start threw an unknown "
+                               "exception: " << task->id();
+        }
+        if (!started) {
             CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id();
-            for (const auto& started_task : started_tasks) {
+            stopTaskNoThrow(task);
-                try {
+            for (auto it = started_tasks.rbegin();
-                    started_task->stop();
+                 it != started_tasks.rend(); ++it) {
-                } catch (...) {
+                stopTaskNoThrow(*it);
-                }
             }
             running_.store(false);
-            return;
+            return false;
         }
         started_tasks.push_back(task);
     }
 
+    running_.store(true);
     try {
         run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s);
     } catch (const std::exception& e) {
         CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what();
         running_.store(false);
-        for (const auto& task : started_tasks) {
+        for (auto it = started_tasks.rbegin();
-            try {
+             it != started_tasks.rend(); ++it) {
-                task->stop();
+            stopTaskNoThrow(*it);
-            } catch (...) {
-            }
         }
-        return;
+        return false;
+    } catch (...) {
+        CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread with an "
+                           "unknown exception";
+        running_.store(false);
+        for (auto it = started_tasks.rbegin();
+             it != started_tasks.rend(); ++it) {
+            stopTaskNoThrow(*it);
+        }
+        return false;
     }
+    return true;
 }
 
 void TaskManager::stopRunTask()
 {
-    bool expected = true;
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
-    if (!running_.compare_exchange_strong(expected, false)) {
+    if (!running_.exchange(false)) {
         return;
     }
 
@@ -202,21 +245,20 @@ void TaskManager::stopRunTask()
         }
     }
     for (const auto& task : tasks) {
-        try {
+        stopTaskNoThrow(task);
-            task->stop();
-        } catch (...) {
-        }
     }
 }
 
-void TaskManager::initTasks()
+bool TaskManager::initTasks()
 {
+    bool all_initialized = true;
     for (const auto& entry : cfg_.tasks()) {
         if (!entry.enable()) {
             continue;
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[TaskManager] Task ID is empty";
+            all_initialized = false;
             continue;
         }
 
@@ -226,9 +268,30 @@ void TaskManager::initTasks()
                        << ", run_mode=" << taskConfigRunModeToString(entry.run_mode())
                        << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file());
 
-        auto task = TaskFactory::create(entry);
+        std::shared_ptr<Task> task;
+        try {
+            task = TaskFactory::create(entry);
+        } catch (const std::exception& error) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task creation threw: "
+                            << entry.id() << ", error=" << error.what();
+            all_initialized = false;
+            continue;
+        } catch (...) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task creation threw an unknown "
+                               "exception: " << entry.id();
+            all_initialized = false;
+            continue;
+        }
         if (!task || task->id() != entry.id()) {
             CMVR_LOG(ERROR) << "[TaskManager] Task ID mismatch: " << entry.id();
+            all_initialized = false;
+            continue;
+        }
+        if (entry.run_mode() ==
+            config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task run_mode is unknown: "
+                            << entry.id();
+            all_initialized = false;
             continue;
         }
         const TaskRunMode configured_run_mode = toTaskRunMode(entry.run_mode());
@@ -236,23 +299,39 @@ void TaskManager::initTasks()
             CMVR_LOG(ERROR) << "[TaskManager] Task run_mode mismatch: id=" << entry.id()
                             << ", configured=" << taskRunModeToString(configured_run_mode)
                             << ", actual=" << taskRunModeToString(task->runMode());
+            all_initialized = false;
             continue;
         }
         double control_period_s = entry.control_period_s();
         if (configured_run_mode == TaskRunMode::PERIODIC_STEP &&
             (!std::isfinite(control_period_s) || control_period_s <= 0.0)) {
             CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s for task: " << entry.id();
+            all_initialized = false;
             continue;
         }
-        if (!task->init()) {
+        bool task_initialized = false;
+        try {
+            task_initialized = task->init();
+        } catch (const std::exception& error) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task init threw: "
+                            << entry.id() << ", error=" << error.what();
+        } catch (...) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task init threw an unknown "
+                               "exception: " << entry.id();
+        }
+        if (!task_initialized) {
             CMVR_LOG(ERROR) << "[TaskManager] Task init failed: " << entry.id()
                             << ", status=" << task->detailStatusString();
+            stopTaskNoThrow(task);
+            all_initialized = false;
             continue;
         }
         {
             std::lock_guard lock(tasks_mutex_);
             if (tasks_.count(entry.id())) {
                 CMVR_LOG(ERROR) << "[TaskManager] Duplicate task ID: " << entry.id();
+                stopTaskNoThrow(task);
+                all_initialized = false;
                 continue;
             }
             if (configured_run_mode == TaskRunMode::PERIODIC_STEP) {
@@ -261,6 +340,7 @@ void TaskManager::initTasks()
             tasks_.emplace(entry.id(), std::move(task));
         }
     }
+    return all_initialized;
 }
 
 void TaskManager::logTaskPlan() const

cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp

@@ -0,0 +1,194 @@
+#include "manager/task_manager/include/task_manager.h"
+
+#include <memory>
+#include <stdexcept>
+
+#include <gtest/gtest.h>
+
+#include "task/task_factory.h"
+
+namespace {
+
+struct TaskBehavior {
+    bool init_result{true};
+    bool start_result{true};
+    bool throw_on_start{false};
+};
+
+TaskBehavior task_behavior;
+
+class LifecycleTask final : public cmvr::task::Task {
+public:
+    explicit LifecycleTask(std::string id)
+        : id_(std::move(id))
+    {
+    }
+
+    const std::string& id() const override { return id_; }
+    cmvr::task::TaskRunMode runMode() const override
+    {
+        return cmvr::task::TaskRunMode::BLOCKING_SERVICE;
+    }
+
+    bool init() override
+    {
+        ++init_calls;
+        state_ = task_behavior.init_result
+                     ? cmvr::task::TaskState::IDLE
+                     : cmvr::task::TaskState::FAILED;
+        return task_behavior.init_result;
+    }
+
+    bool start() override
+    {
+        ++start_calls;
+        if (task_behavior.throw_on_start) {
+            throw std::runtime_error("start failure");
+        }
+        state_ = task_behavior.start_result
+                     ? cmvr::task::TaskState::RUNNING
+                     : cmvr::task::TaskState::FAILED;
+        return task_behavior.start_result;
+    }
+
+    bool step(double) override { return true; }
+
+    void stop() override
+    {
+        ++stop_calls;
+        state_ = cmvr::task::TaskState::STOPPED;
+    }
+
+    cmvr::task::TaskState state() const override { return state_; }
+    bool isBusy() const override
+    {
+        return state_ == cmvr::task::TaskState::RUNNING;
+    }
+    bool isFinished() const override
+    {
+        return state_ == cmvr::task::TaskState::STOPPED;
+    }
+    bool isFailed() const override
+    {
+        return state_ == cmvr::task::TaskState::FAILED;
+    }
+    std::string stateString() const override
+    {
+        return cmvr::task::taskStateToString(state_);
+    }
+    std::string detailStatusString() const override
+    {
+        return stateString();
+    }
+
+    int init_calls{0};
+    int start_calls{0};
+    int stop_calls{0};
+
+private:
+    std::string id_;
+    cmvr::task::TaskState state_{
+        cmvr::task::TaskState::UNINITIALIZED};
+};
+
+std::shared_ptr<LifecycleTask> created_task;
+
+cmvr::config::TaskManagerConfig enabledTaskConfig()
+{
+    cmvr::config::TaskManagerConfig config;
+    auto* entry = config.add_tasks();
+    entry->set_id("lifecycle_task");
+    entry->set_type(
+        cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP);
+    entry->set_enable(true);
+    entry->set_run_mode(
+        cmvr::config::TaskConfigEntry::
+            TASK_RUN_MODE_BLOCKING_SERVICE);
+    return config;
+}
+
+class TaskManagerLifecycleTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        cmvr::task::TaskManager::destroyInstance();
+        task_behavior = {};
+        created_task.reset();
+        cmvr::task::TaskFactory::registerCreator(
+            cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP,
+            [](const cmvr::config::TaskConfigEntry& entry) {
+                created_task =
+                    std::make_shared<LifecycleTask>(entry.id());
+                return created_task;
+            });
+    }
+
+    void TearDown() override
+    {
+        cmvr::task::TaskManager::destroyInstance();
+        created_task.reset();
+    }
+};
+
+TEST_F(TaskManagerLifecycleTest,
+       EnabledTaskInitFailureMarksInitializationFailed)
+{
+    task_behavior.init_result = false;
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_FALSE(manager.initialized());
+    EXPECT_FALSE(manager.startRunTask());
+    EXPECT_FALSE(manager.running());
+    EXPECT_EQ(created_task->init_calls, 1);
+    EXPECT_EQ(created_task->start_calls, 0);
+    EXPECT_EQ(created_task->stop_calls, 1);
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       TaskStartFailureIsReturnedAndRunningRemainsFalse)
+{
+    task_behavior.start_result = false;
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+
+    ASSERT_TRUE(manager.initialized());
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_FALSE(manager.startRunTask());
+    EXPECT_FALSE(manager.running());
+    EXPECT_EQ(created_task->start_calls, 1);
+    EXPECT_EQ(created_task->stop_calls, 1);
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       TaskStartExceptionIsReturnedAndRunningRemainsFalse)
+{
+    task_behavior.throw_on_start = true;
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+
+    ASSERT_TRUE(manager.initialized());
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_FALSE(manager.startRunTask());
+    EXPECT_FALSE(manager.running());
+    EXPECT_EQ(created_task->start_calls, 1);
+    EXPECT_EQ(created_task->stop_calls, 1);
+}
+
+TEST_F(TaskManagerLifecycleTest, SuccessfulStartAndStopAreReported)
+{
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+
+    ASSERT_TRUE(manager.initialized());
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_TRUE(manager.startRunTask());
+    EXPECT_TRUE(manager.running());
+    manager.stopRunTask();
+    EXPECT_FALSE(manager.running());
+    EXPECT_EQ(created_task->start_calls, 1);
+    EXPECT_EQ(created_task->stop_calls, 1);
+}
+
+} // namespace

cmvr-es/runtime/CMakeLists.txt

@@ -13,8 +13,39 @@ target_link_libraries(cmvr_runtime PUBLIC
         cmvr_es::task_manager
         cmvr_es::service
         cmvr_es::quic_edge_task
+        cmvr_es::ume_teleop_task
         cmvr_es::mujoco_viewer
 )
 
 add_library(cmvr_es::runtime ALIAS cmvr_runtime)
 install(TARGETS cmvr_runtime LIBRARY DESTINATION lib ARCHIVE DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(runtime_lifecycle_test
+            tests/runtime_lifecycle_test.cpp
+    )
+    target_compile_features(runtime_lifecycle_test PRIVATE cxx_std_17)
+    target_include_directories(runtime_lifecycle_test PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(runtime_lifecycle_test PRIVATE
+            cmvr_es::runtime
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME runtime_lifecycle_test
+            COMMAND runtime_lifecycle_test
+    )
+    set(_runtime_lifecycle_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _runtime_lifecycle_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(runtime_lifecycle_test PROPERTIES
+            TIMEOUT 20
+            ENVIRONMENT "${_runtime_lifecycle_test_environment}"
+    )
+endif()

cmvr-es/runtime/src/cmvr_runtime.cpp

@@ -12,6 +12,7 @@
 #include "common/io/proto_file_io.h"
 #include "task/grpc_server_task/include/grpc_server_task.h"
 #include "task/quic_edge_task/include/quic_edge_task.h"
+#include "task/ume_teleop_task/include/ume_teleop_task.h"
 
 namespace cmvr {
 namespace {
@@ -100,13 +101,27 @@ bool Runtime::init_(const std::string& config_path,
         return false;
     }
     CMVR_LOG(INFO) << "[Startup] Initialize DeviceManager";
-    device::DeviceManager::getInstance(device_manager_root.device_manager());
+    auto& device_manager =
+        device::DeviceManager::getInstance(
+            device_manager_root.device_manager());
+    if (!device_manager.initialized()) {
+        CMVR_LOG(ERROR) << "[Startup] DeviceManager initialization failed";
+        device_manager.stop();
+        device::DeviceManager::destroyInstance();
+        return false;
+    }
+    const auto rollback_device_manager = [&device_manager]() {
+        device_manager.stop();
+        device::DeviceManager::destroyInstance();
+    };
 
     task::registerGrpcServerTaskFactory();
     task::registerQuicEdgeTaskFactory();
+    task::registerUmeTeleopTaskFactory();
 
     if (app_config.task_manager_config_file().empty()) {
         CMVR_LOG(ERROR) << "TaskManager config file is empty";
+        rollback_device_manager();
         return false;
     }
     logSection("TaskManager");
@@ -116,11 +131,19 @@ bool Runtime::init_(const std::string& config_path,
     if (!ConfigHelper::loadConfigFile(app_config.task_manager_config_file(), task_manager_root)) {
         CMVR_LOG(ERROR) << "Failed to load TaskManager config: "
                         << app_config.task_manager_config_file();
+        rollback_device_manager();
         return false;
     }
 
     CMVR_LOG(INFO) << "[Startup] Initialize TaskManager";
-    task::TaskManager::getInstance(task_manager_root.task_manager());
+    auto& task_manager =
+        task::TaskManager::getInstance(task_manager_root.task_manager());
+    if (!task_manager.initialized()) {
+        CMVR_LOG(ERROR) << "[Startup] TaskManager initialization failed";
+        task::TaskManager::destroyInstance();
+        rollback_device_manager();
+        return false;
+    }
     initialized_ = true;
     return true;
 }
@@ -134,9 +157,26 @@ bool Runtime::startTasks(const double control_period_s)
         return true;
     }
 
+    // Device init() constructs and validates resources; start() owns worker
+    // threads. Start devices before any task can publish commands or sample
+    // them. UME start remains passive and never enables actuators.
+    logSection("Start Devices");
+    CMVR_LOG(INFO) << "[Startup] Start devices";
+    if (!device::DeviceManager::getInstance().start()) {
+        CMVR_LOG(ERROR) << "[Startup] One or more enabled devices failed "
+                           "to start; tasks will not be started";
+        return false;
+    }
+
     logSection("Start Tasks");
     CMVR_LOG(INFO) << "[Startup] Start tasks";
-    task::TaskManager::getInstance().startRunTask(control_period_s);
+    if (!task::TaskManager::getInstance().startRunTask(control_period_s)) {
+        CMVR_LOG(ERROR) << "[Startup] One or more enabled tasks failed "
+                           "to start; stopping devices";
+        device::DeviceManager::getInstance().stop();
+        tasks_started_ = false;
+        return false;
+    }
     tasks_started_ = true;
     return true;
 }

cmvr-es/runtime/tests/runtime_lifecycle_test.cpp

@@ -0,0 +1,266 @@
+#include "runtime/include/cmvr_runtime.h"
+
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <algorithm>
+#include <array>
+#include <cstdlib>
+#include <filesystem>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "cmvr/config/cmvr_es_config/cmvr_es_config.pb.h"
+#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
+#include "cmvr/config/logger_config/logger_config.pb.h"
+#include "cmvr/config/task_manager_config/task_manager_config.pb.h"
+#include "common/io/proto_file_io.h"
+
+namespace {
+
+class TempConfigTree {
+public:
+    TempConfigTree()
+    {
+        std::array<char, 64> pattern{};
+        const std::string value =
+            "/tmp/cmvr-runtime-lifecycle-XXXXXX";
+        std::copy(value.begin(), value.end(), pattern.begin());
+        char* created = ::mkdtemp(pattern.data());
+        if (created) {
+            root_ = created;
+        }
+    }
+
+    ~TempConfigTree()
+    {
+        if (root_.empty()) {
+            return;
+        }
+        std::error_code error;
+        std::filesystem::remove_all(root_, error);
+    }
+
+    bool valid() const { return !root_.empty(); }
+
+    template <typename Message>
+    bool write(const std::string& name, const Message& message) const
+    {
+        return ProtoMessageIo::setProtoToAsciiFile(
+            message, (root_ / name).string());
+    }
+
+    std::string path(const std::string& name) const
+    {
+        return (root_ / name).string();
+    }
+
+    bool writeLogger() const
+    {
+        cmvr::config::LoggerRootConfig root;
+        auto* logger = root.mutable_logger();
+        logger->set_minimum_level(
+            cmvr::config::LOG_LEVEL_INFO);
+        auto* route = logger->add_routes();
+        route->set_level(cmvr::config::LOG_LEVEL_INFO);
+        route->set_terminal(false);
+        route->set_file(false);
+        return write("logger.pb.txt", root);
+    }
+
+    bool writeRoot() const
+    {
+        cmvr::config::CMVRESRootConfig root;
+        auto* config = root.mutable_cmvr_es();
+        config->set_logger_config_file("logger.pb.txt");
+        config->set_device_manager_config_file(
+            "device_manager.pb.txt");
+        config->set_task_manager_config_file(
+            "task_manager.pb.txt");
+        return write("cmvr_es.pb.txt", root);
+    }
+
+private:
+    std::filesystem::path root_;
+};
+
+class OccupiedTcpPort {
+public:
+    OccupiedTcpPort()
+    {
+        fd_ = ::socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0);
+        if (fd_ < 0) {
+            return;
+        }
+
+        sockaddr_in address{};
+        address.sin_family = AF_INET;
+        address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+        address.sin_port = 0;
+        if (::bind(fd_, reinterpret_cast<sockaddr*>(&address),
+                   sizeof(address)) != 0 ||
+            ::listen(fd_, 1) != 0) {
+            ::close(fd_);
+            fd_ = -1;
+            return;
+        }
+
+        socklen_t length = sizeof(address);
+        if (::getsockname(fd_,
+                          reinterpret_cast<sockaddr*>(&address),
+                          &length) != 0) {
+            ::close(fd_);
+            fd_ = -1;
+            return;
+        }
+        port_ = ntohs(address.sin_port);
+    }
+
+    ~OccupiedTcpPort()
+    {
+        if (fd_ >= 0) {
+            ::close(fd_);
+        }
+    }
+
+    bool valid() const { return fd_ >= 0 && port_ != 0; }
+    std::string port() const { return std::to_string(port_); }
+
+private:
+    int fd_{-1};
+    unsigned short port_{0};
+};
+
+TEST(RuntimeLifecycleTest, EnabledDeviceInitFailureFailsRuntimeInit)
+{
+    TempConfigTree tree;
+    ASSERT_TRUE(tree.valid());
+    ASSERT_TRUE(tree.writeLogger());
+    ASSERT_TRUE(tree.writeRoot());
+
+    cmvr::config::DeviceManagerRootConfig devices;
+    auto* entry =
+        devices.mutable_device_manager()->add_devices();
+    entry->set_id("unsupported");
+    entry->set_type(
+        cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN);
+    entry->set_enable(true);
+    ASSERT_TRUE(tree.write("device_manager.pb.txt", devices));
+
+    cmvr::config::TaskManagerRootConfig tasks;
+    ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks));
+
+    cmvr::Runtime runtime;
+    EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt")));
+    EXPECT_FALSE(runtime.initialized());
+    EXPECT_FALSE(runtime.tasksStarted());
+}
+
+TEST(RuntimeLifecycleTest, EnabledTaskInitFailureFailsRuntimeInit)
+{
+    TempConfigTree tree;
+    ASSERT_TRUE(tree.valid());
+    ASSERT_TRUE(tree.writeLogger());
+    ASSERT_TRUE(tree.writeRoot());
+
+    cmvr::config::DeviceManagerRootConfig devices;
+    ASSERT_TRUE(tree.write("device_manager.pb.txt", devices));
+
+    cmvr::config::TaskManagerRootConfig tasks;
+    auto* entry = tasks.mutable_task_manager()->add_tasks();
+    entry->set_id("unsupported");
+    entry->set_type(
+        cmvr::config::TaskConfigEntry::TASK_TYPE_UNKNOWN);
+    entry->set_enable(true);
+    entry->set_run_mode(
+        cmvr::config::TaskConfigEntry::
+            TASK_RUN_MODE_BLOCKING_SERVICE);
+    ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks));
+
+    cmvr::Runtime runtime;
+    EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt")));
+    EXPECT_FALSE(runtime.initialized());
+    EXPECT_FALSE(runtime.tasksStarted());
+}
+
+TEST(RuntimeLifecycleTest,
+     TaskConfigLoadFailureRollsBackDeviceManagerSingleton)
+{
+    TempConfigTree first_tree;
+    ASSERT_TRUE(first_tree.valid());
+    ASSERT_TRUE(first_tree.writeLogger());
+    ASSERT_TRUE(first_tree.writeRoot());
+
+    cmvr::config::DeviceManagerRootConfig first_devices;
+    first_devices.mutable_device_manager()->set_name("first");
+    ASSERT_TRUE(first_tree.write(
+        "device_manager.pb.txt", first_devices));
+    // Deliberately do not create task_manager.pb.txt.
+
+    cmvr::Runtime runtime;
+    ASSERT_FALSE(
+        runtime.init(first_tree.path("cmvr_es.pb.txt")));
+    ASSERT_FALSE(runtime.initialized());
+
+    TempConfigTree second_tree;
+    ASSERT_TRUE(second_tree.valid());
+    ASSERT_TRUE(second_tree.writeLogger());
+    ASSERT_TRUE(second_tree.writeRoot());
+
+    cmvr::config::DeviceManagerRootConfig second_devices;
+    second_devices.mutable_device_manager()->set_name("second");
+    ASSERT_TRUE(second_tree.write(
+        "device_manager.pb.txt", second_devices));
+    cmvr::config::TaskManagerRootConfig second_tasks;
+    ASSERT_TRUE(second_tree.write(
+        "task_manager.pb.txt", second_tasks));
+
+    ASSERT_TRUE(
+        runtime.init(second_tree.path("cmvr_es.pb.txt")));
+    EXPECT_EQ(runtime.deviceManager().name(), "second");
+}
+
+TEST(RuntimeLifecycleTest, GrpcBindFailureDoesNotMarkTasksStarted)
+{
+    OccupiedTcpPort occupied_port;
+    ASSERT_TRUE(occupied_port.valid());
+
+    TempConfigTree tree;
+    ASSERT_TRUE(tree.valid());
+    ASSERT_TRUE(tree.writeLogger());
+    ASSERT_TRUE(tree.writeRoot());
+
+    cmvr::config::DeviceManagerRootConfig devices;
+    ASSERT_TRUE(tree.write("device_manager.pb.txt", devices));
+
+    cmvr::config::GRPCServerRootConfig grpc;
+    auto* grpc_config = grpc.mutable_grpc_server();
+    grpc_config->set_id("grpc_server");
+    grpc_config->set_host("127.0.0.1");
+    grpc_config->set_port(occupied_port.port());
+    ASSERT_TRUE(tree.write("grpc.pb.txt", grpc));
+
+    cmvr::config::TaskManagerRootConfig tasks;
+    auto* entry = tasks.mutable_task_manager()->add_tasks();
+    entry->set_id("grpc_server");
+    entry->set_type(
+        cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER);
+    entry->set_enable(true);
+    entry->set_run_mode(
+        cmvr::config::TaskConfigEntry::
+            TASK_RUN_MODE_BLOCKING_SERVICE);
+    entry->set_config_file("grpc.pb.txt");
+    ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks));
+
+    cmvr::Runtime runtime;
+    ASSERT_TRUE(runtime.init(tree.path("cmvr_es.pb.txt")));
+    EXPECT_FALSE(runtime.startTasks());
+    EXPECT_FALSE(runtime.tasksStarted());
+    EXPECT_FALSE(runtime.taskManager().running());
+}
+
+} // namespace

cmvr-es/service/CMakeLists.txt

@@ -7,6 +7,9 @@ add_library(service
         grpc/src/grpc_head_service.cpp
         grpc/src/grpc_dexhand_service.cpp
         grpc/src/grpc_arm_service.cpp
+        grpc/src/grpc_arm_teleop_service.cpp
+        grpc/src/grpc_robot_arm_teleop_backend.cpp
+        grpc/src/grpc_motor_service.cpp
         grpc/src/grpc_agv_service.cpp
         grpc/src/grpc_hlc_service.cpp
         ../task/grpc_server_task/src/grpc_server_task.cpp
@@ -17,6 +20,7 @@ target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(service PRIVATE
         cmvr_es::proto
         osqp
+        cmvr_es::control_authority
         cmvr_es::device_manager
         cmvr_es::task_manager
         cmvr_es::algorithms::controller
@@ -42,6 +46,131 @@ if(BUILD_TESTING)
             COMMAND grpc_camera_stream_policy_test
     )
     set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10)
+
+    add_executable(grpc_arm_teleop_service_test
+            grpc/tests/grpc_arm_teleop_service_test.cpp
+    )
+    target_include_directories(grpc_arm_teleop_service_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(grpc_arm_teleop_service_test
+            PRIVATE
+            service
+            cmvr_es::proto
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME grpc_arm_teleop_service_test
+            COMMAND grpc_arm_teleop_service_test
+    )
+    set(_grpc_arm_teleop_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _grpc_arm_teleop_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(grpc_arm_teleop_service_test PROPERTIES
+            TIMEOUT 20
+            ENVIRONMENT "${_grpc_arm_teleop_test_environment}"
+    )
+
+    add_executable(grpc_robot_arm_teleop_backend_test
+            grpc/tests/grpc_robot_arm_teleop_backend_test.cpp
+    )
+    target_include_directories(grpc_robot_arm_teleop_backend_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(grpc_robot_arm_teleop_backend_test
+            PRIVATE
+            service
+            cmvr_es::proto
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME grpc_robot_arm_teleop_backend_test
+            COMMAND grpc_robot_arm_teleop_backend_test
+    )
+    set_tests_properties(grpc_robot_arm_teleop_backend_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_grpc_arm_teleop_test_environment}"
+    )
+
+    add_executable(grpc_motor_service_test
+            grpc/tests/grpc_motor_service_test.cpp
+    )
+    target_include_directories(grpc_motor_service_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+            ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager
+    )
+    target_link_libraries(grpc_motor_service_test
+            PRIVATE
+            service
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME grpc_motor_service_test
+            COMMAND grpc_motor_service_test
+    )
+    set(_grpc_motor_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _grpc_motor_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(grpc_motor_service_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_grpc_motor_test_environment}"
+    )
+
+    set(_grpc_motor_modbus_e2e_libmodbus_root
+            "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11")
+    add_executable(grpc_motor_service_modbus_e2e_test
+            grpc/tests/grpc_motor_service_modbus_e2e_test.cpp
+    )
+    target_include_directories(grpc_motor_service_modbus_e2e_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+            ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager
+            ${_grpc_motor_modbus_e2e_libmodbus_root}/include
+    )
+    target_link_directories(grpc_motor_service_modbus_e2e_test
+            PRIVATE
+            ${_grpc_motor_modbus_e2e_libmodbus_root}/lib
+    )
+    target_link_libraries(grpc_motor_service_modbus_e2e_test
+            PRIVATE
+            service
+            modbus
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME grpc_motor_service_modbus_e2e_test
+            COMMAND grpc_motor_service_modbus_e2e_test
+    )
+    set(_grpc_motor_modbus_e2e_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _grpc_motor_modbus_e2e_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(grpc_motor_service_modbus_e2e_test PROPERTIES
+            TIMEOUT 20
+            ENVIRONMENT "${_grpc_motor_modbus_e2e_environment}"
+    )
 endif()
 
 # --------------------------------------------------------

cmvr-es/service/arm_teleop_client/CMakeLists.txt

@@ -0,0 +1,38 @@
+find_package(Threads REQUIRED)
+
+add_library(arm_teleop_client STATIC
+        src/grpc_arm_teleop_client.cpp
+)
+target_compile_features(arm_teleop_client PUBLIC cxx_std_17)
+target_include_directories(arm_teleop_client PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es)
+target_link_libraries(arm_teleop_client
+        PUBLIC
+        cmvr_es::proto
+        PRIVATE
+        Threads::Threads
+)
+
+add_library(cmvr_es::arm_teleop_client ALIAS arm_teleop_client)
+install(TARGETS arm_teleop_client ARCHIVE DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(grpc_arm_teleop_client_test
+            tests/grpc_arm_teleop_client_test.cpp
+    )
+    target_compile_features(grpc_arm_teleop_client_test PRIVATE cxx_std_17)
+    target_link_libraries(grpc_arm_teleop_client_test
+            PRIVATE
+            cmvr_es::arm_teleop_client
+            Threads::Threads
+    )
+    add_test(NAME grpc_arm_teleop_client_test COMMAND grpc_arm_teleop_client_test)
+    set(_arm_teleop_client_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _arm_teleop_client_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(grpc_arm_teleop_client_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_arm_teleop_client_test_environment}")
+endif()

cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h

@@ -0,0 +1,80 @@
+#ifndef CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H
+#define CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H
+
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <mutex>
+
+#include <grpcpp/channel.h>
+#include <grpcpp/client_context.h>
+#include <grpcpp/support/status.h>
+#include <grpcpp/support/sync_stream.h>
+
+#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
+
+namespace cmvr::teleop {
+
+// One synchronous gRPC stream/session. Connection retry and worker ownership
+// belong to UmeTeleopTask; robot algorithms and kinematics belong to UME.
+class GrpcArmTeleopClient final {
+public:
+    using Api = api::armteleop::v1::ArmTeleopService;
+    using ClientFrame = api::armteleop::v1::ClientFrame;
+    using ServerFrame = api::armteleop::v1::ServerFrame;
+    using OpenSession = api::armteleop::v1::OpenSession;
+    using JointSetpoint = api::armteleop::v1::JointSetpoint;
+    using ClientHeartbeat = api::armteleop::v1::ClientHeartbeat;
+    using StopSession = api::armteleop::v1::StopSession;
+    using FrameCallback = std::function<void(const ServerFrame&)>;
+    using CancelPredicate = std::function<bool()>;
+
+    explicit GrpcArmTeleopClient(
+        std::shared_ptr<grpc::ChannelInterface> channel);
+    ~GrpcArmTeleopClient();
+
+    GrpcArmTeleopClient(const GrpcArmTeleopClient&) = delete;
+    GrpcArmTeleopClient& operator=(const GrpcArmTeleopClient&) = delete;
+
+    // Blocks until the peer closes the stream or tryCancel() is called. The
+    // OpenSession frame is always the first client frame.
+    grpc::Status runSession(const OpenSession& open_session,
+                            FrameCallback callback = {},
+                            CancelPredicate cancel_requested = {});
+
+    // These methods only transport already-computed protocol values.
+    bool sendSetpoint(const JointSetpoint& setpoint,
+                      std::uint64_t expected_session_generation = 0);
+    bool sendHeartbeat(const ClientHeartbeat& heartbeat,
+                       std::uint64_t expected_session_generation = 0);
+    bool sendStop(const StopSession& stop,
+                  std::uint64_t expected_session_generation = 0);
+
+    bool isSessionActive() const;
+    std::uint64_t activeSessionGeneration() const;
+
+    // Thread-safe and intentionally named after the gRPC primitive used. It
+    // interrupts a blocked Read/Write/Finish so the owning Task can join.
+    void tryCancel();
+
+private:
+    using Stream = grpc::ClientReaderWriterInterface<ClientFrame, ServerFrame>;
+
+    bool writeFrame(const ClientFrame& frame,
+                    std::uint64_t expected_session_generation);
+    void clearSession(const std::shared_ptr<grpc::ClientContext>& context,
+                      const std::shared_ptr<Stream>& stream);
+
+    std::unique_ptr<Api::StubInterface> stub_;
+
+    mutable std::mutex lifecycle_mutex_;
+    std::mutex write_mutex_;
+    std::shared_ptr<grpc::ClientContext> active_context_;
+    std::shared_ptr<Stream> active_stream_;
+    std::uint64_t next_session_generation_{0};
+    std::uint64_t active_session_generation_{0};
+};
+
+} // namespace cmvr::teleop
+
+#endif // CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H

cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp

@@ -0,0 +1,223 @@
+#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h"
+
+#include <exception>
+#include <string>
+#include <utility>
+
+namespace cmvr::teleop {
+namespace {
+
+grpc::Status clientStatus(const grpc::StatusCode code, const char* detail)
+{
+    return grpc::Status(code, detail);
+}
+
+} // namespace
+
+GrpcArmTeleopClient::GrpcArmTeleopClient(
+    std::shared_ptr<grpc::ChannelInterface> channel)
+{
+    if (channel) {
+        stub_ = Api::NewStub(channel);
+    }
+}
+
+GrpcArmTeleopClient::~GrpcArmTeleopClient()
+{
+    tryCancel();
+}
+
+grpc::Status GrpcArmTeleopClient::runSession(
+    const OpenSession& open_session,
+    FrameCallback callback,
+    CancelPredicate cancel_requested)
+{
+    if (!stub_) {
+        return clientStatus(
+            grpc::StatusCode::FAILED_PRECONDITION,
+            "arm teleop client has no channel");
+    }
+    if (cancel_requested && cancel_requested()) {
+        return clientStatus(
+            grpc::StatusCode::CANCELLED,
+            "arm teleop session cancelled before start");
+    }
+
+    auto context = std::make_shared<grpc::ClientContext>();
+    {
+        std::lock_guard lock(lifecycle_mutex_);
+        if (active_context_) {
+            return clientStatus(
+                grpc::StatusCode::ALREADY_EXISTS,
+                "arm teleop session is already active");
+        }
+        // Publish the context before opening/writing the stream so tryCancel()
+        // can interrupt every blocking phase of the synchronous RPC.
+        active_context_ = context;
+    }
+    // Closes the small race where the owning Task requests stop immediately
+    // before active_context_ becomes visible to tryCancel().
+    if (cancel_requested && cancel_requested()) {
+        context->TryCancel();
+    }
+
+    auto unique_stream = stub_->Teleoperate(context.get());
+    if (!unique_stream) {
+        clearSession(context, {});
+        return clientStatus(
+            grpc::StatusCode::UNAVAILABLE,
+            "failed to create arm teleop stream");
+    }
+    auto stream = std::shared_ptr<Stream>(std::move(unique_stream));
+
+    ClientFrame first_frame;
+    *first_frame.mutable_open() = open_session;
+    {
+        std::lock_guard write_lock(write_mutex_);
+        if (!stream->Write(first_frame)) {
+            const grpc::Status status = stream->Finish();
+            clearSession(context, stream);
+            return status.ok()
+                       ? clientStatus(
+                             grpc::StatusCode::UNAVAILABLE,
+                             "peer closed before OpenSession was written")
+                       : status;
+        }
+    }
+
+    {
+        std::lock_guard lock(lifecycle_mutex_);
+        // Cancellation can race the initial Write. Keeping the stream visible
+        // is safe; subsequent writes will fail and runSession will clean it.
+        if (active_context_ == context) {
+            active_stream_ = stream;
+            active_session_generation_ = ++next_session_generation_;
+        }
+    }
+
+    bool callback_failed = false;
+    std::string callback_error;
+    ServerFrame frame;
+    while (stream->Read(&frame)) {
+        if (!callback) {
+            continue;
+        }
+        try {
+            callback(frame);
+        } catch (const std::exception& error) {
+            callback_failed = true;
+            callback_error = error.what();
+            context->TryCancel();
+            break;
+        } catch (...) {
+            callback_failed = true;
+            callback_error = "server-frame callback raised an unknown exception";
+            context->TryCancel();
+            break;
+        }
+    }
+
+    {
+        std::lock_guard write_lock(write_mutex_);
+        stream->WritesDone();
+    }
+    const grpc::Status status = stream->Finish();
+    clearSession(context, stream);
+
+    if (callback_failed) {
+        return grpc::Status(
+            grpc::StatusCode::INTERNAL,
+            "arm teleop callback failed: " + callback_error);
+    }
+    return status;
+}
+
+bool GrpcArmTeleopClient::sendSetpoint(
+    const JointSetpoint& setpoint,
+    const std::uint64_t expected_session_generation)
+{
+    ClientFrame frame;
+    *frame.mutable_setpoint() = setpoint;
+    return writeFrame(frame, expected_session_generation);
+}
+
+bool GrpcArmTeleopClient::sendHeartbeat(
+    const ClientHeartbeat& heartbeat,
+    const std::uint64_t expected_session_generation)
+{
+    ClientFrame frame;
+    *frame.mutable_heartbeat() = heartbeat;
+    return writeFrame(frame, expected_session_generation);
+}
+
+bool GrpcArmTeleopClient::sendStop(
+    const StopSession& stop,
+    const std::uint64_t expected_session_generation)
+{
+    ClientFrame frame;
+    *frame.mutable_stop() = stop;
+    return writeFrame(frame, expected_session_generation);
+}
+
+bool GrpcArmTeleopClient::isSessionActive() const
+{
+    std::lock_guard lock(lifecycle_mutex_);
+    return active_stream_ != nullptr;
+}
+
+std::uint64_t GrpcArmTeleopClient::activeSessionGeneration() const
+{
+    std::lock_guard lock(lifecycle_mutex_);
+    return active_session_generation_;
+}
+
+void GrpcArmTeleopClient::tryCancel()
+{
+    std::shared_ptr<grpc::ClientContext> context;
+    {
+        std::lock_guard lock(lifecycle_mutex_);
+        context = active_context_;
+    }
+    if (context) {
+        context->TryCancel();
+    }
+}
+
+bool GrpcArmTeleopClient::writeFrame(
+    const ClientFrame& frame,
+    const std::uint64_t expected_session_generation)
+{
+    std::shared_ptr<Stream> stream;
+    {
+        std::lock_guard lock(lifecycle_mutex_);
+        if (expected_session_generation != 0 &&
+            expected_session_generation != active_session_generation_) {
+            return false;
+        }
+        stream = active_stream_;
+    }
+    if (!stream) {
+        return false;
+    }
+
+    // gRPC permits one read and one write concurrently, but concurrent writes
+    // must be serialized by the application.
+    std::lock_guard write_lock(write_mutex_);
+    return stream->Write(frame);
+}
+
+void GrpcArmTeleopClient::clearSession(
+    const std::shared_ptr<grpc::ClientContext>& context,
+    const std::shared_ptr<Stream>& stream)
+{
+    std::lock_guard lock(lifecycle_mutex_);
+    if (active_context_ == context) {
+        active_context_.reset();
+    }
+    if (!stream || active_stream_ == stream) {
+        active_stream_.reset();
+        active_session_generation_ = 0;
+    }
+}
+
+} // namespace cmvr::teleop

cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp

@@ -0,0 +1,220 @@
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdio>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include <grpcpp/grpcpp.h>
+#include <unistd.h>
+
+#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
+#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h"
+
+namespace {
+
+using namespace std::chrono_literals;
+namespace api = cmvr::api::armteleop::v1;
+
+class TestArmTeleopService final : public api::ArmTeleopService::Service {
+public:
+    grpc::Status Teleoperate(
+        grpc::ServerContext*,
+        grpc::ServerReaderWriter<api::ServerFrame, api::ClientFrame>* stream) override
+    {
+        api::ClientFrame frame;
+        if (!stream->Read(&frame) || !frame.has_open()) {
+            return grpc::Status(
+                grpc::StatusCode::INVALID_ARGUMENT,
+                "OpenSession must be first");
+        }
+
+        {
+            std::lock_guard lock(mutex_);
+            open_received_ = true;
+        }
+        condition_.notify_all();
+
+        api::ServerFrame opened;
+        opened.mutable_status()->set_session_id("client-test-session");
+        opened.mutable_status()->set_phase(api::SESSION_PHASE_OPENED);
+        if (!stream->Write(opened)) {
+            return grpc::Status::OK;
+        }
+
+        while (stream->Read(&frame)) {
+            if (frame.has_heartbeat()) {
+                heartbeat_received_.store(true);
+                condition_.notify_all();
+            }
+        }
+        handler_finished_.store(true);
+        condition_.notify_all();
+        return grpc::Status::OK;
+    }
+
+    bool waitForOpen(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(lock, timeout, [this] { return open_received_; });
+    }
+
+    bool waitForHeartbeat(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [this] { return heartbeat_received_.load(); });
+    }
+
+    bool waitForHandlerFinish(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [this] { return handler_finished_.load(); });
+    }
+
+private:
+    std::mutex mutex_;
+    std::condition_variable condition_;
+    bool open_received_{false};
+    std::atomic<bool> heartbeat_received_{false};
+    std::atomic<bool> handler_finished_{false};
+};
+
+int fail(const std::string& detail)
+{
+    std::cerr << "grpc_arm_teleop_client_test: " << detail << '\n';
+    return 1;
+}
+
+} // namespace
+
+int main()
+{
+    TestArmTeleopService service;
+    const std::string socket_path =
+        "/tmp/cmvr_arm_teleop_client_test_" +
+        std::to_string(static_cast<long long>(::getpid())) + ".sock";
+    std::remove(socket_path.c_str());
+    const std::string endpoint = "unix:" + socket_path;
+
+    grpc::ServerBuilder builder;
+    builder.AddListeningPort(
+        endpoint,
+        grpc::InsecureServerCredentials());
+    builder.RegisterService(&service);
+    std::unique_ptr<grpc::Server> server = builder.BuildAndStart();
+    if (!server) {
+        return fail("failed to start in-process gRPC server");
+    }
+
+    auto channel = grpc::CreateChannel(
+        endpoint,
+        grpc::InsecureChannelCredentials());
+    cmvr::teleop::GrpcArmTeleopClient client(channel);
+
+    api::OpenSession open;
+    open.set_protocol_major(1);
+    open.set_protocol_minor(0);
+    open.set_client_instance_id("grpc-client-test");
+    open.mutable_expected_robot()->set_robot_id("test-arm");
+    open.set_watchdog_timeout_ms(100);
+    open.set_requested_lease_ms(500);
+
+    std::mutex frame_mutex;
+    std::condition_variable frame_condition;
+    bool opened_received = false;
+    grpc::Status session_status;
+    std::thread session_thread([&] {
+        session_status = client.runSession(
+            open,
+            [&](const api::ServerFrame& frame) {
+                if (frame.has_status() &&
+                    frame.status().phase() == api::SESSION_PHASE_OPENED) {
+                    {
+                        std::lock_guard lock(frame_mutex);
+                        opened_received = true;
+                    }
+                    frame_condition.notify_all();
+                }
+            });
+    });
+
+    if (!service.waitForOpen(2s)) {
+        client.tryCancel();
+        session_thread.join();
+        server->Shutdown();
+        return fail("server did not receive OpenSession");
+    }
+    {
+        std::unique_lock lock(frame_mutex);
+        if (!frame_condition.wait_for(lock, 2s, [&] { return opened_received; })) {
+            client.tryCancel();
+            session_thread.join();
+            server->Shutdown();
+            return fail("client did not receive OPENED status");
+        }
+    }
+    if (!client.isSessionActive()) {
+        client.tryCancel();
+        session_thread.join();
+        server->Shutdown();
+        return fail("client did not expose an active session");
+    }
+    const std::uint64_t generation =
+        client.activeSessionGeneration();
+    if (generation == 0) {
+        client.tryCancel();
+        session_thread.join();
+        server->Shutdown();
+        return fail("active stream did not expose a session generation");
+    }
+
+    api::ClientHeartbeat heartbeat;
+    heartbeat.set_sequence(1);
+    if (client.sendHeartbeat(heartbeat, generation + 1)) {
+        client.tryCancel();
+        session_thread.join();
+        server->Shutdown();
+        return fail("stale session generation was allowed to write");
+    }
+    if (!client.sendHeartbeat(heartbeat, generation) ||
+        !service.waitForHeartbeat(2s)) {
+        client.tryCancel();
+        session_thread.join();
+        server->Shutdown();
+        return fail("heartbeat did not traverse the active stream");
+    }
+
+    const auto cancel_begin = std::chrono::steady_clock::now();
+    client.tryCancel();
+    session_thread.join();
+    const auto cancel_elapsed = std::chrono::steady_clock::now() - cancel_begin;
+
+    if (cancel_elapsed > 2s) {
+        server->Shutdown();
+        return fail("TryCancel did not unblock and join the session promptly");
+    }
+    if (session_status.error_code() != grpc::StatusCode::CANCELLED) {
+        server->Shutdown();
+        return fail(
+            "cancelled session returned unexpected status: " +
+            std::to_string(session_status.error_code()));
+    }
+    if (client.isSessionActive()) {
+        server->Shutdown();
+        return fail("client retained an active stream after cancellation");
+    }
+    if (!service.waitForHandlerFinish(2s)) {
+        server->Shutdown();
+        return fail("server handler did not observe client cancellation");
+    }
+
+    server->Shutdown();
+    std::remove(socket_path.c_str());
+    std::cout << "grpc_arm_teleop_client_test: PASS\n";
+    return 0;
+}

cmvr-es/service/grpc/include/grpc_arm_teleop_service.h

@@ -0,0 +1,91 @@
+#pragma once
+
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include <grpcpp/grpcpp.h>
+
+#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
+#include "manager/control_authority/include/control_authority_manager.h"
+
+namespace cmvr::service {
+
+namespace arm_teleop = cmvr::api::armteleop::v1;
+
+struct ArmTeleopBackendResult {
+    bool success{false};
+    grpc::StatusCode status_code{grpc::StatusCode::INTERNAL};
+    std::string detail;
+
+    static ArmTeleopBackendResult ok()
+    {
+        return {true, grpc::StatusCode::OK, {}};
+    }
+
+    static ArmTeleopBackendResult failure(
+        const grpc::StatusCode code,
+        std::string message)
+    {
+        return {false, code, std::move(message)};
+    }
+};
+
+struct ArmTeleopBackendSnapshot {
+    arm_teleop::JointState joint_state;
+    arm_teleop::RobotSafetyState safety;
+};
+
+// Execution boundary for ArmTeleopService. The first implementation registers a
+// disabled backend in production and injects a fake backend in tests. A future
+// RobotArm adapter must live behind this interface so the gRPC reader thread can
+// remain a bounded mailbox producer and never touch hardware. Implementations
+// must keep every call bounded and non-blocking with respect to hardware I/O;
+// snapshot() must return cached state rather than synchronously polling a bus.
+class ArmTeleopBackend {
+public:
+    virtual ~ArmTeleopBackend() = default;
+
+    virtual bool available() const noexcept = 0;
+    virtual std::string unavailableReason() const { return {}; }
+    virtual arm_teleop::RobotManifest manifest() const = 0;
+    virtual bool supportsForceFeedback() const noexcept = 0;
+    virtual ArmTeleopBackendResult open(
+        const arm_teleop::OpenSession& request) = 0;
+    // The deadline is computed from the receiver's local monotonic clock.
+    // Implementations must re-check it immediately before committing a
+    // hardware command; the protobuf valid_for duration is never interpreted
+    // as a cross-machine absolute timestamp.
+    virtual ArmTeleopBackendResult applySetpoint(
+        const arm_teleop::JointSetpoint& setpoint,
+        std::chrono::steady_clock::time_point deadline) = 0;
+    virtual ArmTeleopBackendResult stop(
+        arm_teleop::StopReason reason,
+        const std::string& detail) = 0;
+    virtual ArmTeleopBackendSnapshot snapshot() const = 0;
+};
+
+std::shared_ptr<ArmTeleopBackend> makeDisabledArmTeleopBackend();
+
+class ArmTeleopServiceImpl final
+    : public arm_teleop::ArmTeleopService::Service {
+public:
+    explicit ArmTeleopServiceImpl(
+        std::shared_ptr<ArmTeleopBackend> backend =
+            makeDisabledArmTeleopBackend(),
+        control::ControlAuthorityManager* authority = nullptr);
+    ~ArmTeleopServiceImpl() override = default;
+
+    grpc::Status Teleoperate(
+        grpc::ServerContext* context,
+        grpc::ServerReaderWriter<arm_teleop::ServerFrame,
+                                 arm_teleop::ClientFrame>* stream) override;
+
+private:
+    std::shared_ptr<ArmTeleopBackend> backend_;
+    control::ControlAuthorityManager* authority_{nullptr};
+};
+
+} // namespace cmvr::service

cmvr-es/service/grpc/include/grpc_motor_service.h

@@ -0,0 +1,197 @@
+#pragma once
+
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <unordered_map>
+
+#include "cmvr/api/motor_service.grpc.pb.h"
+#include "devices/motor/abstract_motor.h"
+
+namespace cmvr::device {
+class DeviceManager;
+}
+
+namespace cmvr::service {
+
+class gRPCMotorServiceImplTestAccess;
+
+// A deliberately thin synchronous gRPC facade over AbstractMotor. It does not
+// schedule trajectories or retain asynchronous operations. The small amount of
+// state below only prevents two RPCs from owning one motor at the same time and
+// lets emergencyStop invalidate an already-running blocking RPC/stream.
+class gRPCMotorServiceImpl final : public api::MotorService::Service {
+public:
+    gRPCMotorServiceImpl();
+    ~gRPCMotorServiceImpl() override = default;
+
+    grpc::Status setZero(grpc::ServerContext* context,
+                         const api::SetMotorZeroRequest* request,
+                         api::MotorCommandResponse* response) override;
+    grpc::Status moveToZero(grpc::ServerContext* context,
+                            const api::MoveMotorToZeroRequest* request,
+                            api::MotorCommandResponse* response) override;
+    grpc::Status profilePosition(grpc::ServerContext* context,
+                                 const api::ProfilePositionRequest* request,
+                                 api::MotorCommandResponse* response) override;
+    grpc::Status profileVelocity(grpc::ServerContext* context,
+                                 const api::ProfileVelocityRequest* request,
+                                 api::MotorCommandResponse* response) override;
+    grpc::Status streamCyclicPosition(
+        grpc::ServerContext* context,
+        grpc::ServerReaderWriter<api::CyclicControlResponse,
+                                 api::CyclicPositionRequest>* stream) override;
+    grpc::Status streamCyclicVelocity(
+        grpc::ServerContext* context,
+        grpc::ServerReaderWriter<api::CyclicControlResponse,
+                                 api::CyclicVelocityRequest>* stream) override;
+    grpc::Status emergencyStop(grpc::ServerContext* context,
+                               const api::EmergencyStopRequest* request,
+                               api::MotorCommandResponse* response) override;
+    grpc::Status getStatus(grpc::ServerContext* context,
+                           const api::GetMotorStatusRequest* request,
+                           api::GetMotorStatusResponse* response) override;
+    grpc::Status setEnabled(grpc::ServerContext* context,
+                            const api::SetMotorEnabledRequest* request,
+                            api::MotorCommandResponse* response) override;
+
+private:
+    friend class gRPCMotorServiceImplTestAccess;
+
+    struct MotorControlState {
+        std::mutex mutex;
+        // Serializes all motion/enable writes with emergency quick-stop. The
+        // cancel-generation check and the corresponding motor write must occur
+        // while this mutex is held to prevent stale writes after an E-stop.
+        std::mutex command_mutex;
+        // Keeps the two-phase best-effort/final quick-stop sequence exclusive.
+        // Without this, one concurrent E-stop could clear the shared
+        // in-progress flag while another E-stop is still dispatching.
+        std::mutex emergency_mutex;
+        // Serializes exception cleanup from ownership inspection through the
+        // final release. A second stale cleanup must re-check ownership only
+        // after the first cleanup has fully completed.
+        std::mutex exception_cleanup_mutex;
+        bool busy{false};
+        bool emergency_stopped{false};
+        bool emergency_stop_in_progress{false};
+        bool exception_cleanup_pending{false};
+        std::uint64_t cancel_generation{0};
+        api::MotorControlType active_control{api::MOTOR_CONTROL_NONE};
+        std::string last_error;
+    };
+
+    struct ResolvedMotor {
+        std::shared_ptr<device::AbstractMotor> motor;
+        std::shared_ptr<MotorControlState> control;
+    };
+
+    struct MotorControlEntry {
+        std::weak_ptr<device::AbstractMotor> owner;
+        std::shared_ptr<MotorControlState> state;
+    };
+
+    class ControlLease {
+    public:
+        ControlLease(std::shared_ptr<MotorControlState> state,
+                     std::uint64_t generation);
+        ~ControlLease();
+        ControlLease(const ControlLease&) = delete;
+        ControlLease& operator=(const ControlLease&) = delete;
+
+        std::uint64_t generation() const noexcept { return generation_; }
+
+    private:
+        std::shared_ptr<MotorControlState> state_;
+        std::uint64_t generation_{0};
+        int uncaught_on_entry_{0};
+    };
+
+    grpc::Status resolveMotor(const api::MotorTarget& target,
+                              ResolvedMotor& resolved) const;
+    std::shared_ptr<MotorControlState> stateFor(
+        const std::shared_ptr<device::AbstractMotor>& motor) const;
+    std::unique_ptr<ControlLease> acquireControl(
+        const ResolvedMotor& resolved,
+        api::MotorControlType control,
+        grpc::Status& failure,
+        bool allow_emergency_stopped = false) const;
+
+    grpc::Status runProfilePosition(grpc::ServerContext* context,
+                                    const ResolvedMotor& resolved,
+                                    double target_position_rad,
+                                    double max_velocity_rad_s,
+                                    double acceleration_rad_s2,
+                                    const api::MotorWaitOptions& wait,
+                                    api::MotorCommandResponse* response);
+    grpc::Status waitForPosition(grpc::ServerContext* context,
+                                 const ResolvedMotor& resolved,
+                                 std::uint64_t generation,
+                                 double target_position_rad,
+                                 const api::MotorWaitOptions& wait,
+                                 api::MotorCommandResponse* response,
+                                 std::chrono::steady_clock::time_point started);
+    grpc::Status waitForVelocity(grpc::ServerContext* context,
+                                 const ResolvedMotor& resolved,
+                                 std::uint64_t generation,
+                                 double target_velocity_rad_s,
+                                 const api::MotorWaitOptions& wait,
+                                 api::MotorCommandResponse* response,
+                                 std::chrono::steady_clock::time_point started);
+
+    grpc::Status setZeroImpl(grpc::ServerContext* context,
+                             const api::SetMotorZeroRequest* request,
+                             api::MotorCommandResponse* response);
+    grpc::Status moveToZeroImpl(grpc::ServerContext* context,
+                                const api::MoveMotorToZeroRequest* request,
+                                api::MotorCommandResponse* response);
+    grpc::Status profilePositionImpl(
+        grpc::ServerContext* context,
+        const api::ProfilePositionRequest* request,
+        api::MotorCommandResponse* response);
+    grpc::Status profileVelocityImpl(
+        grpc::ServerContext* context,
+        const api::ProfileVelocityRequest* request,
+        api::MotorCommandResponse* response);
+    grpc::Status emergencyStopImpl(
+        grpc::ServerContext* context,
+        const api::EmergencyStopRequest* request,
+        api::MotorCommandResponse* response);
+    grpc::Status getStatusImpl(grpc::ServerContext* context,
+                               const api::GetMotorStatusRequest* request,
+                               api::GetMotorStatusResponse* response);
+    grpc::Status setEnabledImpl(
+        grpc::ServerContext* context,
+        const api::SetMotorEnabledRequest* request,
+        api::MotorCommandResponse* response);
+    grpc::Status streamCyclicPositionImpl(
+        grpc::ServerContext* context,
+        grpc::ServerReaderWriter<api::CyclicControlResponse,
+                                 api::CyclicPositionRequest>* stream,
+        std::optional<api::MotorTarget>& cleanup_target);
+    grpc::Status streamCyclicVelocityImpl(
+        grpc::ServerContext* context,
+        grpc::ServerReaderWriter<api::CyclicControlResponse,
+                                 api::CyclicVelocityRequest>* stream,
+        std::optional<api::MotorTarget>& cleanup_target);
+
+    void bestEffortQuickStop(const api::MotorTarget& target,
+                             const std::string& error) noexcept;
+    void latchUnsafeAfterFailedStop(
+        const std::shared_ptr<MotorControlState>& state,
+        const std::string& error) const;
+    void fillMotorStatus(const ResolvedMotor& resolved,
+                         api::MotorStatus* status) const;
+    void setLastError(const std::shared_ptr<MotorControlState>& state,
+                      const std::string& error) const;
+
+    device::DeviceManager& dmgr_;
+    mutable std::mutex states_mutex_;
+    mutable std::unordered_map<const device::AbstractMotor*,
+                               MotorControlEntry> states_;
+};
+
+} // namespace cmvr::service

cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <memory>
+
+#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
+#include "devices/arm/robot_arm.h"
+#include "service/grpc/include/grpc_arm_teleop_service.h"
+
+namespace cmvr::service {
+
+// Creates a fail-closed adapter from the process RobotArm abstraction to the
+// session-based ArmTeleop backend. available() remains false unless the config,
+// RobotModel and RobotArm capability all pass static validation.
+std::shared_ptr<ArmTeleopBackend> makeRobotArmTeleopBackend(
+    std::shared_ptr<device::RobotArm> arm,
+    const config::ArmTeleopBackendConfig& config);
+
+} // namespace cmvr::service

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

@@ -1,8 +1,13 @@
 #include "service/grpc/include/grpc_arm_service.h"
 
+#include <atomic>
+#include <chrono>
+#include <utility>
+
 #include <google/protobuf/util/time_util.h>
 
 #include "common/base/logging/logger.h"
+#include "manager/control_authority/include/control_authority_manager.h"
 
 using google::protobuf::util::TimeUtil;
 
@@ -119,6 +124,64 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
     return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
 }
 
+grpc::Status setControlLeaseConflict(
+    api::CommandHeader_Feedback* response,
+    const std::string& device_id)
+{
+    const std::string message =
+        "RobotArm control is leased by another active control operation: " +
+        device_id;
+    fillFeedback(response, false, message);
+    return grpc::Status(
+        grpc::StatusCode::FAILED_PRECONDITION, message);
+}
+
+template <typename Response>
+grpc::Status setControlLeaseConflict(
+    Response* response,
+    const std::string& device_id)
+{
+    return setControlLeaseConflict(
+        response->mutable_header(), device_id);
+}
+
+class ScopedUnaryControlLease final {
+public:
+    ScopedUnaryControlLease(
+        const std::string& device_id,
+        const char* operation)
+        : manager_(control::ControlAuthorityManager::instance())
+    {
+        static std::atomic<std::uint64_t> sequence{0};
+        const std::string owner =
+            std::string("grpc-arm-unary:") + operation + ":" +
+            std::to_string(
+                sequence.fetch_add(
+                    1U, std::memory_order_relaxed) +
+                1U);
+        auto acquired = manager_.tryAcquire(
+            device_id,
+            owner,
+            std::chrono::duration_cast<
+                control::ControlAuthorityManager::Duration>(
+                std::chrono::hours(24)));
+        acquired_ = acquired.acquired;
+        token_ = std::move(acquired.token);
+    }
+
+    ~ScopedUnaryControlLease()
+    {
+        manager_.release(token_);
+    }
+
+    bool acquired() const noexcept { return acquired_; }
+
+private:
+    control::ControlAuthorityManager& manager_;
+    control::ControlLeaseToken token_;
+    bool acquired_{false};
+};
+
 } // namespace
 
 gRPCArmServiceImpl::gRPCArmServiceImpl()
@@ -136,6 +199,10 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        // A safety-disable command preempts any network teleoperation lease.
+        // The teleoperation executor must fail its next renew before it can
+        // dispatch another setpoint.
+        control::ControlAuthorityManager::instance().revoke(device_id);
         const auto result = arm->torqueOff();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -158,6 +225,11 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "torqueOn");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->torqueOn();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -180,6 +252,11 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "moveJ");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->moveJ(toJointPositionCommand(request->target()),
                                        toMotionOptions(request->options()));
         if (result.ok()) {
@@ -203,6 +280,11 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "moveL");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->moveL(toCartesianPose(request->target()),
                                        toMotionOptions(request->options()),
                                        toFrameType(request->frame()));
@@ -227,6 +309,11 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "speedJ");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
                                         request->acceleration(),
                                         request->duration());
@@ -253,6 +340,11 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "speedL");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
                                         request->acceleration(),
                                         request->duration(),
@@ -280,6 +372,11 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "servoJ");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->servoJ(toJointPositionCommand(request->target()));
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id
@@ -302,6 +399,7 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        control::ControlAuthorityManager::instance().revoke(device_id);
         const auto result = arm->stopMotion();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -379,6 +477,11 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "calibrateZeroQ");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->calibrateZeroQ(request->joint_name());
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id
@@ -417,6 +520,11 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        ScopedUnaryControlLease control_lease(
+            device_id, "clearFault");
+        if (!control_lease.acquired()) {
+            return setControlLeaseConflict(response, device_id);
+        }
         const auto result = arm->clearFault();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         return resultToStatus(result);
@@ -426,4 +534,3 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
     }
 }
 } // namespace cmvr::service
-

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

@@ -0,0 +1,649 @@
+#include "service/grpc/include/grpc_robot_arm_teleop_backend.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cctype>
+#include <cstdint>
+#include <limits>
+#include <mutex>
+#include <sstream>
+#include <unordered_set>
+#include <utility>
+
+namespace cmvr::service {
+namespace {
+
+using Clock = std::chrono::steady_clock;
+
+constexpr double kMinimumServoPeriodS = 0.0001;
+constexpr double kMaximumServoPeriodS = 0.1;
+
+bool isDigest(const std::string& value)
+{
+    return value.size() == 64 &&
+           std::all_of(
+               value.begin(), value.end(), [](const unsigned char value) {
+                   return std::isxdigit(value) != 0;
+               });
+}
+
+arm_teleop::EffortSource toProtoEffortSource(
+    const device::JointEffortSource source)
+{
+    switch (source) {
+        case device::JointEffortSource::MotorEstimate:
+            return arm_teleop::EFFORT_SOURCE_MOTOR_ESTIMATE;
+        case device::JointEffortSource::JointSensor:
+            return arm_teleop::EFFORT_SOURCE_JOINT_SENSOR;
+        case device::JointEffortSource::ForceTorqueSensor:
+            return arm_teleop::EFFORT_SOURCE_FORCE_TORQUE_SENSOR;
+        case device::JointEffortSource::Observer:
+            return arm_teleop::EFFORT_SOURCE_OBSERVER;
+        case device::JointEffortSource::Unspecified:
+        default:
+            return arm_teleop::EFFORT_SOURCE_UNSPECIFIED;
+    }
+}
+
+grpc::StatusCode toStatusCode(const device::ArmErrorCode code)
+{
+    switch (code) {
+        case device::ArmErrorCode::InvalidArgument:
+        case device::ArmErrorCode::InvalidDof:
+            return grpc::StatusCode::INVALID_ARGUMENT;
+        case device::ArmErrorCode::OutOfJointLimit:
+        case device::ArmErrorCode::OutOfVelocityLimit:
+        case device::ArmErrorCode::OutOfAccelerationLimit:
+        case device::ArmErrorCode::OutOfWorkspace:
+            return grpc::StatusCode::OUT_OF_RANGE;
+        case device::ArmErrorCode::NotConnected:
+        case device::ArmErrorCode::RobotNotReady:
+        case device::ArmErrorCode::RobotNotPowered:
+        case device::ArmErrorCode::RobotInFault:
+        case device::ArmErrorCode::RobotInProtectiveStop:
+        case device::ArmErrorCode::RobotInEmergencyStop:
+        case device::ArmErrorCode::CommandRejected:
+        case device::ArmErrorCode::UnsupportedCommand:
+            return grpc::StatusCode::FAILED_PRECONDITION;
+        case device::ArmErrorCode::Timeout:
+            return grpc::StatusCode::DEADLINE_EXCEEDED;
+        case device::ArmErrorCode::ConnectionFailed:
+            return grpc::StatusCode::UNAVAILABLE;
+        case device::ArmErrorCode::AlreadyConnected:
+            return grpc::StatusCode::ALREADY_EXISTS;
+        case device::ArmErrorCode::CommandFailed:
+        case device::ArmErrorCode::UnknownError:
+        case device::ArmErrorCode::OK:
+        default:
+            return grpc::StatusCode::INTERNAL;
+    }
+}
+
+ArmTeleopBackendResult fromArmResult(
+    const device::Result& result,
+    const char* operation)
+{
+    if (result.ok()) {
+        return ArmTeleopBackendResult::ok();
+    }
+    std::string detail(operation);
+    detail += " failed";
+    if (!result.message.empty()) {
+        detail += ": " + result.message;
+    }
+    return ArmTeleopBackendResult::failure(
+        toStatusCode(result.code), std::move(detail));
+}
+
+class RobotArmTeleopBackend final : public ArmTeleopBackend {
+public:
+    RobotArmTeleopBackend(
+        std::shared_ptr<device::RobotArm> arm,
+        config::ArmTeleopBackendConfig config)
+        : arm_(std::move(arm)),
+          config_(std::move(config)),
+          require_powered_(
+              !config_.has_require_powered() ||
+              config_.require_powered())
+    {
+        validateStaticConfiguration();
+    }
+
+    bool available() const noexcept override
+    {
+        return unavailable_reason_.empty();
+    }
+
+    std::string unavailableReason() const override
+    {
+        return unavailable_reason_;
+    }
+
+    arm_teleop::RobotManifest manifest() const override
+    {
+        return manifest_;
+    }
+
+    bool supportsForceFeedback() const noexcept override
+    {
+        // A non-zero effort vector is not enough. The RobotArm must explicitly
+        // identify a verified effort source.
+        return available() && arm_->jointEffortSource() !=
+                                  device::JointEffortSource::Unspecified;
+    }
+
+    ArmTeleopBackendResult open(
+        const arm_teleop::OpenSession& request) override
+    {
+        std::lock_guard operation_lock(operation_mutex_);
+        if (!available()) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                unavailable_reason_);
+        }
+        if (session_open_) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::ALREADY_EXISTS,
+                "RobotArm teleoperation servo mode is already open");
+        }
+
+        const auto state = arm_->getRobotState();
+        cacheState(state);
+        const auto safety_result = validateSafety(state);
+        if (!safety_result.success) {
+            return safety_result;
+        }
+        if (!validMeasuredPosition(state.actual_joint_state)) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "RobotArm initial joint position cache is invalid");
+        }
+        if (request.requested_command_rate_hz() == 0) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::INVALID_ARGUMENT,
+                "requested_command_rate_hz must be non-zero");
+        }
+        const double requested_period_s =
+            1.0 /
+            static_cast<double>(request.requested_command_rate_hz());
+        // A client may request a slower command stream, but it may not claim a
+        // rate faster than the reviewed RobotArm servo period.
+        if (requested_period_s + 1e-12 <
+            config_.servo_period_s()) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "requested command rate exceeds configured RobotArm servo rate");
+        }
+        minimum_dispatch_period_ =
+            std::chrono::duration_cast<Clock::duration>(
+                std::chrono::duration<double>(
+                    std::max(
+                        requested_period_s,
+                        config_.servo_period_s())));
+
+        device::ServoOptions options;
+        options.period = config_.servo_period_s();
+        const auto start = Clock::now();
+        const auto result = arm_->startServoMode(options);
+        const auto elapsed = Clock::now() - start;
+        if (!result.ok()) {
+            return fromArmResult(result, "startServoMode");
+        }
+        if (elapsed > std::chrono::microseconds(
+                          config_.max_apply_duration_us())) {
+            bestEffortStop();
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::DEADLINE_EXCEEDED,
+                "startServoMode exceeded max_apply_duration_us");
+        }
+
+        initial_position_ = state.actual_joint_state.position;
+        last_position_.clear();
+        last_dispatch_time_ = Clock::time_point{};
+        session_open_ = true;
+        return ArmTeleopBackendResult::ok();
+    }
+
+    ArmTeleopBackendResult applySetpoint(
+        const arm_teleop::JointSetpoint& setpoint,
+        const Clock::time_point deadline) override
+    {
+        std::lock_guard operation_lock(operation_mutex_);
+        if (Clock::now() >= deadline) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::DEADLINE_EXCEEDED,
+                "setpoint expired before RobotArm backend validation");
+        }
+        if (!session_open_) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "RobotArm teleoperation servo mode is not open");
+        }
+
+        const auto input_result = validateSetpointInput(setpoint);
+        if (!input_result.success) {
+            return input_result;
+        }
+
+        const auto state = arm_->getRobotState();
+        cacheState(state);
+        const auto safety_result = validateSafety(state);
+        if (!safety_result.success) {
+            return safety_result;
+        }
+
+        const std::vector<double> target(
+            setpoint.position_rad().begin(),
+            setpoint.position_rad().end());
+        const auto& reference =
+            last_position_.empty() ? initial_position_ : last_position_;
+        const double allowed_step =
+            last_position_.empty()
+                ? config_.max_initial_position_step_rad()
+                : config_.max_position_step_rad();
+        for (std::size_t index = 0; index < target.size(); ++index) {
+            const double delta = std::abs(target[index] - reference[index]);
+            if (delta > allowed_step) {
+                std::ostringstream detail;
+                detail << "joint " << model_.joint_names[index]
+                       << " position step " << delta
+                       << " exceeds configured limit " << allowed_step;
+                return ArmTeleopBackendResult::failure(
+                    grpc::StatusCode::OUT_OF_RANGE, detail.str());
+            }
+            // Once a target has been accepted, also enforce the RobotModel
+            // velocity limit on target-to-target motion.
+            if (!last_position_.empty() &&
+                delta /
+                        std::chrono::duration<double>(
+                            minimum_dispatch_period_)
+                            .count() >
+                    model_.joint_limits[index].max_velocity) {
+                std::ostringstream detail;
+                detail << "joint " << model_.joint_names[index]
+                       << " target delta exceeds RobotModel velocity limit";
+                return ArmTeleopBackendResult::failure(
+                    grpc::StatusCode::OUT_OF_RANGE, detail.str());
+            }
+        }
+
+        const auto dispatch_time = Clock::now();
+        if (last_dispatch_time_ != Clock::time_point{} &&
+            dispatch_time - last_dispatch_time_ <
+                minimum_dispatch_period_) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::RESOURCE_EXHAUSTED,
+                "setpoint arrived before the negotiated RobotArm dispatch period");
+        }
+
+        device::JointPositionCommand command;
+        command.position = target;
+        // Robot state validation and command preparation may consume the
+        // remaining validity window. Re-check on the receiver's monotonic
+        // timeline at the last point before the RobotArm commit.
+        const auto start = Clock::now();
+        if (start >= deadline) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::DEADLINE_EXCEEDED,
+                "setpoint expired before RobotArm command dispatch");
+        }
+        if (deadline - start <
+            std::chrono::microseconds(
+                config_.max_apply_duration_us())) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::DEADLINE_EXCEEDED,
+                "setpoint lacks the configured RobotArm apply-time budget");
+        }
+        last_dispatch_time_ = start;
+        const auto result = arm_->servoJ(command);
+        const auto elapsed = Clock::now() - start;
+        if (!result.ok()) {
+            return fromArmResult(result, "servoJ");
+        }
+
+        // servoJ has already accepted this target even when the local timing
+        // contract is exceeded; remember it before returning the failure so a
+        // caller can never treat an older target as the last applied command.
+        last_position_ = target;
+        if (elapsed > std::chrono::microseconds(
+                          config_.max_apply_duration_us())) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::DEADLINE_EXCEEDED,
+                "servoJ exceeded max_apply_duration_us");
+        }
+        return ArmTeleopBackendResult::ok();
+    }
+
+    ArmTeleopBackendResult stop(
+        const arm_teleop::StopReason,
+        const std::string&) override
+    {
+        std::lock_guard operation_lock(operation_mutex_);
+        const auto motion_result = arm_ ? arm_->stopMotion()
+                                        : device::Result::success();
+        // This is deliberately called even when stopMotion fails.
+        const auto servo_result = arm_ ? arm_->stopServoMode()
+                                       : device::Result::success();
+        session_open_ = false;
+        initial_position_.clear();
+        last_position_.clear();
+        last_dispatch_time_ = Clock::time_point{};
+        minimum_dispatch_period_ = Clock::duration::zero();
+        if (arm_) {
+            cacheState(arm_->getRobotState());
+        }
+        if (!motion_result.ok()) {
+            return fromArmResult(motion_result, "stopMotion");
+        }
+        return fromArmResult(servo_result, "stopServoMode");
+    }
+
+    ArmTeleopBackendSnapshot snapshot() const override
+    {
+        std::lock_guard cache_lock(cache_mutex_);
+        auto result = cached_snapshot_;
+        if (cache_time_ != Clock::time_point{}) {
+            const auto age =
+                std::chrono::duration_cast<std::chrono::microseconds>(
+                    Clock::now() - cache_time_)
+                    .count();
+            result.joint_state.set_sample_age_us(
+                age > 0 ? static_cast<std::uint64_t>(age) : 0U);
+        }
+        return result;
+    }
+
+private:
+    void validateStaticConfiguration()
+    {
+        if (!config_.enable()) {
+            unavailable_reason_ =
+                "RobotArm teleoperation backend is explicitly disabled";
+            return;
+        }
+        if (!arm_) {
+            unavailable_reason_ =
+                "configured RobotArm device was not found";
+            return;
+        }
+        if (config_.device_id().empty() ||
+            config_.device_id() != arm_->id()) {
+            unavailable_reason_ =
+                "arm_teleop device_id must exactly match RobotArm.id";
+            return;
+        }
+        if (!arm_->supportsTeleopGroupServo()) {
+            unavailable_reason_ =
+                "RobotArm teleop group-servo capability is not enabled";
+            return;
+        }
+        if (!isDigest(config_.model_sha256()) ||
+            !isDigest(config_.calibration_sha256())) {
+            unavailable_reason_ =
+                "arm_teleop model and calibration SHA256 values must be 64 hex characters";
+            return;
+        }
+        if (config_.base_frame().empty() ||
+            config_.tool_frame().empty()) {
+            unavailable_reason_ =
+                "arm_teleop base_frame and tool_frame are required";
+            return;
+        }
+        if (!std::isfinite(config_.servo_period_s()) ||
+            config_.servo_period_s() < kMinimumServoPeriodS ||
+            config_.servo_period_s() > kMaximumServoPeriodS) {
+            unavailable_reason_ =
+                "arm_teleop servo_period_s must be in [0.0001, 0.1]";
+            return;
+        }
+        const double period_us =
+            config_.servo_period_s() * 1000000.0;
+        if (config_.max_apply_duration_us() == 0 ||
+            static_cast<double>(config_.max_apply_duration_us()) >
+                period_us) {
+            unavailable_reason_ =
+                "arm_teleop max_apply_duration_us must be non-zero and no greater than one servo period";
+            return;
+        }
+        if (!std::isfinite(config_.max_initial_position_step_rad()) ||
+            config_.max_initial_position_step_rad() <= 0.0 ||
+            !std::isfinite(config_.max_position_step_rad()) ||
+            config_.max_position_step_rad() <= 0.0) {
+            unavailable_reason_ =
+                "arm_teleop position step limits must be finite and positive";
+            return;
+        }
+
+        model_ = arm_->getRobotModel();
+        if (!model_.valid() ||
+            model_.joint_limits.size() != model_.dof) {
+            unavailable_reason_ =
+                "RobotModel must contain one safety limit for every joint";
+            return;
+        }
+        std::unordered_set<std::string> names;
+        for (std::size_t index = 0; index < model_.dof; ++index) {
+            const auto& name = model_.joint_names[index];
+            const auto& limit = model_.joint_limits[index];
+            if (name.empty() || !names.insert(name).second ||
+                !std::isfinite(limit.lower) ||
+                !std::isfinite(limit.upper) ||
+                !std::isfinite(limit.max_velocity) ||
+                limit.lower >= limit.upper ||
+                limit.max_velocity <= 0.0) {
+                unavailable_reason_ =
+                    "RobotModel joint names and position/velocity limits are invalid";
+                return;
+            }
+        }
+
+        manifest_.set_robot_id(config_.device_id());
+        manifest_.set_model_sha256(config_.model_sha256());
+        manifest_.set_calibration_sha256(
+            config_.calibration_sha256());
+        for (const auto& name : model_.joint_names) {
+            manifest_.add_joint_names(name);
+        }
+        manifest_.set_position_unit("rad");
+        manifest_.set_velocity_unit("rad/s");
+        manifest_.set_effort_unit("N*m");
+        manifest_.set_base_frame(config_.base_frame());
+        manifest_.set_tool_frame(config_.tool_frame());
+    }
+
+    ArmTeleopBackendResult validateSafety(
+        const device::ArmState& state) const
+    {
+        if (!state.connected) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "RobotArm is not connected");
+        }
+        if (require_powered_ && !state.powered_on) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "RobotArm is not powered on");
+        }
+        if (state.emergency_stopped ||
+            state.safety_mode == device::SafetyMode::EmergencyStop ||
+            state.safety_mode == device::SafetyMode::SystemEmergencyStop) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "RobotArm emergency stop is active");
+        }
+        if (state.protective_stopped ||
+            state.safety_mode == device::SafetyMode::ProtectiveStop ||
+            state.safety_mode == device::SafetyMode::SafeguardStop) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "RobotArm protective stop is active");
+        }
+        if (state.fault ||
+            state.robot_mode == device::RobotMode::Fault ||
+            state.safety_mode == device::SafetyMode::Fault) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::FAILED_PRECONDITION,
+                "RobotArm fault is active");
+        }
+        return ArmTeleopBackendResult::ok();
+    }
+
+    bool validMeasuredPosition(
+        const device::JointGroupState& state) const
+    {
+        if (!state.position_valid ||
+            state.position.size() != model_.dof) {
+            return false;
+        }
+        for (std::size_t index = 0; index < model_.dof; ++index) {
+            const double value = state.position[index];
+            const auto& limit = model_.joint_limits[index];
+            if (!std::isfinite(value) ||
+                value < limit.lower || value > limit.upper) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    ArmTeleopBackendResult validateSetpointInput(
+        const arm_teleop::JointSetpoint& setpoint) const
+    {
+        if (setpoint.position_rad_size() !=
+                static_cast<int>(model_.dof) ||
+            setpoint.velocity_rad_s_size() !=
+                static_cast<int>(model_.dof)) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::INVALID_ARGUMENT,
+                "setpoint dimensions do not match RobotModel");
+        }
+        for (std::size_t index = 0; index < model_.dof; ++index) {
+            const double position =
+                setpoint.position_rad(static_cast<int>(index));
+            const double velocity =
+                setpoint.velocity_rad_s(static_cast<int>(index));
+            const auto& limit = model_.joint_limits[index];
+            if (!std::isfinite(position) || !std::isfinite(velocity)) {
+                return ArmTeleopBackendResult::failure(
+                    grpc::StatusCode::INVALID_ARGUMENT,
+                    "setpoint position and velocity must be finite");
+            }
+            if (position < limit.lower || position > limit.upper) {
+                return ArmTeleopBackendResult::failure(
+                    grpc::StatusCode::OUT_OF_RANGE,
+                    "setpoint position exceeds RobotModel joint limit");
+            }
+            if (std::abs(velocity) > limit.max_velocity) {
+                return ArmTeleopBackendResult::failure(
+                    grpc::StatusCode::OUT_OF_RANGE,
+                    "setpoint velocity exceeds RobotModel joint limit");
+            }
+        }
+        return ArmTeleopBackendResult::ok();
+    }
+
+    void cacheState(const device::ArmState& state) const
+    {
+        ArmTeleopBackendSnapshot snapshot;
+        auto* joint = &snapshot.joint_state;
+        const auto& source = state.actual_joint_state;
+        joint->set_sample_sequence(source.sequence);
+        for (const double value : source.position) {
+            joint->add_position_rad(value);
+        }
+        for (const double value : source.velocity) {
+            joint->add_velocity_rad_s(value);
+        }
+
+        const auto effort_source =
+            toProtoEffortSource(arm_->jointEffortSource());
+        const bool effort_valid =
+            source.effort_valid &&
+            source.effort.size() == model_.dof &&
+            effort_source != arm_teleop::EFFORT_SOURCE_UNSPECIFIED &&
+            std::all_of(
+                source.effort.begin(), source.effort.end(),
+                [](const double value) { return std::isfinite(value); });
+        if (effort_valid) {
+            for (const double value : source.effort) {
+                joint->add_effort_nm(value);
+            }
+        }
+        joint->set_position_valid(validMeasuredPosition(source));
+        joint->set_velocity_valid(
+            source.velocity_valid &&
+            source.velocity.size() == model_.dof &&
+            std::all_of(
+                source.velocity.begin(), source.velocity.end(),
+                [](const double value) { return std::isfinite(value); }));
+        joint->set_effort_valid(effort_valid);
+        joint->set_effort_source(
+            effort_valid ? effort_source
+                         : arm_teleop::EFFORT_SOURCE_UNSPECIFIED);
+
+        auto* safety = &snapshot.safety;
+        safety->set_connected(state.connected);
+        safety->set_powered_on(state.powered_on);
+        safety->set_protective_stopped(state.protective_stopped);
+        safety->set_emergency_stopped(state.emergency_stopped);
+        safety->set_fault(
+            state.fault ||
+            state.robot_mode == device::RobotMode::Fault ||
+            state.safety_mode == device::SafetyMode::Fault);
+        if (safety->fault()) {
+            safety->set_fault_detail("RobotArm reports a fault");
+        }
+
+        std::lock_guard cache_lock(cache_mutex_);
+        cached_snapshot_ = std::move(snapshot);
+        cache_time_ = Clock::now();
+    }
+
+    void bestEffortStop() noexcept
+    {
+        try {
+            arm_->stopMotion();
+        } catch (...) {
+        }
+        try {
+            arm_->stopServoMode();
+        } catch (...) {
+        }
+        session_open_ = false;
+        initial_position_.clear();
+        last_position_.clear();
+        last_dispatch_time_ = Clock::time_point{};
+        minimum_dispatch_period_ = Clock::duration::zero();
+    }
+
+    std::shared_ptr<device::RobotArm> arm_;
+    config::ArmTeleopBackendConfig config_;
+    bool require_powered_{true};
+    device::RobotModel model_;
+    arm_teleop::RobotManifest manifest_;
+    std::string unavailable_reason_;
+
+    mutable std::mutex operation_mutex_;
+    bool session_open_{false};
+    std::vector<double> initial_position_;
+    std::vector<double> last_position_;
+    Clock::time_point last_dispatch_time_{};
+    Clock::duration minimum_dispatch_period_{Clock::duration::zero()};
+
+    mutable std::mutex cache_mutex_;
+    mutable ArmTeleopBackendSnapshot cached_snapshot_;
+    mutable Clock::time_point cache_time_{};
+};
+
+} // namespace
+
+std::shared_ptr<ArmTeleopBackend> makeRobotArmTeleopBackend(
+    std::shared_ptr<device::RobotArm> arm,
+    const config::ArmTeleopBackendConfig& config)
+{
+    return std::make_shared<RobotArmTeleopBackend>(
+        std::move(arm), config);
+}
+
+} // namespace cmvr::service