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.

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/manager/device_manager.pb.txt

@@ -111,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/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
+            // option does not configure the physical link bitrate or state.
+            if (enable_fd_) {
                 int enable = 1;
-            ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable,
+                ret = ::setsockopt(dev_handler_, SOL_CAN_RAW,
-                               sizeof(enable));
+                                   CAN_RAW_FD_FRAMES, &enable, sizeof(enable));
                 if (ret < 0) {
-                CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret;
+                    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;
-
+            if (dev_handler_ >= 0) {
-                int ret = close(dev_handler_);
+                const int fd = 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) {
-            if (frame_num == nullptr) {
+            return sendWithDeadline_(
-                CMVR_LOG(FATAL) << "frame_num is null";
+                frames, frame_num,
+                std::chrono::steady_clock::now() +
+                    std::chrono::microseconds(send_timeout_us_));
         }
-            if (frames.size() != static_cast<size_t>(*frame_num)) {
+
-                CMVR_LOG(FATAL) << "frames size does not match frame_num";
+        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(ERROR) << "frame_num is null";
+                return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+            }
+            if (*frame_num < 0 ||
+                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
-                            << ", 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;
+            // Validate the complete batch before committing its first frame.
+            // This prevents a malformed later element from causing a valid
+            // prefix of a cyclic command batch to reach the bus.
+            for (size_t i = 0; i < frames.size(); ++i) {
+                const auto& source = frames[i];
+                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 {
-                    send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK);
+                    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;
                 }
-                // 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
+                while (true) {
-                int ret = static_cast<int>(
+                    const auto written = ::send(
-                    write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i])));
+                        dev_handler_, payload, expected,
-                if (ret <= 0) {
+                        MSG_DONTWAIT | MSG_NOSIGNAL);
-                    CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret;
+                    if (written == static_cast<ssize_t>(expected)) {
-                    return ErrorCode::CAN_CLIENT_ERROR_BASE;
+                        ++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) {
-                }
-                if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH ||
-                    recv_frames_[i].can_dlc < 0) {
-                    CMVR_LOG(ERROR) << "recv_frames_[" << i
-                            << "].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) {
+                    CMVR_LOG(ERROR) << "receive CAN message failed: "
-                    cf.id = enable_can_err_check_
+                                    << std::strerror(errno);
-                                ? 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;
+                if (ret != CAN_MTU && ret != CANFD_MTU) {
-                cf.len = recv_frames_[i].can_dlc;
+                    CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret;
-                std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc);
+                    return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+                }
+
+                const canid_t raw_id = raw.can_id;
+                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/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/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) {
+        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);
         }
     }
 
-void DeviceManager::restart() {
+    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;
+}
+
+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;
+    {
+        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;
-            CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id();
-            for (const auto& started_task : started_tasks) {
         try {
-                    started_task->stop();
+            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();
+            stopTaskNoThrow(task);
+            for (auto it = started_tasks.rbegin();
+                 it != started_tasks.rend(); ++it) {
+                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);
+        }
+        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;
+    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";
+    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,8 @@ 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
@@ -18,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
@@ -44,6 +47,61 @@ if(BUILD_TESTING)
     )
     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
     )

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_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

cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp

@@ -0,0 +1,671 @@
+#include "service/grpc/include/grpc_arm_teleop_service.h"
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdio>
+#include <memory>
+#include <mutex>
+#include <set>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <grpcpp/grpcpp.h>
+#include <gtest/gtest.h>
+#include <unistd.h>
+
+namespace cmvr::service {
+namespace {
+
+using namespace std::chrono_literals;
+
+std::atomic<std::uint64_t> g_socket_sequence{0};
+
+arm_teleop::RobotManifest makeManifest()
+{
+    arm_teleop::RobotManifest manifest;
+    manifest.set_robot_id("fake_humanoid_arm");
+    manifest.set_model_sha256(std::string(64, 'a'));
+    manifest.set_calibration_sha256(std::string(64, 'b'));
+    manifest.add_joint_names("shoulder_joint");
+    manifest.add_joint_names("elbow_joint");
+    manifest.set_position_unit("rad");
+    manifest.set_velocity_unit("rad/s");
+    manifest.set_effort_unit("N*m");
+    manifest.set_base_frame("base_link");
+    manifest.set_tool_frame("tool_link");
+    return manifest;
+}
+
+arm_teleop::ClientFrame makeOpenFrame(
+    const arm_teleop::RobotManifest& manifest,
+    const std::uint32_t watchdog_ms = 100,
+    const std::uint32_t lease_ms = 2000,
+    const bool request_force_feedback = false)
+{
+    arm_teleop::ClientFrame frame;
+    auto* open = frame.mutable_open();
+    open->set_protocol_major(1);
+    open->set_protocol_minor(0);
+    open->set_client_instance_id("test-client");
+    *open->mutable_expected_robot() = manifest;
+    open->set_requested_command_rate_hz(200);
+    open->set_requested_state_rate_hz(100);
+    open->set_watchdog_timeout_ms(watchdog_ms);
+    open->set_requested_lease_ms(lease_ms);
+    open->set_request_force_feedback(request_force_feedback);
+    return frame;
+}
+
+arm_teleop::ClientFrame makeSetpoint(
+    const std::uint64_t sequence,
+    const std::uint32_t valid_for_us = 50000)
+{
+    arm_teleop::ClientFrame frame;
+    auto* setpoint = frame.mutable_setpoint();
+    setpoint->set_sequence(sequence);
+    setpoint->add_position_rad(0.1 * static_cast<double>(sequence));
+    setpoint->add_position_rad(0.2 * static_cast<double>(sequence));
+    setpoint->add_velocity_rad_s(0.01);
+    setpoint->add_velocity_rad_s(0.02);
+    setpoint->set_valid_for_us(valid_for_us);
+    return frame;
+}
+
+arm_teleop::ClientFrame makeHeartbeat(const std::uint64_t sequence)
+{
+    arm_teleop::ClientFrame frame;
+    frame.mutable_heartbeat()->set_sequence(sequence);
+    return frame;
+}
+
+arm_teleop::ClientFrame makeStop(
+    const arm_teleop::StopReason reason =
+        arm_teleop::STOP_REASON_OPERATOR_REQUEST)
+{
+    arm_teleop::ClientFrame frame;
+    frame.mutable_stop()->set_reason(reason);
+    frame.mutable_stop()->set_detail("test stop");
+    return frame;
+}
+
+class FakeArmTeleopBackend final : public ArmTeleopBackend {
+public:
+    explicit FakeArmTeleopBackend(
+        arm_teleop::RobotManifest manifest = makeManifest())
+        : manifest_(std::move(manifest))
+    {
+    }
+
+    bool available() const noexcept override { return true; }
+
+    arm_teleop::RobotManifest manifest() const override
+    {
+        recordThread();
+        return manifest_;
+    }
+
+    bool supportsForceFeedback() const noexcept override { return true; }
+
+    ArmTeleopBackendResult open(
+        const arm_teleop::OpenSession&) override
+    {
+        recordThread();
+        std::lock_guard lock(mutex_);
+        ++open_calls_;
+        return open_result_;
+    }
+
+    ArmTeleopBackendResult applySetpoint(
+        const arm_teleop::JointSetpoint& setpoint,
+        const std::chrono::steady_clock::time_point deadline) override
+    {
+        if (std::chrono::steady_clock::now() >= deadline) {
+            return ArmTeleopBackendResult::failure(
+                grpc::StatusCode::DEADLINE_EXCEEDED,
+                "fake backend received an expired setpoint");
+        }
+        recordThread();
+        std::unique_lock lock(mutex_);
+        apply_entered_ = true;
+        apply_entered_sequence_ = setpoint.sequence();
+        cv_.notify_all();
+        cv_.wait(lock, [&]() { return !block_apply_; });
+        applied_sequences_.push_back(setpoint.sequence());
+        return apply_result_;
+    }
+
+    ArmTeleopBackendResult stop(
+        const arm_teleop::StopReason reason,
+        const std::string&) override
+    {
+        recordThread();
+        std::lock_guard lock(mutex_);
+        stop_reasons_.push_back(reason);
+        return stop_result_;
+    }
+
+    ArmTeleopBackendSnapshot snapshot() const override
+    {
+        recordThread();
+        std::lock_guard lock(mutex_);
+        ArmTeleopBackendSnapshot snapshot;
+        auto* state = &snapshot.joint_state;
+        state->set_sample_sequence(++sample_sequence_);
+        state->add_position_rad(0.1);
+        state->add_position_rad(0.2);
+        state->add_velocity_rad_s(0.01);
+        state->add_velocity_rad_s(0.02);
+        state->add_effort_nm(1.0);
+        state->add_effort_nm(2.0);
+        state->set_position_valid(true);
+        state->set_velocity_valid(true);
+        state->set_effort_valid(true);
+        state->set_effort_source(
+            arm_teleop::EFFORT_SOURCE_JOINT_SENSOR);
+        snapshot.safety.set_connected(true);
+        snapshot.safety.set_powered_on(true);
+        return snapshot;
+    }
+
+    void blockApply()
+    {
+        std::lock_guard lock(mutex_);
+        block_apply_ = true;
+        apply_entered_ = false;
+        apply_entered_sequence_ = 0;
+    }
+
+    void releaseApply()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            block_apply_ = false;
+        }
+        cv_.notify_all();
+    }
+
+    bool waitForApply(
+        const std::uint64_t sequence,
+        const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return cv_.wait_for(lock, timeout, [&]() {
+            return apply_entered_ &&
+                   apply_entered_sequence_ == sequence;
+        });
+    }
+
+    int openCalls() const
+    {
+        std::lock_guard lock(mutex_);
+        return open_calls_;
+    }
+
+    std::vector<std::uint64_t> appliedSequences() const
+    {
+        std::lock_guard lock(mutex_);
+        return applied_sequences_;
+    }
+
+    std::vector<arm_teleop::StopReason> stopReasons() const
+    {
+        std::lock_guard lock(mutex_);
+        return stop_reasons_;
+    }
+
+    std::size_t backendThreadCount() const
+    {
+        std::lock_guard lock(thread_mutex_);
+        return backend_threads_.size();
+    }
+
+private:
+    void recordThread() const
+    {
+        std::lock_guard lock(thread_mutex_);
+        backend_threads_.insert(std::this_thread::get_id());
+    }
+
+    arm_teleop::RobotManifest manifest_;
+    mutable std::mutex mutex_;
+    mutable std::condition_variable cv_;
+    bool block_apply_{false};
+    bool apply_entered_{false};
+    std::uint64_t apply_entered_sequence_{0};
+    int open_calls_{0};
+    std::vector<std::uint64_t> applied_sequences_;
+    std::vector<arm_teleop::StopReason> stop_reasons_;
+    mutable std::uint64_t sample_sequence_{0};
+    ArmTeleopBackendResult open_result_{
+        ArmTeleopBackendResult::ok()};
+    ArmTeleopBackendResult apply_result_{
+        ArmTeleopBackendResult::ok()};
+    ArmTeleopBackendResult stop_result_{
+        ArmTeleopBackendResult::ok()};
+
+    mutable std::mutex thread_mutex_;
+    mutable std::set<std::thread::id> backend_threads_;
+};
+
+class TeleopServerHarness final {
+public:
+    explicit TeleopServerHarness(
+        std::shared_ptr<ArmTeleopBackend> backend)
+        : service_(std::move(backend))
+    {
+        socket_path_ =
+            "/tmp/cmvr_arm_teleop_service_test_" +
+            std::to_string(static_cast<long long>(::getpid())) + "_" +
+            std::to_string(
+                g_socket_sequence.fetch_add(
+                    1, std::memory_order_relaxed)) +
+            ".sock";
+        std::remove(socket_path_.c_str());
+        const std::string address = "unix:" + socket_path_;
+
+        grpc::ServerBuilder builder;
+        builder.AddListeningPort(
+            address, grpc::InsecureServerCredentials());
+        builder.RegisterService(&service_);
+        server_ = builder.BuildAndStart();
+        if (!server_) {
+            throw std::runtime_error(
+                "failed to start in-process arm teleoperation server");
+        }
+        channel_ = grpc::CreateChannel(
+            address, grpc::InsecureChannelCredentials());
+        if (!channel_->WaitForConnected(
+                std::chrono::system_clock::now() + 2s)) {
+            throw std::runtime_error(
+                "failed to connect arm teleoperation test channel");
+        }
+        stub_ = arm_teleop::ArmTeleopService::NewStub(channel_);
+    }
+
+    ~TeleopServerHarness()
+    {
+        if (server_) {
+            server_->Shutdown();
+            server_->Wait();
+        }
+        if (!socket_path_.empty()) {
+            std::remove(socket_path_.c_str());
+        }
+    }
+
+    arm_teleop::ArmTeleopService::Stub& stub() { return *stub_; }
+
+private:
+    ArmTeleopServiceImpl service_;
+    std::unique_ptr<grpc::Server> server_;
+    std::shared_ptr<grpc::Channel> channel_;
+    std::unique_ptr<arm_teleop::ArmTeleopService::Stub> stub_;
+    std::string socket_path_;
+};
+
+template <typename Stream>
+void expectOpeningFrames(Stream& stream)
+{
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream.Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_OPENED);
+    EXPECT_FALSE(response.status().session_id().empty());
+    ASSERT_TRUE(stream.Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_READY);
+    EXPECT_TRUE(response.safety().connected());
+}
+
+TEST(ArmTeleopServiceTest, ProductionDisabledBackendRejectsOpen)
+{
+    TeleopServerHarness harness(makeDisabledArmTeleopBackend());
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest())));
+    ASSERT_TRUE(stream->WritesDone());
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_REJECTED);
+    EXPECT_TRUE(response.safety().fault());
+    const grpc::Status status = stream->Finish();
+    EXPECT_EQ(
+        status.error_code(),
+        grpc::StatusCode::FAILED_PRECONDITION);
+}
+
+TEST(ArmTeleopServiceTest, RequiresOpenAsFirstFrame)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(makeSetpoint(1)));
+    ASSERT_TRUE(stream->WritesDone());
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_REJECTED);
+    const grpc::Status status = stream->Finish();
+    EXPECT_EQ(
+        status.error_code(),
+        grpc::StatusCode::INVALID_ARGUMENT);
+    EXPECT_EQ(backend->openCalls(), 0);
+}
+
+TEST(ArmTeleopServiceTest, RejectsManifestMismatchBeforeBackendOpen)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    auto mismatched = makeManifest();
+    mismatched.set_calibration_sha256(std::string(64, 'c'));
+    ASSERT_TRUE(stream->Write(makeOpenFrame(mismatched)));
+    ASSERT_TRUE(stream->WritesDone());
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_REJECTED);
+    const grpc::Status status = stream->Finish();
+    EXPECT_EQ(
+        status.error_code(),
+        grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_EQ(backend->openCalls(), 0);
+}
+
+TEST(ArmTeleopServiceTest, RejectsNonIncreasingSequenceAndStops)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest())));
+    expectOpeningFrames(*stream);
+    ASSERT_TRUE(stream->Write(makeSetpoint(1)));
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_ACTIVE);
+    EXPECT_EQ(response.status().applied_sequence(), 1U);
+
+    ASSERT_TRUE(stream->Write(makeHeartbeat(1)));
+    ASSERT_TRUE(stream->WritesDone());
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_REJECTED);
+    EXPECT_EQ(
+        response.status().stop_reason(),
+        arm_teleop::STOP_REASON_PROTOCOL_ERROR);
+
+    const grpc::Status status = stream->Finish();
+    EXPECT_EQ(
+        status.error_code(),
+        grpc::StatusCode::INVALID_ARGUMENT);
+    ASSERT_FALSE(backend->stopReasons().empty());
+    EXPECT_EQ(
+        backend->stopReasons().back(),
+        arm_teleop::STOP_REASON_PROTOCOL_ERROR);
+    EXPECT_EQ(backend->backendThreadCount(), 1U);
+}
+
+TEST(ArmTeleopServiceTest, ProtocolErrorCancelsReaderWithoutClientHalfClose)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest())));
+    expectOpeningFrames(*stream);
+    ASSERT_TRUE(stream->Write(makeHeartbeat(1)));
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_READY);
+
+    // Deliberately keep the client write half open after the duplicate
+    // sequence. The server must cancel its reader and return promptly.
+    ASSERT_TRUE(stream->Write(makeHeartbeat(1)));
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_REJECTED);
+    EXPECT_EQ(
+        response.status().stop_reason(),
+        arm_teleop::STOP_REASON_PROTOCOL_ERROR);
+    const auto status = stream->Finish();
+    // TryCancel is required to interrupt the server reader's blocking Read
+    // when the peer deliberately keeps its write half open. Depending on
+    // gRPC completion ordering, the client may therefore observe CANCELLED
+    // after it has already received the explicit protocol-error frame.
+    EXPECT_TRUE(
+        status.error_code() == grpc::StatusCode::INVALID_ARGUMENT ||
+        status.error_code() == grpc::StatusCode::CANCELLED)
+        << status.error_message();
+}
+
+TEST(ArmTeleopServiceTest, WatchdogExpiresWithoutValidClientActivity)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(
+        makeOpenFrame(makeManifest(), 20, 2000)));
+    expectOpeningFrames(*stream);
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED);
+    EXPECT_EQ(
+        response.status().stop_reason(),
+        arm_teleop::STOP_REASON_WATCHDOG);
+
+    const grpc::Status status = stream->Finish();
+    EXPECT_TRUE(
+        status.error_code() == grpc::StatusCode::DEADLINE_EXCEEDED ||
+        status.error_code() == grpc::StatusCode::CANCELLED)
+        << status.error_message();
+    ASSERT_FALSE(backend->stopReasons().empty());
+    EXPECT_EQ(
+        backend->stopReasons().back(),
+        arm_teleop::STOP_REASON_WATCHDOG);
+}
+
+TEST(ArmTeleopServiceTest, ValidHeartbeatsRenewControlLease)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 3s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(
+        makeOpenFrame(makeManifest(), 100, 200)));
+    expectOpeningFrames(*stream);
+
+    arm_teleop::ServerFrame response;
+    for (std::uint64_t sequence = 1; sequence <= 7; ++sequence) {
+        std::this_thread::sleep_for(40ms);
+        ASSERT_TRUE(stream->Write(makeHeartbeat(sequence)));
+        ASSERT_TRUE(stream->Read(&response));
+        EXPECT_EQ(
+            response.status().phase(),
+            arm_teleop::SESSION_PHASE_READY);
+        EXPECT_EQ(
+            response.status().received_sequence(),
+            sequence);
+        EXPECT_GT(response.status().lease_remaining_ms(), 0U);
+    }
+
+    ASSERT_TRUE(stream->Write(makeStop()));
+    ASSERT_TRUE(stream->WritesDone());
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_STOPPED);
+    EXPECT_TRUE(stream->Finish().ok());
+}
+
+TEST(ArmTeleopServiceTest, RejectsSecondControllerWhileLeaseIsActive)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+
+    grpc::ClientContext first_context;
+    first_context.set_deadline(
+        std::chrono::system_clock::now() + 3s);
+    auto first_stream =
+        harness.stub().Teleoperate(&first_context);
+    ASSERT_TRUE(first_stream->Write(
+        makeOpenFrame(makeManifest(), 500, 2000)));
+    expectOpeningFrames(*first_stream);
+
+    grpc::ClientContext second_context;
+    second_context.set_deadline(
+        std::chrono::system_clock::now() + 2s);
+    auto second_stream =
+        harness.stub().Teleoperate(&second_context);
+    ASSERT_TRUE(second_stream->Write(
+        makeOpenFrame(makeManifest(), 500, 2000)));
+    ASSERT_TRUE(second_stream->WritesDone());
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(second_stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_REJECTED);
+    const grpc::Status second_status =
+        second_stream->Finish();
+    EXPECT_EQ(
+        second_status.error_code(),
+        grpc::StatusCode::RESOURCE_EXHAUSTED);
+
+    ASSERT_TRUE(first_stream->Write(makeStop()));
+    ASSERT_TRUE(first_stream->WritesDone());
+    ASSERT_TRUE(first_stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_STOPPED);
+    EXPECT_TRUE(first_stream->Finish().ok());
+}
+
+TEST(ArmTeleopServiceTest, LatestOnlySlotDropsIntermediateSetpoints)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 3s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(
+        makeOpenFrame(makeManifest(), 500, 2000)));
+    expectOpeningFrames(*stream);
+
+    backend->blockApply();
+    ASSERT_TRUE(stream->Write(makeSetpoint(1, 400000)));
+    ASSERT_TRUE(backend->waitForApply(1, 1s));
+    ASSERT_TRUE(stream->Write(makeSetpoint(2, 400000)));
+    ASSERT_TRUE(stream->Write(makeSetpoint(3, 400000)));
+    ASSERT_TRUE(stream->Write(makeSetpoint(4, 400000)));
+    std::this_thread::sleep_for(30ms);
+    backend->releaseApply();
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(response.status().applied_sequence(), 1U);
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(response.status().applied_sequence(), 4U);
+    EXPECT_GE(response.status().dropped_setpoints(), 2U);
+
+    ASSERT_TRUE(stream->Write(makeStop()));
+    ASSERT_TRUE(stream->WritesDone());
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_STOPPED);
+    EXPECT_TRUE(stream->Finish().ok());
+
+    const auto applied = backend->appliedSequences();
+    ASSERT_EQ(applied.size(), 2U);
+    EXPECT_EQ(applied[0], 1U);
+    EXPECT_EQ(applied[1], 4U);
+    EXPECT_EQ(backend->backendThreadCount(), 1U);
+}
+
+TEST(ArmTeleopServiceTest, ExpiredSetpointIsNeverDispatched)
+{
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 3s);
+    auto stream = harness.stub().Teleoperate(&context);
+
+    ASSERT_TRUE(stream->Write(
+        makeOpenFrame(makeManifest(), 500, 2000)));
+    expectOpeningFrames(*stream);
+
+    backend->blockApply();
+    ASSERT_TRUE(stream->Write(makeSetpoint(1, 400000)));
+    ASSERT_TRUE(backend->waitForApply(1, 1s));
+    ASSERT_TRUE(stream->Write(makeSetpoint(2, 1000)));
+    std::this_thread::sleep_for(30ms);
+    backend->releaseApply();
+
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(response.status().applied_sequence(), 1U);
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_HOLDING);
+    EXPECT_EQ(response.status().received_sequence(), 2U);
+    EXPECT_EQ(response.status().applied_sequence(), 1U);
+    EXPECT_EQ(response.status().rejected_setpoints(), 1U);
+
+    ASSERT_TRUE(stream->Write(makeStop()));
+    ASSERT_TRUE(stream->WritesDone());
+    ASSERT_TRUE(stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_STOPPED);
+    EXPECT_TRUE(stream->Finish().ok());
+
+    const auto applied = backend->appliedSequences();
+    ASSERT_EQ(applied.size(), 1U);
+    EXPECT_EQ(applied.front(), 1U);
+}
+
+} // namespace
+} // namespace cmvr::service

cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp

@@ -0,0 +1,525 @@
+#include "service/grpc/include/grpc_robot_arm_teleop_backend.h"
+
+#include <chrono>
+#include <cstddef>
+#include <memory>
+#include <optional>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::service {
+namespace {
+
+class FakeRobotArm final : public device::RobotArm {
+public:
+    FakeRobotArm()
+    {
+        id_ = "right_arm";
+        model_.name = id_;
+        model_.dof = 2;
+        model_.joint_names = {"joint_1", "joint_2"};
+        model_.joint_limits = {
+            {-1.0, 1.0, 2.0, 5.0, 10.0},
+            {-0.5, 0.5, 3.0, 6.0, 10.0}};
+
+        state_.connected = true;
+        state_.powered_on = true;
+        state_.robot_mode = device::RobotMode::Idle;
+        state_.safety_mode = device::SafetyMode::Normal;
+        state_.actual_joint_state.position = {0.0, 0.0};
+        state_.actual_joint_state.velocity = {0.0, 0.0};
+        state_.actual_joint_state.effort = {1.0, 2.0};
+        state_.actual_joint_state.sequence = 7;
+        state_.actual_joint_state.position_valid = true;
+        state_.actual_joint_state.velocity_valid = true;
+        state_.actual_joint_state.effort_valid = true;
+    }
+
+    std::string typeName() const override { return "FakeRobotArm"; }
+    device::RobotModel getRobotModel() const override { return model_; }
+    std::size_t getDof() const override { return model_.dof; }
+    device::ArmState getRobotState() const override
+    {
+        ++get_state_calls;
+        return state_;
+    }
+    device::JointGroupState getJointState() const override
+    {
+        return state_.actual_joint_state;
+    }
+    device::CartesianPose getTcpPose(
+        device::FrameType = device::FrameType::Base) const override
+    {
+        return {};
+    }
+    device::RobotMode getRobotMode() const override
+    {
+        return state_.robot_mode;
+    }
+    device::SafetyMode getSafetyMode() const override
+    {
+        return state_.safety_mode;
+    }
+    device::ControlMode getControlMode() const override
+    {
+        return device::ControlMode::Servo;
+    }
+    bool supportsTeleopGroupServo() const noexcept override
+    {
+        return group_servo_capability;
+    }
+    device::JointEffortSource jointEffortSource() const noexcept override
+    {
+        return effort_source;
+    }
+
+    device::Result torqueOn() override
+    {
+        ++torque_on_calls;
+        return device::Result::success();
+    }
+    device::Result torqueOff() override { return device::Result::success(); }
+    device::Result calibrateZeroQ(const std::string&) override
+    {
+        return device::Result::success();
+    }
+    device::Result emergencyStop() override
+    {
+        return device::Result::success();
+    }
+    device::Result protectiveStop() override
+    {
+        return device::Result::success();
+    }
+    device::Result setSpeedScaling(double) override
+    {
+        return device::Result::success();
+    }
+    double getSpeedScaling() const override { return 1.0; }
+    bool isProtectiveStopped() const override { return false; }
+    bool isEmergencyStopped() const override { return false; }
+    bool isFault() const override { return state_.fault; }
+
+    device::Result moveJ(
+        const device::JointPositionCommand&,
+        const device::MotionOptions&) override
+    {
+        return device::Result::success();
+    }
+    device::Result speedJ(
+        const device::JointVelocityCommand&, double, double) override
+    {
+        return device::Result::success();
+    }
+    device::Result stopJ(double) override
+    {
+        return device::Result::success();
+    }
+    device::Result moveL(
+        const device::CartesianPose&,
+        const device::MotionOptions&,
+        device::FrameType = device::FrameType::Base) override
+    {
+        return device::Result::success();
+    }
+    device::Result speedL(
+        const device::CartesianVelocity&,
+        double,
+        double,
+        device::FrameType = device::FrameType::Base) override
+    {
+        return device::Result::success();
+    }
+    device::Result stopL(std::optional<double> = std::nullopt) override
+    {
+        return device::Result::success();
+    }
+    device::Result stopMotion() override
+    {
+        ++stop_motion_calls;
+        return stop_motion_result;
+    }
+
+    device::Result startServoMode(
+        const device::ServoOptions& options) override
+    {
+        ++start_servo_calls;
+        last_servo_period = options.period;
+        return start_servo_result;
+    }
+    device::Result servoJ(
+        const device::JointPositionCommand& target) override
+    {
+        ++servo_j_calls;
+        last_command = target.position;
+        if (servo_sleep.count() > 0) {
+            std::this_thread::sleep_for(servo_sleep);
+        }
+        return servo_j_result;
+    }
+    device::Result servoL(
+        const device::CartesianPose&,
+        device::FrameType = device::FrameType::Base) override
+    {
+        return device::Result::success();
+    }
+    device::Result servoSpeedJ(
+        const device::JointVelocityCommand&) override
+    {
+        return device::Result::success();
+    }
+    device::Result servoSpeedL(
+        const device::CartesianVelocity&,
+        device::FrameType = device::FrameType::Base) override
+    {
+        return device::Result::success();
+    }
+    device::Result stopServoMode() override
+    {
+        ++stop_servo_calls;
+        return stop_servo_result;
+    }
+
+    device::Result connect(const std::string&, int) override
+    {
+        return device::Result::success();
+    }
+    device::Result disconnect() override
+    {
+        return device::Result::success();
+    }
+    bool isConnected() const override { return state_.connected; }
+    device::Result powerOn() override { return torqueOn(); }
+    device::Result powerOff() override { return torqueOff(); }
+    device::Result brakeRelease() override
+    {
+        return device::Result::success();
+    }
+    device::Result shutdown() override
+    {
+        return device::Result::success();
+    }
+    device::Result clearFault() override
+    {
+        return device::Result::success();
+    }
+    device::Result unlockProtectiveStop() override
+    {
+        return device::Result::success();
+    }
+    device::Result loadProgram(const std::string&) override
+    {
+        return device::Result::success();
+    }
+    device::Result playProgram() override
+    {
+        return device::Result::success();
+    }
+    device::Result pauseProgram() override
+    {
+        return device::Result::success();
+    }
+    device::Result stopProgram() override
+    {
+        return device::Result::success();
+    }
+    std::vector<double> ik(
+        const std::string&,
+        const std::string&,
+        const device::CartesianPose&) override
+    {
+        return {};
+    }
+    std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override
+    {
+        return nullptr;
+    }
+    device::CartesianPose fk(
+        const std::string&, const std::string&) override
+    {
+        return {};
+    }
+    device::CartesianPose fk(bool = true) override { return {}; }
+    device::CartesianVelocity getSpeedLCommandTwistBase() const override
+    {
+        return {};
+    }
+    bool busy() const override { return false; }
+
+    bool group_servo_capability{true};
+    device::JointEffortSource effort_source{
+        device::JointEffortSource::Unspecified};
+    mutable int get_state_calls{0};
+    int torque_on_calls{0};
+    int start_servo_calls{0};
+    int servo_j_calls{0};
+    int stop_motion_calls{0};
+    int stop_servo_calls{0};
+    double last_servo_period{0.0};
+    std::vector<double> last_command;
+    std::chrono::microseconds servo_sleep{0};
+    device::Result start_servo_result{device::Result::success()};
+    device::Result servo_j_result{device::Result::success()};
+    device::Result stop_motion_result{device::Result::success()};
+    device::Result stop_servo_result{device::Result::success()};
+    device::RobotModel model_;
+    device::ArmState state_;
+};
+
+config::ArmTeleopBackendConfig validConfig()
+{
+    config::ArmTeleopBackendConfig config;
+    config.set_enable(true);
+    config.set_device_id("right_arm");
+    config.set_model_sha256(std::string(64, 'a'));
+    config.set_calibration_sha256(std::string(64, 'b'));
+    config.set_base_frame("base_link");
+    config.set_tool_frame("tool_link");
+    config.set_servo_period_s(0.01);
+    config.set_max_apply_duration_us(5000);
+    config.set_require_powered(true);
+    config.set_max_initial_position_step_rad(0.2);
+    config.set_max_position_step_rad(0.015);
+    return config;
+}
+
+arm_teleop::JointSetpoint setpoint(
+    const double first,
+    const double second,
+    const double first_velocity = 0.0,
+    const double second_velocity = 0.0)
+{
+    arm_teleop::JointSetpoint value;
+    value.set_sequence(1);
+    value.add_position_rad(first);
+    value.add_position_rad(second);
+    value.add_velocity_rad_s(first_velocity);
+    value.add_velocity_rad_s(second_velocity);
+    value.set_valid_for_us(5000);
+    return value;
+}
+
+arm_teleop::OpenSession openRequest()
+{
+    arm_teleop::OpenSession request;
+    request.set_requested_command_rate_hz(100);
+    return request;
+}
+
+std::chrono::steady_clock::time_point liveDeadline()
+{
+    return std::chrono::steady_clock::now() +
+           std::chrono::seconds(1);
+}
+
+TEST(RobotArmTeleopBackendTest, RejectsArmWithoutExplicitGroupServoCapability)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    arm->group_servo_capability = false;
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+
+    EXPECT_FALSE(backend->available());
+    EXPECT_NE(
+        backend->unavailableReason().find("capability"),
+        std::string::npos);
+    EXPECT_EQ(arm->start_servo_calls, 0);
+}
+
+TEST(RobotArmTeleopBackendTest, OpenStartsServoButNeverPowersArm)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+
+    ASSERT_TRUE(backend->available())
+        << backend->unavailableReason();
+    EXPECT_TRUE(backend->open(openRequest()).success);
+    EXPECT_EQ(arm->start_servo_calls, 1);
+    EXPECT_DOUBLE_EQ(arm->last_servo_period, 0.01);
+    EXPECT_EQ(arm->torque_on_calls, 0);
+}
+
+TEST(RobotArmTeleopBackendTest, AppliesSetpointAndAlwaysRunsBothStopPaths)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    ASSERT_TRUE(backend->open(openRequest()).success);
+
+    ASSERT_TRUE(
+        backend->applySetpoint(
+            setpoint(0.1, 0.1), liveDeadline()).success);
+    EXPECT_EQ(arm->servo_j_calls, 1);
+    EXPECT_EQ(arm->last_command, (std::vector<double>{0.1, 0.1}));
+
+    arm->stop_motion_result = device::Result::failure(
+        device::ArmErrorCode::CommandFailed, "motion stop failed");
+    const auto result = backend->stop(
+        arm_teleop::STOP_REASON_OPERATOR_REQUEST, "test");
+    EXPECT_FALSE(result.success);
+    EXPECT_EQ(arm->stop_motion_calls, 1);
+    EXPECT_EQ(arm->stop_servo_calls, 1);
+}
+
+TEST(RobotArmTeleopBackendTest, RejectsInitialAndContinuousPositionSteps)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    ASSERT_TRUE(backend->open(openRequest()).success);
+
+    EXPECT_EQ(
+        backend->applySetpoint(
+            setpoint(0.21, 0.0), liveDeadline()).status_code,
+        grpc::StatusCode::OUT_OF_RANGE);
+    ASSERT_TRUE(
+        backend->applySetpoint(
+            setpoint(0.1, 0.1), liveDeadline()).success);
+    EXPECT_EQ(
+        backend->applySetpoint(
+            setpoint(0.12, 0.1), liveDeadline()).status_code,
+        grpc::StatusCode::OUT_OF_RANGE);
+    EXPECT_EQ(arm->servo_j_calls, 1);
+}
+
+TEST(RobotArmTeleopBackendTest, RejectsJointPositionAndVelocityLimits)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    ASSERT_TRUE(backend->open(openRequest()).success);
+
+    EXPECT_EQ(
+        backend->applySetpoint(
+            setpoint(1.01, 0.0), liveDeadline()).status_code,
+        grpc::StatusCode::OUT_OF_RANGE);
+    EXPECT_EQ(
+        backend->applySetpoint(
+            setpoint(0.0, 0.0, 2.01, 0.0), liveDeadline())
+            .status_code,
+        grpc::StatusCode::OUT_OF_RANGE);
+    EXPECT_EQ(arm->servo_j_calls, 0);
+}
+
+TEST(RobotArmTeleopBackendTest, DetectsServoApplyTimeout)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    auto config = validConfig();
+    config.set_max_apply_duration_us(500);
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, config);
+    ASSERT_TRUE(backend->open(openRequest()).success);
+    arm->servo_sleep = std::chrono::microseconds(1500);
+
+    const auto result =
+        backend->applySetpoint(
+            setpoint(0.1, 0.1), liveDeadline());
+    EXPECT_FALSE(result.success);
+    EXPECT_EQ(
+        result.status_code,
+        grpc::StatusCode::DEADLINE_EXCEEDED);
+    EXPECT_EQ(arm->servo_j_calls, 1);
+}
+
+TEST(RobotArmTeleopBackendTest, RejectsInsufficientDeadlineBudgetBeforeDispatch)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    ASSERT_TRUE(backend->open(openRequest()).success);
+
+    const auto result = backend->applySetpoint(
+        setpoint(0.1, 0.1),
+        std::chrono::steady_clock::now() +
+            std::chrono::microseconds(100));
+    EXPECT_FALSE(result.success);
+    EXPECT_EQ(
+        result.status_code,
+        grpc::StatusCode::DEADLINE_EXCEEDED);
+    EXPECT_EQ(arm->servo_j_calls, 0);
+}
+
+TEST(RobotArmTeleopBackendTest, ExpiredDeadlineNeverDispatchesServoCommand)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    ASSERT_TRUE(backend->open(openRequest()).success);
+
+    const auto result = backend->applySetpoint(
+        setpoint(0.1, 0.1),
+        std::chrono::steady_clock::now());
+    EXPECT_FALSE(result.success);
+    EXPECT_EQ(
+        result.status_code,
+        grpc::StatusCode::DEADLINE_EXCEEDED);
+    EXPECT_EQ(arm->servo_j_calls, 0);
+}
+
+TEST(RobotArmTeleopBackendTest, RejectsUnsupportedRateAndEarlyRedispatch)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    auto too_fast = openRequest();
+    too_fast.set_requested_command_rate_hz(101);
+    EXPECT_EQ(
+        backend->open(too_fast).status_code,
+        grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_EQ(arm->start_servo_calls, 0);
+
+    ASSERT_TRUE(backend->open(openRequest()).success);
+    ASSERT_TRUE(
+        backend->applySetpoint(
+            setpoint(0.1, 0.1), liveDeadline()).success);
+    EXPECT_EQ(
+        backend->applySetpoint(
+            setpoint(0.105, 0.105), liveDeadline()).status_code,
+        grpc::StatusCode::RESOURCE_EXHAUSTED);
+    EXPECT_EQ(arm->servo_j_calls, 1);
+}
+
+TEST(RobotArmTeleopBackendTest, SnapshotUsesCacheAndHidesUnverifiedEffort)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    ASSERT_TRUE(backend->open(openRequest()).success);
+    const int calls_after_open = arm->get_state_calls;
+
+    const auto first = backend->snapshot();
+    const auto second = backend->snapshot();
+    EXPECT_EQ(arm->get_state_calls, calls_after_open);
+    EXPECT_TRUE(first.joint_state.position_valid());
+    EXPECT_TRUE(first.joint_state.velocity_valid());
+    EXPECT_FALSE(first.joint_state.effort_valid());
+    EXPECT_EQ(first.joint_state.effort_nm_size(), 0);
+    EXPECT_EQ(
+        second.joint_state.effort_source(),
+        arm_teleop::EFFORT_SOURCE_UNSPECIFIED);
+}
+
+TEST(RobotArmTeleopBackendTest, PublishesEffortOnlyWithExplicitSource)
+{
+    auto arm = std::make_shared<FakeRobotArm>();
+    arm->effort_source = device::JointEffortSource::JointSensor;
+    const auto backend =
+        makeRobotArmTeleopBackend(arm, validConfig());
+    ASSERT_TRUE(backend->open(openRequest()).success);
+
+    const auto snapshot = backend->snapshot();
+    EXPECT_TRUE(backend->supportsForceFeedback());
+    EXPECT_TRUE(snapshot.joint_state.effort_valid());
+    EXPECT_EQ(snapshot.joint_state.effort_nm_size(), 2);
+    EXPECT_EQ(
+        snapshot.joint_state.effort_source(),
+        arm_teleop::EFFORT_SOURCE_JOINT_SENSOR);
+}
+
+} // namespace
+} // namespace cmvr::service

cmvr-es/task/grpc_server_task/include/grpc_server_task.h

@@ -11,6 +11,10 @@
 #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
 #include "task/task.h"
 
+namespace cmvr::service {
+class ArmTeleopBackend;
+}
+
 namespace cmvr::task {
 
 class GrpcServerTask final : public Task {
@@ -55,6 +59,9 @@ private:
     std::unique_ptr<grpc::Service> dexhand_service_;
     std::unique_ptr<grpc::Service> biohand_service_;
     std::unique_ptr<grpc::Service> arm_service_;
+    std::unique_ptr<grpc::Service> arm_teleop_service_;
+    std::shared_ptr<cmvr::service::ArmTeleopBackend>
+        arm_teleop_backend_;
     std::unique_ptr<grpc::Service> motor_service_;
     std::unique_ptr<grpc::Service> agv_service_;
     std::unique_ptr<grpc::Service> hlc_service_;

cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp

@@ -8,8 +8,12 @@
 #include "cmvr/config/task_manager_config/task_manager_config.pb.h"
 #include "common/base/logging/logger.h"
 #include "common/config/config_files.h"
+#include "devices/arm/robot_arm.h"
+#include "manager/device_manager/include/device_manager.h"
 #include "service/grpc/include/grpc_agv_service.h"
 #include "service/grpc/include/grpc_arm_service.h"
+#include "service/grpc/include/grpc_arm_teleop_service.h"
+#include "service/grpc/include/grpc_robot_arm_teleop_backend.h"
 #include "service/grpc/include/grpc_camera_service.h"
 #include "service/grpc/include/grpc_dexhand_service.h"
 #include "service/grpc/include/grpc_head_service.h"
@@ -106,6 +110,10 @@ bool GrpcServerTask::start()
     dexhand_service_ = std::make_unique<service::gRPCDexHandServiceImpl>();
     biohand_service_ = std::make_unique<service::gRPCMBioHeadServiceImpl>();
     arm_service_ = std::make_unique<service::gRPCArmServiceImpl>();
+    arm_teleop_service_ =
+        std::make_unique<service::ArmTeleopServiceImpl>(
+            arm_teleop_backend_ ? arm_teleop_backend_
+                                : service::makeDisabledArmTeleopBackend());
     motor_service_ = std::make_unique<service::gRPCMotorServiceImpl>();
     agv_service_ = std::make_unique<service::gRPCAgvServiceImpl>();
     hlc_service_ = std::make_unique<service::gRPCHlcServiceImpl>();
@@ -119,6 +127,7 @@ bool GrpcServerTask::start()
     builder.RegisterService(dexhand_service_.get());
     builder.RegisterService(biohand_service_.get());
     builder.RegisterService(arm_service_.get());
+    builder.RegisterService(arm_teleop_service_.get());
     builder.RegisterService(motor_service_.get());
     builder.RegisterService(agv_service_.get());
     builder.RegisterService(hlc_service_.get());
@@ -152,6 +161,53 @@ bool GrpcServerTask::init()
         state_ = TaskState::FAILED;
         return false;
     }
+
+    arm_teleop_backend_ = service::makeDisabledArmTeleopBackend();
+    if (cfg_.has_arm_teleop_backend() &&
+        cfg_.arm_teleop_backend().enable()) {
+        const auto& backend_config = cfg_.arm_teleop_backend();
+        if (backend_config.device_id().empty()) {
+            last_error_ =
+                "enabled ArmTeleop backend requires device_id";
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        auto arm =
+            device::DeviceManager::getInstance()
+                .getDevice<device::RobotArm>(
+                    backend_config.device_id());
+        if (!arm) {
+            last_error_ =
+                "ArmTeleop RobotArm device was not found: " +
+                backend_config.device_id();
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        auto backend =
+            service::makeRobotArmTeleopBackend(
+                std::move(arm), backend_config);
+        if (!backend->available()) {
+            last_error_ =
+                "ArmTeleop backend rejected configuration: " +
+                backend->unavailableReason();
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        // RobotArmTeleopBackend builds robot_id directly from device_id. Keep
+        // this assertion at the registration boundary so the process-wide
+        // control lease resource and unary ArmService device ID cannot drift.
+        if (backend->manifest().robot_id() !=
+            backend_config.device_id()) {
+            last_error_ =
+                "ArmTeleop lease resource must equal device_id";
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        arm_teleop_backend_ = std::move(backend);
+        CMVR_LOG(INFO)
+            << "[GrpcServerTask] ArmTeleop RobotArm backend enabled for "
+            << backend_config.device_id();
+    }
     last_error_.clear();
     state_ = TaskState::IDLE;
     return true;
@@ -258,6 +314,7 @@ void GrpcServerTask::clearServices()
     hlc_service_.reset();
     agv_service_.reset();
     motor_service_.reset();
+    arm_teleop_service_.reset();
     arm_service_.reset();
     biohand_service_.reset();
     dexhand_service_.reset();

cmvr-es/task/quic_edge_task/CMakeLists.txt

@@ -21,7 +21,14 @@ if(BUILD_TESTING)
         get_property(_quic_task_test_library_dirs DIRECTORY PROPERTY LINK_DIRECTORIES)
         list(PREPEND _quic_task_test_library_dirs "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime")
         list(JOIN _quic_task_test_library_dirs ":" _quic_task_test_library_path)
+        set(_quic_task_test_environment
+                "LD_LIBRARY_PATH=${_quic_task_test_library_path}")
+        if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+            list(APPEND _quic_task_test_environment
+                    "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+        endif()
         set_tests_properties(quic_edge_task_test PROPERTIES
-                ENVIRONMENT "LD_LIBRARY_PATH=${_quic_task_test_library_path}")
+                TIMEOUT 10
+                ENVIRONMENT "${_quic_task_test_environment}")
     endif()
 endif()

cmvr-es/task/ume_teleop_task/CMakeLists.txt

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

cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h

@@ -0,0 +1,99 @@
+#ifndef CMVR_ES_UME_TELEOP_TASK_H
+#define CMVR_ES_UME_TELEOP_TASK_H
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <thread>
+
+#include "cmvr/config/ume_teleop_config/ume_teleop_config.pb.h"
+#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h"
+#include "task/task.h"
+
+namespace cmvr::task {
+
+class UmeTeleopTask final : public Task {
+public:
+    explicit UmeTeleopTask(
+        const config::UmeTeleopConfig& config,
+        std::shared_ptr<teleop::GrpcArmTeleopClient> client = {});
+    ~UmeTeleopTask() override;
+
+    const std::string& id() const override { return id_; }
+    TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; }
+    bool init() override;
+    bool start() override;
+    bool step(double dt) override;
+    void stop() override;
+
+    TaskState state() const override;
+    bool isBusy() const override;
+    bool isFinished() const override;
+    bool isFailed() const override;
+    std::string stateString() const override;
+    std::string detailStatusString() const override;
+
+    // Thread-safe, capacity-one command mailbox. The caller provides only
+    // already-computed joint values; sequence is assigned by the sender loop.
+    // A newer command replaces an unsent older command. Submission is rejected
+    // unless the current session is ready, and pending values are discarded
+    // across disconnect/reconnect so motion cannot resume from stale intent.
+    bool submitSetpoint(
+        const api::armteleop::v1::JointSetpoint& setpoint);
+
+private:
+    using Clock = std::chrono::steady_clock;
+
+    struct PendingSetpoint {
+        api::armteleop::v1::JointSetpoint value;
+        Clock::time_point submitted;
+    };
+
+    bool validateConfig(std::string& error) const;
+    void run();
+    void runSender();
+    void handleServerFrame(
+        const api::armteleop::v1::ServerFrame& frame);
+
+    config::UmeTeleopConfig config_;
+    std::string id_;
+    std::shared_ptr<teleop::GrpcArmTeleopClient> client_;
+
+    mutable std::mutex mutex_;
+    std::condition_variable stop_condition_;
+    std::thread worker_;
+    std::thread sender_;
+    std::atomic<bool> stop_requested_{false};
+    TaskState state_{TaskState::UNINITIALIZED};
+    std::string last_error_;
+    std::string session_id_;
+    std::string server_detail_;
+    api::armteleop::v1::SessionPhase server_phase_{
+        api::armteleop::v1::SESSION_PHASE_UNSPECIFIED};
+    std::uint64_t connection_attempts_{0};
+
+    bool receiver_session_active_{false};
+    bool session_ready_{false};
+    std::uint64_t client_session_generation_{0};
+    std::uint32_t negotiated_watchdog_ms_{0};
+
+    std::optional<PendingSetpoint> pending_setpoint_;
+    std::uint64_t mailbox_replacements_{0};
+    std::uint64_t stale_setpoints_dropped_{0};
+    std::uint64_t heartbeats_sent_{0};
+    std::uint64_t setpoints_sent_{0};
+
+    bool stop_write_attempted_{false};
+    bool stop_write_succeeded_{false};
+};
+
+void registerUmeTeleopTaskFactory();
+
+} // namespace cmvr::task
+
+#endif // CMVR_ES_UME_TELEOP_TASK_H

cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp

@@ -0,0 +1,651 @@
+#include "task/ume_teleop_task/include/ume_teleop_task.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <sstream>
+#include <utility>
+
+#include <grpcpp/create_channel.h>
+#include <grpcpp/security/credentials.h>
+
+#include "cmvr/config/task_manager_config/task_manager_config.pb.h"
+#include "common/base/logging/logger.h"
+#include "common/config/config_files.h"
+#include "task/task_factory.h"
+
+namespace cmvr::task {
+namespace {
+
+constexpr auto kStopWriteGrace = std::chrono::milliseconds(50);
+constexpr auto kStopAckGrace = std::chrono::milliseconds(20);
+
+std::shared_ptr<Task> createUmeTeleopTask(
+    const config::TaskConfigEntry& entry)
+{
+    if (entry.id().empty() || entry.config_file().empty()) {
+        CMVR_LOG(ERROR) << "[UmeTeleopTask] Task id or config_file is empty";
+        return nullptr;
+    }
+
+    config::UmeTeleopRootConfig root;
+    if (!ConfigHelper::loadConfigFile(entry.config_file(), root)) {
+        CMVR_LOG(ERROR) << "[UmeTeleopTask] Failed to load config: "
+                        << entry.config_file();
+        return nullptr;
+    }
+    const auto& config = root.ume_teleop();
+    if (config.id().empty() || config.id() != entry.id()) {
+        CMVR_LOG(ERROR) << "[UmeTeleopTask] Task ID mismatch: manager="
+                        << entry.id() << ", config=" << config.id();
+        return nullptr;
+    }
+    return std::make_shared<UmeTeleopTask>(config);
+}
+
+std::string grpcStatusDetail(const grpc::Status& status)
+{
+    std::ostringstream output;
+    output << "gRPC code=" << static_cast<int>(status.error_code());
+    if (!status.error_message().empty()) {
+        output << " message=" << status.error_message();
+    }
+    return output.str();
+}
+
+} // namespace
+
+UmeTeleopTask::UmeTeleopTask(
+    const config::UmeTeleopConfig& config,
+    std::shared_ptr<teleop::GrpcArmTeleopClient> client)
+    : config_(config), id_(config.id()), client_(std::move(client))
+{
+}
+
+UmeTeleopTask::~UmeTeleopTask()
+{
+    stop();
+}
+
+bool UmeTeleopTask::init()
+{
+    std::lock_guard lock(mutex_);
+    if (state_ == TaskState::IDLE) {
+        return true;
+    }
+    if (worker_.joinable() || sender_.joinable()) {
+        last_error_ = "cannot initialize while worker is running";
+        state_ = TaskState::FAILED;
+        return false;
+    }
+
+    std::string error;
+    if (!validateConfig(error)) {
+        last_error_ = std::move(error);
+        state_ = TaskState::FAILED;
+        return false;
+    }
+
+    if (!client_) {
+        auto channel = grpc::CreateChannel(
+            config_.server_address(),
+            grpc::InsecureChannelCredentials());
+        if (!channel) {
+            last_error_ = "failed to create gRPC channel";
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        client_ = std::make_shared<teleop::GrpcArmTeleopClient>(
+            std::move(channel));
+    }
+
+    stop_requested_ = false;
+    last_error_.clear();
+    session_id_.clear();
+    server_detail_.clear();
+    server_phase_ = api::armteleop::v1::SESSION_PHASE_UNSPECIFIED;
+    connection_attempts_ = 0;
+    receiver_session_active_ = false;
+    session_ready_ = false;
+    client_session_generation_ = 0;
+    negotiated_watchdog_ms_ = 0;
+    pending_setpoint_.reset();
+    mailbox_replacements_ = 0;
+    stale_setpoints_dropped_ = 0;
+    heartbeats_sent_ = 0;
+    setpoints_sent_ = 0;
+    stop_write_attempted_ = false;
+    stop_write_succeeded_ = false;
+    state_ = TaskState::IDLE;
+    return true;
+}
+
+bool UmeTeleopTask::start()
+{
+    std::unique_lock lock(mutex_);
+    if (state_ == TaskState::RUNNING) {
+        return true;
+    }
+    if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) ||
+        !client_ || worker_.joinable() || sender_.joinable()) {
+        last_error_ = "UME teleop task is not initialized";
+        state_ = TaskState::FAILED;
+        return false;
+    }
+
+    stop_requested_ = false;
+    session_ready_ = false;
+    client_session_generation_ = 0;
+    negotiated_watchdog_ms_ = 0;
+    pending_setpoint_.reset();
+    stop_write_attempted_ = false;
+    stop_write_succeeded_ = false;
+    try {
+        worker_ = std::thread(&UmeTeleopTask::run, this);
+        sender_ = std::thread(&UmeTeleopTask::runSender, this);
+    } catch (const std::exception& error) {
+        last_error_ = std::string("failed to start worker: ") + error.what();
+        stop_requested_ = true;
+        stop_condition_.notify_all();
+        auto client = client_;
+        std::thread worker;
+        std::thread sender;
+        if (worker_.joinable()) {
+            worker = std::move(worker_);
+        }
+        if (sender_.joinable()) {
+            sender = std::move(sender_);
+        }
+        lock.unlock();
+        client->tryCancel();
+        if (sender.joinable()) {
+            sender.join();
+        }
+        if (worker.joinable()) {
+            worker.join();
+        }
+        lock.lock();
+        state_ = TaskState::FAILED;
+        return false;
+    } catch (...) {
+        last_error_ = "failed to start worker";
+        stop_requested_ = true;
+        stop_condition_.notify_all();
+        auto client = client_;
+        std::thread worker;
+        std::thread sender;
+        if (worker_.joinable()) {
+            worker = std::move(worker_);
+        }
+        if (sender_.joinable()) {
+            sender = std::move(sender_);
+        }
+        lock.unlock();
+        client->tryCancel();
+        if (sender.joinable()) {
+            sender.join();
+        }
+        if (worker.joinable()) {
+            worker.join();
+        }
+        lock.lock();
+        state_ = TaskState::FAILED;
+        return false;
+    }
+
+    state_ = TaskState::RUNNING;
+    return true;
+}
+
+bool UmeTeleopTask::step(const double dt)
+{
+    (void)dt;
+    return !isFailed();
+}
+
+void UmeTeleopTask::stop()
+{
+    std::shared_ptr<teleop::GrpcArmTeleopClient> client;
+    std::thread worker;
+    std::thread sender;
+    {
+        std::unique_lock lock(mutex_);
+        stop_requested_ = true;
+        stop_condition_.notify_all();
+        client = client_;
+
+        // The sender is the only normal writer. Give it a bounded opportunity
+        // to put StopSession on the stream before cancellation interrupts a
+        // blocked Write or Read.
+        if (sender_.joinable()) {
+            stop_condition_.wait_for(
+                lock,
+                kStopWriteGrace,
+                [this] { return stop_write_attempted_; });
+            if (stop_write_succeeded_ && receiver_session_active_) {
+                stop_condition_.wait_for(
+                    lock,
+                    kStopAckGrace,
+                    [this] { return !receiver_session_active_; });
+            }
+            sender = std::move(sender_);
+        }
+        if (worker_.joinable()) {
+            worker = std::move(worker_);
+        }
+    }
+
+    // Do not hold the task mutex while cancelling or joining: the receive
+    // callback and the worker exit path both update task status under it.
+    if (client) {
+        client->tryCancel();
+    }
+    if (sender.joinable()) {
+        sender.join();
+    }
+    if (worker.joinable()) {
+        worker.join();
+    }
+
+    std::lock_guard lock(mutex_);
+    if (state_ != TaskState::FAILED) {
+        state_ = TaskState::STOPPED;
+    }
+}
+
+TaskState UmeTeleopTask::state() const
+{
+    std::lock_guard lock(mutex_);
+    return state_;
+}
+
+bool UmeTeleopTask::isBusy() const
+{
+    return state() == TaskState::RUNNING;
+}
+
+bool UmeTeleopTask::isFinished() const
+{
+    return state() == TaskState::STOPPED;
+}
+
+bool UmeTeleopTask::isFailed() const
+{
+    return state() == TaskState::FAILED;
+}
+
+std::string UmeTeleopTask::stateString() const
+{
+    return taskStateToString(state());
+}
+
+std::string UmeTeleopTask::detailStatusString() const
+{
+    std::lock_guard lock(mutex_);
+    std::ostringstream output;
+    output << taskStateToString(state_)
+           << " target=" << config_.server_address()
+           << " attempts=" << connection_attempts_
+           << " phase="
+           << api::armteleop::v1::SessionPhase_Name(server_phase_)
+           << " watchdog_ms=" << negotiated_watchdog_ms_
+           << " heartbeats=" << heartbeats_sent_
+           << " setpoints=" << setpoints_sent_
+           << " mailbox_replacements=" << mailbox_replacements_
+           << " stale_dropped=" << stale_setpoints_dropped_;
+    if (!session_id_.empty()) {
+        output << " session=" << session_id_;
+    }
+    if (!server_detail_.empty()) {
+        output << " server_detail=" << server_detail_;
+    }
+    if (!last_error_.empty()) {
+        output << " error=" << last_error_;
+    }
+    return output.str();
+}
+
+bool UmeTeleopTask::submitSetpoint(
+    const api::armteleop::v1::JointSetpoint& setpoint)
+{
+    if (setpoint.valid_for_us() == 0U) {
+        return false;
+    }
+
+    std::lock_guard lock(mutex_);
+    if (state_ != TaskState::RUNNING ||
+        stop_requested_.load(std::memory_order_acquire) ||
+        !session_ready_ ||
+        client_session_generation_ == 0U) {
+        return false;
+    }
+    if (pending_setpoint_.has_value()) {
+        ++mailbox_replacements_;
+    }
+
+    PendingSetpoint pending;
+    pending.value = setpoint;
+    // The Task owns the only session sequence generator.
+    pending.value.set_sequence(0);
+    pending.submitted = Clock::now();
+    pending_setpoint_ = std::move(pending);
+    stop_condition_.notify_all();
+    return true;
+}
+
+bool UmeTeleopTask::validateConfig(std::string& error) const
+{
+    if (id_.empty()) {
+        error = "UME teleop task id is empty";
+        return false;
+    }
+    if (config_.server_address().empty()) {
+        error = "UME teleop server_address is empty";
+        return false;
+    }
+    if (!config_.allow_insecure()) {
+        error =
+            "M6 requires explicit allow_insecure=true; TLS is not configured";
+        return false;
+    }
+
+    const auto& open = config_.open_session();
+    if (open.protocol_major() == 0U ||
+        open.client_instance_id().empty() ||
+        open.expected_robot().robot_id().empty() ||
+        open.expected_robot().joint_names().empty() ||
+        open.requested_command_rate_hz() == 0U ||
+        open.requested_state_rate_hz() == 0U ||
+        open.watchdog_timeout_ms() == 0U ||
+        open.requested_lease_ms() == 0U) {
+        error = "UME teleop OpenSession configuration is incomplete";
+        return false;
+    }
+
+    const auto& reconnect = config_.reconnect();
+    if (reconnect.initial_delay_ms() == 0U ||
+        reconnect.maximum_delay_ms() < reconnect.initial_delay_ms() ||
+        !std::isfinite(reconnect.multiplier()) ||
+        reconnect.multiplier() < 1.0) {
+        error = "UME teleop reconnect configuration is invalid";
+        return false;
+    }
+    return true;
+}
+
+void UmeTeleopTask::run()
+{
+    auto backoff =
+        std::chrono::milliseconds(config_.reconnect().initial_delay_ms());
+    const auto maximum_backoff =
+        std::chrono::milliseconds(config_.reconnect().maximum_delay_ms());
+
+    while (true) {
+        {
+            std::lock_guard lock(mutex_);
+            if (stop_requested_) {
+                break;
+            }
+            ++connection_attempts_;
+            receiver_session_active_ = true;
+            session_ready_ = false;
+            client_session_generation_ = 0;
+            negotiated_watchdog_ms_ = 0;
+            // A command produced for an old or disconnected session must
+            // never become the first motion command after reconnect.
+            pending_setpoint_.reset();
+            stop_condition_.notify_all();
+        }
+
+        const grpc::Status status = client_->runSession(
+            config_.open_session(),
+            [this](const api::armteleop::v1::ServerFrame& frame) {
+                handleServerFrame(frame);
+            },
+            [this] {
+                return stop_requested_.load(std::memory_order_acquire);
+            });
+
+        std::unique_lock lock(mutex_);
+        receiver_session_active_ = false;
+        session_ready_ = false;
+        client_session_generation_ = 0;
+        negotiated_watchdog_ms_ = 0;
+        pending_setpoint_.reset();
+        stop_condition_.notify_all();
+        if (stop_requested_) {
+            break;
+        }
+
+        last_error_ = status.ok()
+                          ? "arm teleop peer closed the session"
+                          : grpcStatusDetail(status);
+        if (stop_condition_.wait_for(
+                lock,
+                backoff,
+                [this] {
+                    return stop_requested_.load(std::memory_order_acquire);
+                })) {
+            break;
+        }
+
+        const double multiplied =
+            static_cast<double>(backoff.count()) *
+            config_.reconnect().multiplier();
+        const auto next_count = static_cast<std::int64_t>(
+            std::min(multiplied, static_cast<double>(maximum_backoff.count())));
+        backoff = std::chrono::milliseconds(std::max<std::int64_t>(1, next_count));
+    }
+}
+
+void UmeTeleopTask::runSender()
+{
+    const auto command_period = std::chrono::microseconds(
+        std::max<std::uint64_t>(
+            1U,
+            1000000ULL /
+                static_cast<std::uint64_t>(
+                    config_.open_session().requested_command_rate_hz())));
+
+    std::uint64_t observed_generation = 0;
+    std::uint64_t next_sequence = 0;
+    auto next_command_time = Clock::now();
+    auto next_heartbeat_time = Clock::time_point::max();
+
+    for (;;) {
+        std::optional<PendingSetpoint> command;
+        bool heartbeat = false;
+        bool stop = false;
+        std::uint64_t generation = 0;
+        std::uint64_t sequence = 0;
+        std::chrono::microseconds heartbeat_period{0};
+
+        {
+            std::unique_lock lock(mutex_);
+            for (;;) {
+                if (stop_requested_.load(std::memory_order_acquire)) {
+                    stop = true;
+                    generation = client_session_generation_;
+                    break;
+                }
+
+                if (!session_ready_ ||
+                    client_session_generation_ == 0 ||
+                    negotiated_watchdog_ms_ == 0U) {
+                    stop_condition_.wait(lock, [this] {
+                        return stop_requested_.load(
+                                   std::memory_order_acquire) ||
+                               (session_ready_ &&
+                                client_session_generation_ != 0 &&
+                                negotiated_watchdog_ms_ != 0U);
+                    });
+                    continue;
+                }
+
+                if (observed_generation != client_session_generation_) {
+                    observed_generation = client_session_generation_;
+                    next_sequence = 0;
+                    next_command_time = Clock::now();
+                    heartbeat_period = std::chrono::microseconds(
+                        std::max<std::uint64_t>(
+                            1000U,
+                            static_cast<std::uint64_t>(
+                                negotiated_watchdog_ms_) *
+                                1000ULL / 3ULL));
+                    next_heartbeat_time = Clock::now() + heartbeat_period;
+                } else {
+                    heartbeat_period = std::chrono::microseconds(
+                        std::max<std::uint64_t>(
+                            1000U,
+                            static_cast<std::uint64_t>(
+                                negotiated_watchdog_ms_) *
+                                1000ULL / 3ULL));
+                }
+
+                const auto now = Clock::now();
+                if (pending_setpoint_.has_value()) {
+                    const auto queued_age =
+                        std::chrono::duration_cast<std::chrono::microseconds>(
+                            now - pending_setpoint_->submitted);
+                    if (queued_age.count() >=
+                        pending_setpoint_->value.valid_for_us()) {
+                        pending_setpoint_.reset();
+                        ++stale_setpoints_dropped_;
+                    }
+                }
+
+                if (pending_setpoint_.has_value() &&
+                    now >= next_command_time) {
+                    command = std::move(pending_setpoint_);
+                    pending_setpoint_.reset();
+                    generation = client_session_generation_;
+                    sequence = ++next_sequence;
+                    break;
+                }
+                if (now >= next_heartbeat_time) {
+                    heartbeat = true;
+                    generation = client_session_generation_;
+                    sequence = ++next_sequence;
+                    break;
+                }
+
+                auto wake_time = next_heartbeat_time;
+                if (pending_setpoint_.has_value()) {
+                    wake_time = std::min(wake_time, next_command_time);
+                }
+                stop_condition_.wait_until(lock, wake_time);
+            }
+        }
+
+        if (stop) {
+            api::armteleop::v1::StopSession stop_frame;
+            stop_frame.set_reason(
+                api::armteleop::v1::STOP_REASON_CLIENT_SHUTDOWN);
+            stop_frame.set_detail("UME teleoperation task stopped");
+            const bool sent = client_->sendStop(stop_frame, generation);
+            {
+                std::lock_guard lock(mutex_);
+                stop_write_attempted_ = true;
+                stop_write_succeeded_ = sent;
+            }
+            stop_condition_.notify_all();
+            return;
+        }
+
+        bool sent = false;
+        if (command.has_value()) {
+            const auto queued_age =
+                std::chrono::duration_cast<std::chrono::microseconds>(
+                    Clock::now() - command->submitted);
+            const auto original_validity =
+                static_cast<std::uint64_t>(
+                    command->value.valid_for_us());
+            if (queued_age.count() >= 0 &&
+                static_cast<std::uint64_t>(queued_age.count()) <
+                    original_validity) {
+                command->value.set_sequence(sequence);
+                command->value.set_valid_for_us(
+                    static_cast<std::uint32_t>(
+                        original_validity -
+                        static_cast<std::uint64_t>(queued_age.count())));
+                sent = client_->sendSetpoint(
+                    command->value, generation);
+            } else {
+                std::lock_guard lock(mutex_);
+                ++stale_setpoints_dropped_;
+                continue;
+            }
+        } else if (heartbeat) {
+            api::armteleop::v1::ClientHeartbeat heartbeat_frame;
+            heartbeat_frame.set_sequence(sequence);
+            sent = client_->sendHeartbeat(
+                heartbeat_frame, generation);
+        }
+
+        const auto sent_at = Clock::now();
+        bool cancel_session = false;
+        {
+            std::lock_guard lock(mutex_);
+            if (generation == client_session_generation_) {
+                if (sent) {
+                    next_heartbeat_time =
+                        sent_at + heartbeat_period;
+                    if (command.has_value()) {
+                        ++setpoints_sent_;
+                        next_command_time =
+                            sent_at + command_period;
+                    } else if (heartbeat) {
+                        ++heartbeats_sent_;
+                    }
+                } else {
+                    session_ready_ = false;
+                    cancel_session = true;
+                }
+            }
+        }
+        if (cancel_session) {
+            stop_condition_.notify_all();
+            client_->tryCancel();
+        }
+    }
+}
+
+void UmeTeleopTask::handleServerFrame(
+    const api::armteleop::v1::ServerFrame& frame)
+{
+    if (!frame.has_status()) {
+        return;
+    }
+
+    const std::uint64_t generation =
+        client_->activeSessionGeneration();
+    std::lock_guard lock(mutex_);
+    server_phase_ = frame.status().phase();
+    session_id_ = frame.status().session_id();
+    server_detail_ = frame.status().detail();
+    if (server_phase_ == api::armteleop::v1::SESSION_PHASE_OPENED ||
+        server_phase_ == api::armteleop::v1::SESSION_PHASE_READY ||
+        server_phase_ == api::armteleop::v1::SESSION_PHASE_ACTIVE) {
+        last_error_.clear();
+        if (generation != 0 &&
+            frame.status().negotiated_watchdog_ms() != 0U) {
+            client_session_generation_ = generation;
+            negotiated_watchdog_ms_ =
+                frame.status().negotiated_watchdog_ms();
+            session_ready_ = true;
+        }
+    } else {
+        session_ready_ = false;
+        pending_setpoint_.reset();
+    }
+    stop_condition_.notify_all();
+}
+
+void registerUmeTeleopTaskFactory()
+{
+    TaskFactory::registerCreator(
+        config::TaskConfigEntry::TASK_TYPE_UME_TELEOP,
+        createUmeTeleopTask);
+}
+
+} // namespace cmvr::task

cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp

@@ -0,0 +1,429 @@
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdio>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#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"
+#include "task/ume_teleop_task/include/ume_teleop_task.h"
+
+namespace {
+
+using namespace std::chrono_literals;
+namespace api = cmvr::api::armteleop::v1;
+
+class WatchdogArmTeleopService final : public api::ArmTeleopService::Service {
+public:
+    explicit WatchdogArmTeleopService(
+        const std::chrono::milliseconds watchdog)
+        : watchdog_(watchdog)
+    {
+    }
+
+    grpc::Status Teleoperate(
+        grpc::ServerContext* context,
+        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;
+            last_activity_ = std::chrono::steady_clock::now();
+        }
+        condition_.notify_all();
+
+        api::ServerFrame opened;
+        opened.mutable_status()->set_session_id("task-test-session");
+        opened.mutable_status()->set_phase(api::SESSION_PHASE_OPENED);
+        opened.mutable_status()->set_negotiated_watchdog_ms(
+            static_cast<std::uint32_t>(watchdog_.count()));
+        if (!stream->Write(opened)) {
+            return grpc::Status::OK;
+        }
+
+        api::ServerFrame ready;
+        ready.mutable_status()->set_session_id("task-test-session");
+        ready.mutable_status()->set_phase(api::SESSION_PHASE_READY);
+        ready.mutable_status()->set_negotiated_watchdog_ms(
+            static_cast<std::uint32_t>(watchdog_.count()));
+        if (!stream->Write(ready)) {
+            return grpc::Status::OK;
+        }
+
+        std::thread reader([&] {
+            api::ClientFrame incoming;
+            while (stream->Read(&incoming)) {
+                const auto arrived = std::chrono::steady_clock::now();
+                bool terminal = false;
+                {
+                    std::lock_guard lock(mutex_);
+                    if (incoming.has_heartbeat() ||
+                        incoming.has_setpoint()) {
+                        const std::uint64_t sequence =
+                            incoming.has_heartbeat()
+                                ? incoming.heartbeat().sequence()
+                                : incoming.setpoint().sequence();
+                        if (sequence == 0 || sequence <= last_sequence_) {
+                            sequence_valid_ = false;
+                        }
+                        last_sequence_ = sequence;
+                        last_activity_ = arrived;
+                        ++activity_version_;
+                        if (incoming.has_heartbeat()) {
+                            ++heartbeat_count_;
+                        } else {
+                            setpoints_.push_back(incoming.setpoint());
+                        }
+                    } else if (incoming.has_stop()) {
+                        stop_received_ = true;
+                        terminal = true;
+                    }
+                    if (terminal) {
+                        reader_finished_ = true;
+                    }
+                }
+                condition_.notify_all();
+                incoming.Clear();
+                if (terminal) {
+                    break;
+                }
+            }
+            {
+                std::lock_guard lock(mutex_);
+                reader_finished_ = true;
+            }
+            condition_.notify_all();
+        });
+
+        bool expired = false;
+        {
+            std::unique_lock lock(mutex_);
+            std::uint64_t observed_activity = activity_version_;
+            while (!reader_finished_) {
+                const auto deadline = last_activity_ + watchdog_;
+                if (!condition_.wait_until(
+                        lock,
+                        deadline,
+                        [&] {
+                            return reader_finished_ ||
+                                   activity_version_ != observed_activity;
+                        })) {
+                    watchdog_expired_ = true;
+                    expired = true;
+                    break;
+                }
+                observed_activity = activity_version_;
+            }
+        }
+        if (expired) {
+            context->TryCancel();
+        }
+        if (reader.joinable()) {
+            reader.join();
+        }
+        {
+            std::lock_guard lock(mutex_);
+            handler_finished_ = true;
+        }
+        condition_.notify_all();
+        return expired
+                   ? grpc::Status(
+                         grpc::StatusCode::DEADLINE_EXCEEDED,
+                         "test watchdog expired")
+                   : 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 waitForHandlerFinish(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [this] { return handler_finished_; });
+    }
+
+    bool waitForHeartbeatCount(
+        const std::size_t count,
+        const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [&] { return heartbeat_count_ >= count; });
+    }
+
+    bool waitForSetpointCount(
+        const std::size_t count,
+        const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [&] { return setpoints_.size() >= count; });
+    }
+
+    std::size_t setpointCount() const
+    {
+        std::lock_guard lock(mutex_);
+        return setpoints_.size();
+    }
+
+    api::JointSetpoint lastSetpoint() const
+    {
+        std::lock_guard lock(mutex_);
+        return setpoints_.empty()
+                   ? api::JointSetpoint{}
+                   : setpoints_.back();
+    }
+
+    bool watchdogExpired() const
+    {
+        std::lock_guard lock(mutex_);
+        return watchdog_expired_;
+    }
+
+    bool sequenceValid() const
+    {
+        std::lock_guard lock(mutex_);
+        return sequence_valid_;
+    }
+
+    bool stopReceived() const
+    {
+        std::lock_guard lock(mutex_);
+        return stop_received_;
+    }
+
+private:
+    const std::chrono::milliseconds watchdog_;
+    mutable std::mutex mutex_;
+    std::condition_variable condition_;
+    bool open_received_{false};
+    bool reader_finished_{false};
+    bool handler_finished_{false};
+    bool watchdog_expired_{false};
+    bool sequence_valid_{true};
+    bool stop_received_{false};
+    std::uint64_t activity_version_{0};
+    std::uint64_t last_sequence_{0};
+    std::size_t heartbeat_count_{0};
+    std::vector<api::JointSetpoint> setpoints_;
+    std::chrono::steady_clock::time_point last_activity_{};
+};
+
+cmvr::config::UmeTeleopConfig validConfig(const std::string& endpoint)
+{
+    cmvr::config::UmeTeleopConfig config;
+    config.set_id("ume_teleop_test");
+    config.set_server_address(endpoint);
+    config.set_allow_insecure(true);
+
+    auto* open = config.mutable_open_session();
+    open->set_protocol_major(1);
+    open->set_protocol_minor(0);
+    open->set_client_instance_id("ume-task-test");
+    open->set_requested_command_rate_hz(20);
+    open->set_requested_state_rate_hz(250);
+    open->set_watchdog_timeout_ms(120);
+    open->set_requested_lease_ms(500);
+    open->mutable_expected_robot()->set_robot_id("test-arm");
+    open->mutable_expected_robot()->add_joint_names("joint1");
+
+    config.mutable_reconnect()->set_initial_delay_ms(10);
+    config.mutable_reconnect()->set_maximum_delay_ms(50);
+    config.mutable_reconnect()->set_multiplier(2.0);
+    return config;
+}
+
+int fail(const std::string& detail)
+{
+    std::cerr << "ume_teleop_task_test: " << detail << '\n';
+    return 1;
+}
+
+} // namespace
+
+int main()
+{
+    {
+        cmvr::config::UmeTeleopConfig invalid;
+        invalid.set_id("invalid");
+        cmvr::task::UmeTeleopTask task(invalid);
+        if (task.init() || task.state() != cmvr::task::TaskState::FAILED) {
+            return fail("invalid transport configuration was not rejected");
+        }
+        task.stop();
+        if (task.state() != cmvr::task::TaskState::FAILED) {
+            return fail("stop did not preserve an initialization failure");
+        }
+    }
+
+    {
+        // Exercise the start/stop race before a synchronous stream necessarily
+        // publishes its ClientContext. The Task's cancellation predicate closes
+        // this gap, while tryCancel() interrupts it once the context is visible.
+        const std::string unavailable_endpoint =
+            "unix:/tmp/cmvr_ume_teleop_unavailable_" +
+            std::to_string(static_cast<long long>(::getpid())) + ".sock";
+        auto channel = grpc::CreateChannel(
+            unavailable_endpoint,
+            grpc::InsecureChannelCredentials());
+        auto client =
+            std::make_shared<cmvr::teleop::GrpcArmTeleopClient>(channel);
+        cmvr::task::UmeTeleopTask task(
+            validConfig(unavailable_endpoint), client);
+        if (!task.init() || !task.start()) {
+            return fail("immediate-stop task did not initialize and start");
+        }
+        api::JointSetpoint disconnected_setpoint;
+        disconnected_setpoint.add_position_rad(1.0);
+        disconnected_setpoint.set_valid_for_us(100000);
+        if (task.submitSetpoint(disconnected_setpoint)) {
+            task.stop();
+            return fail(
+                "task accepted a motion command without a ready session");
+        }
+        const auto stop_begin = std::chrono::steady_clock::now();
+        task.stop();
+        if (std::chrono::steady_clock::now() - stop_begin > 2s ||
+            task.state() != cmvr::task::TaskState::STOPPED) {
+            return fail("immediate Task stop did not cancel and join promptly");
+        }
+    }
+
+    WatchdogArmTeleopService service(120ms);
+    const std::string socket_path =
+        "/tmp/cmvr_ume_teleop_task_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());
+    auto client =
+        std::make_shared<cmvr::teleop::GrpcArmTeleopClient>(channel);
+    cmvr::task::UmeTeleopTask task(validConfig(endpoint), client);
+
+    if (task.runMode() != cmvr::task::TaskRunMode::BLOCKING_SERVICE) {
+        server->Shutdown();
+        return fail("task is not a BLOCKING_SERVICE");
+    }
+    if (!task.init() || !task.start()) {
+        server->Shutdown();
+        return fail("valid task did not initialize and start");
+    }
+    if (!service.waitForOpen(2s)) {
+        task.stop();
+        server->Shutdown();
+        return fail("task worker did not open the teleoperation stream");
+    }
+
+    // No application commands are submitted here. Heartbeats alone must keep
+    // the session alive for longer than the negotiated watchdog.
+    if (!service.waitForHeartbeatCount(4, 2s) ||
+        service.watchdogExpired() || !task.isBusy()) {
+        task.stop();
+        server->Shutdown();
+        return fail("silent command stream did not survive via heartbeats");
+    }
+
+    api::JointSetpoint first;
+    first.set_sequence(999); // Task must replace caller-owned sequence values.
+    first.add_position_rad(1.0);
+    first.add_velocity_rad_s(0.0);
+    first.set_valid_for_us(100000);
+    if (!task.submitSetpoint(first) ||
+        !service.waitForSetpointCount(1, 2s)) {
+        task.stop();
+        server->Shutdown();
+        return fail("first mailbox setpoint was not sent");
+    }
+
+    // The sender rate is 20 Hz. These arrive inside one command period, so the
+    // capacity-one mailbox must publish only the newest value.
+    for (int value = 2; value <= 4; ++value) {
+        api::JointSetpoint setpoint;
+        setpoint.set_sequence(1000 + value);
+        setpoint.add_position_rad(static_cast<double>(value));
+        setpoint.add_velocity_rad_s(0.0);
+        setpoint.set_valid_for_us(100000);
+        if (!task.submitSetpoint(setpoint)) {
+            task.stop();
+            server->Shutdown();
+            return fail("latest-only mailbox rejected a valid setpoint");
+        }
+    }
+    if (!service.waitForSetpointCount(2, 2s)) {
+        task.stop();
+        server->Shutdown();
+        return fail("latest mailbox setpoint was not sent");
+    }
+    std::this_thread::sleep_for(70ms);
+    const api::JointSetpoint latest = service.lastSetpoint();
+    if (service.setpointCount() != 2 ||
+        latest.position_rad_size() != 1 ||
+        latest.position_rad(0) != 4.0 ||
+        latest.sequence() == 0 ||
+        latest.sequence() >= 1000) {
+        task.stop();
+        server->Shutdown();
+        return fail("mailbox did not collapse queued commands to the latest value");
+    }
+
+    const auto stop_begin = std::chrono::steady_clock::now();
+    task.stop();
+    const auto stop_elapsed = std::chrono::steady_clock::now() - stop_begin;
+    if (stop_elapsed > 2s) {
+        server->Shutdown();
+        return fail("Task stop did not TryCancel and join promptly");
+    }
+    if (task.state() != cmvr::task::TaskState::STOPPED ||
+        task.isBusy() || !task.isFinished()) {
+        server->Shutdown();
+        return fail("task did not reach STOPPED after joining its worker");
+    }
+    if (!service.waitForHandlerFinish(2s)) {
+        server->Shutdown();
+        return fail("server handler did not observe Task cancellation");
+    }
+    if (!service.stopReceived()) {
+        server->Shutdown();
+        return fail("Task did not send StopSession before TryCancel");
+    }
+    if (service.watchdogExpired() || !service.sequenceValid()) {
+        server->Shutdown();
+        return fail("heartbeat/setpoint sequence or watchdog contract failed");
+    }
+
+    server->Shutdown();
+    std::remove(socket_path.c_str());
+    std::cout << "ume_teleop_task_test: PASS\n";
+    return 0;
+}

docs/teleoperation/ume_cmvr_architecture.md

@@ -0,0 +1,111 @@
+# UME to CMVR-ES Teleoperation Architecture
+
+## Scope
+
+This document freezes the first implementation stage of the wired,
+cross-machine teleoperation path:
+
+- both edge computers run `cmvr_es`;
+- the UME computer owns the Damiao SocketCAN-FD interfaces;
+- `UmeRobotArm` is a `RobotArm` backend;
+- the UME computer performs the leader/follower model calculations;
+- the robot computer validates and executes joint-servo references;
+- the transport is a versioned gRPC bidirectional stream;
+- the original UME algorithm is migrated before any SEW fusion work.
+
+SEW fusion, passivity research, paper experiments, and physical human testing
+are deliberately outside this implementation stage.
+
+## Target process and ownership boundary
+
+```text
+UME cmvr_es
+  UmeRobotArm
+    Damiao SocketCAN-FD
+    UmeHapticLoop
+    local safety guard
+       |
+       +-- UmeLegacyAlgorithm
+       |
+  UmeTeleopTask
+    GrpcArmTeleopClient
+                  |
+                  | wired Ethernet / gRPC bidi stream
+                  v
+Robot cmvr_es
+  ArmTeleopService
+    control lease
+    latest-only command slot
+    watchdog
+    safe servo executor
+       |
+       v
+    RobotArm
+```
+
+The UME high-frequency loop never performs a network RPC. Network workers and
+hardware loops exchange only bounded latest-state snapshots.
+
+## Implemented boundary in this revision
+
+This revision establishes the device, algorithm-library, session-transport and
+robot-backend boundaries, but it intentionally does not connect them into a
+physical end-to-end controller:
+
+- `UmeRobotArm` owns one eight-axis Damiao bus and a bounded local torque loop.
+  It starts passive, requires an explicit fresh torque command before
+  `torqueOn`, and latches watchdog/transport faults.
+- a successful SocketCAN send means that the complete frame batch was accepted
+  by the local kernel before its deadline. The current MIT transport has no
+  reviewed Disable acknowledgement, so software must not describe that result
+  as actuator-confirmed torque-off.
+- the original UME dynamics, friction, stiction and haptic projection code is a
+  standalone tested library under `algorithms/controllers/ume_legacy`;
+- `UmeTeleopTask` owns reconnect, heartbeat, sequence and a capacity-one
+  outbound mailbox. Its `submitSetpoint()` input is deliberately an algorithm
+  boundary; no production source calls it yet;
+- the server-side `RobotArmTeleopBackend` is implemented and tested behind an
+  explicit capability gate. The current `MotorRobotArm` remains unavailable
+  because its `servoJ` path is sequential per joint rather than an accepted
+  atomic/timed group primitive;
+- server state is currently returned with session events. The configured
+  requested state rate is not yet an independent periodic publisher;
+- follower effort is validated and transported when a backend declares a
+  verified source, but it is not yet consumed by a UME haptic coordinator.
+
+Accordingly, this code is an M0-M9 fail-closed framework and original-algorithm
+migration, not a claim of runnable force-feedback teleoperation. The next
+integration step must add a concrete UME-to-follower retargeting producer,
+connect verified follower effort to the local haptic coordinator, and retain
+the high-frequency/network-thread separation above.
+
+## Safety invariants
+
+1. Opening a CAN interface never enables a motor.
+2. Clearing a fault never arms a motor.
+3. A reconnect creates a new session and never restores active motion.
+4. Cross-machine `steady_clock` values are diagnostic only. A receiver derives
+   command expiry from its local receive time plus `valid_for_us`.
+5. Commands are strictly increasing by sequence within one session.
+6. Queues on the cyclic path are latest-only and bounded.
+7. Invalid or stale follower effort ramps haptic feedback to zero.
+8. Model, joint order, units, and calibration hashes must match before motion.
+9. New physical hardware configurations remain disabled by default.
+10. A software stop does not replace an independent physical emergency stop.
+11. UME hardware enable also requires an explicit firmware-reviewed feedback
+    status whitelist and raw temperature thresholds; empty values never mean
+    "accept all".
+12. UME shutdown timing is diagnosed against a configured budget, but physical
+    torque removal still requires an independent emergency-stop path until a
+    reviewed actuator Disable acknowledgement exists.
+
+## Initial rate boundary
+
+- UME local hardware/haptic loop: configurable, initially 800 Hz to match the
+  legacy UME setup.
+- network command rate: configurable independently of the local loop.
+- robot servo rate: selected from the robot backend capability and never
+  inferred from the network rate.
+
+No hard real-time or stability claim is made until the target computers and
+physical devices have completed staged validation.

docs/teleoperation/ume_cmvr_validation.md

@@ -0,0 +1,118 @@
+# UME / CMVR-ES validation gates
+
+This checklist is part of the first UME migration. Passing a software gate
+does not authorize a physical power-on. The checked-in UME device entries and
+their `hardware_enabled` fields remain `false`.
+
+The current revision has no production setpoint producer for
+`UmeTeleopTask::submitSetpoint()` and no haptic consumer for returned follower
+effort. M9 therefore validates the framework, protocol and migrated original
+algorithm separately; it is not an end-to-end motion or force-feedback test.
+
+## M9: software and network validation
+
+Run these gates on both target CPU architectures before deploying:
+
+1. Build the complete `cmvr_es` target with tests enabled.
+2. Run the UME legacy controller and Pinocchio model-adapter golden tests.
+3. Run the Damiao codec, CAN-FD chain, and `UmeRobotArm` lifecycle tests.
+4. Run the process-wide control-authority tests.
+5. Run the gRPC client, `UmeTeleopTask`, and `ArmTeleopService` tests.
+6. Repeat the concurrent client/task/service tests to screen for shutdown and
+   reconnect races.
+7. Start each checked-in edge profile without UME hardware and verify that it
+   never opens `can4`/`can5` or issues actuator enable frames.
+
+The communication tests must demonstrate all of the following:
+
+- an `OpenSession` manifest mismatch is rejected before backend activation;
+- a second controller cannot acquire the same robot control resource;
+- sequence numbers are non-zero and strictly increasing per session;
+- the sender and receiver use capacity-one, latest-only command storage;
+- a setpoint whose local validity has expired is never dispatched;
+- heartbeat loss, lease loss, stream cancellation, and backend failure call
+  the robot safe-stop boundary;
+- reconnect clears pending motion intent and starts a new sequence space;
+- `StopSession` is attempted before client cancellation;
+- the legacy unary ArmService cannot issue motion, enable, calibration, or
+  fault-reset commands while the teleoperation lease is active;
+- `torqueOff` and `stopMotion` remain available as safety overrides.
+
+Before a real follower backend can be enabled, control authority must also be
+extended to every local arm task and to the underlying MotorService resources.
+The current process-wide lease covers ArmTeleop and unary ArmService only.
+
+For a wired two-computer run, record at least:
+
+- one-way command age at the robot ingress;
+- command mailbox overwrite and rejection counters;
+- heartbeat and control-lease remaining time;
+- servo apply duration and deadline misses;
+- disconnect detection-to-safe-stop time;
+- packet loss, reordering, and delay from an explicit network impairment
+  profile rather than an assumed LAN condition.
+
+No end-to-end stability or transparency claim is supported until those logs
+are tied to a specified controller rate, robot servo period, payload, motion
+envelope, and network impairment profile.
+
+## M10: staged physical commissioning
+
+Every row is a separate sign-off. Do not combine first power-on with a human
+wearing the UME.
+
+- [ ] Independent physical emergency stop is installed and verified.
+- [ ] CAN arbitration/data bitrates and CAN-FD+BRS MTU are verified for each
+      interface.
+- [ ] Motor product, firmware, command ID, feedback ID, and reported motor ID
+      are read back and matched to the configuration.
+- [ ] The four-bit Damiao feedback status meanings and raw temperature limits
+      are verified for the exact product/firmware and entered as an explicit
+      per-joint whitelist/threshold contract.
+- [ ] Joint direction and zero reference are verified one joint at a time with
+      the mechanism unloaded.
+- [ ] Mechanical position, velocity, and torque limits replace the checked-in
+      placeholders and receive an independent review.
+- [ ] Motor feedback timestamps and the 800 Hz cycle are measured on the UME
+      target computer under load.
+- [ ] The exact Damiao firmware's Disable acknowledgement semantics are
+      documented and verified. Until then, SocketCAN send success is only
+      evidence that the local kernel accepted the complete frame batch.
+- [ ] Because MIT feedback has no sequence field, stale request/reply
+      correlation is resolved by a reviewed firmware marker or by measured,
+      enforced bus timing; draining only the frames already queued is not
+      sufficient evidence.
+- [ ] Every UME control-thread I/O operation is shown to be deadline-bounded
+      and interruptible on the target kernel. The configured shutdown timeout
+      is currently a diagnostic failure threshold, not a C++ timed-join
+      primitive.
+- [ ] With torque disabled, both edge profiles run for at least 30 minutes
+      without sequence, deadline, lease, or reconnect anomalies.
+- [ ] With the UME fixed to a stand, each joint is enabled independently at a
+      low torque limit and its watchdog disable path is measured.
+- [ ] Both UME arms are tested together on stands; CAN and CPU deadline margins
+      are recorded.
+- [ ] The robot backend's group `servoJ` semantics and worst-case call duration
+      are measured. Sequential per-joint dispatch is not accepted as an
+      atomic group backend without a documented skew bound.
+- [ ] gRPC writer backpressure cannot stall the independent robot watchdog,
+      and an expired lease cannot be regranted until safe stop is confirmed.
+- [ ] TouchScreenTask and direct MotorService commands are either disabled by
+      the deployment profile or participate in the same resource authority.
+- [ ] Robot-only low-speed setpoint execution is validated before connecting
+      the leader-side algorithm.
+- [ ] Wired-network cable removal, peer process kill, delayed packets, stale
+      commands, duplicate sequences, lease theft, and robot fault injection
+      all lead to a bounded safe stop.
+- [ ] The original UME gravity/friction/feedback controller is commissioned on
+      a stand with force feedback initially clamped to zero, then increased in
+      reviewed steps.
+- [ ] Only after all previous evidence is archived may a supervised human test
+      be considered under a separate risk assessment.
+
+## Evidence record
+
+For each completed physical gate, archive the exact Git revision, installed
+`output/` checksum, configuration files, model and calibration SHA-256 values,
+test operator, hardware serial numbers, raw logs, and pass/fail decision. A
+successful build or simulator run must not be recorded as physical validation.

model/ume/README.md

@@ -0,0 +1,28 @@
+# UME legacy dynamics models
+
+These MJCF files are exact copies from Universal-Manipulation-Exoskeleton
+commit `e087df5cd3b281418722e155d9975695f163698e`:
+
+- `v6_bimanual/robot.xml`: fixed-base model used for
+  `LOCAL_WORLD_ALIGNED` shoulder/wrist rotational Jacobians.
+  SHA-256:
+  `c185fe505ab52275d1be9c5b563df5c6505a3da81e0e622cffd66324e3839f3b`.
+- `v6_imu/robot.xml`: floating-base model used for the original
+  `rnea(q, v, 0)` compensation path.
+  SHA-256:
+  `db1dad82ebca9413edf268a980c5be5a307c990b86654db840098095d8bd5e14`.
+
+The source paths are
+`ume/robot/ume/v6_bimanual/models/robot.xml` and
+`ume/robot/ume/v6_imu/models/robot.xml`, respectively.
+
+Only the model topology/dynamics parser is used. Pinocchio 3.6
+`mjcf::buildModel` and the adapter tests load these files without resolving
+their visual STL assets, so no geometry files are duplicated here.
+
+The adapter enforces the model's structural contract. Integrations that require
+byte-for-byte model identity must also compare the SHA-256 values above before
+arming motion.
+
+The repository's root install rule copies `model/` to the deployment tree at
+`bin/model/`.

model/ume/v6_bimanual/robot.xml

@@ -0,0 +1,356 @@
+<?xml version="1.0" ?>
+<mujoco model="models">
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <default>
+    <default class="models">
+      <joint frictionloss="0.1" armature="0.005"/>
+      <position kp="50" dampratio="1"/>
+      <default class="visual">
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
+      </default>
+      <default class="collision">
+        <geom group="3"/>
+      </default>
+    </default>
+  </default>
+  <worldbody>
+    <!-- Link v5_4040_j1 -->
+    <body name="v5_4040_j1" pos="0 0 0" quat="1 0 0 0" childclass="models">
+      <inertial pos="-0.00725532 0.0615 -0" mass="1.044" fullinertia="0.0381441 0.000626549 0.0381676 -0 -0 6e-11"/>
+      <!-- Part dm_j4340_2ec -->
+      <geom type="mesh" class="visual" pos="0.015 0.2615 -0" quat="0 -0.707107 0 0.707107" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <geom type="mesh" class="collision" pos="0.015 0.2615 -0" quat="0 -0.707107 0 0.707107" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <!-- Part v5_4040_j1 -->
+      <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 0 0" mesh="v5_4040_j1" material="v5_4040_j1_material"/>
+      <geom type="mesh" class="collision" pos="0 0 0" quat="1 0 0 0" mesh="v5_4040_j1" material="v5_4040_j1_material"/>
+      <!-- Part dm_j4340_2ec_2 -->
+      <geom type="mesh" class="visual" pos="0.015 -0.1385 0" quat="0.707107 0 -0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <geom type="mesh" class="collision" pos="0.015 -0.1385 0" quat="0.707107 0 -0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <!-- Part 4040x30cm -->
+      <geom type="mesh" class="visual" pos="-0 0.2115 -0" quat="0.707107 0.707107 -0 -0" mesh="4040x30cm" material="4040x30cm_material"/>
+      <geom type="mesh" class="collision" pos="-0 0.2115 -0" quat="0.707107 0.707107 -0 -0" mesh="4040x30cm" material="4040x30cm_material"/>
+      <!-- Part v5_4040_j1_2 -->
+      <geom type="mesh" class="visual" pos="0 0.123 -0" quat="0 1 0 0" mesh="v5_4040_j1" material="v5_4040_j1_material"/>
+      <geom type="mesh" class="collision" pos="0 0.123 -0" quat="0 1 0 0" mesh="v5_4040_j1" material="v5_4040_j1_material"/>
+      <!-- Link v6_j1_20 -->
+      <body name="v6_j1_20" pos="-0.0383 -0.1385 0" quat="0.183013 0.683013 -0.183013 -0.683013">
+        <!-- Joint from v5_4040_j1 to v6_j1_20 -->
+        <joint axis="0 0 1" name="RJ1" type="hinge"/>
+        <inertial pos="0.150386 -5.99e-10 -0.0862657" mass="0.64749" fullinertia="0.0032465 0.00514141 0.00215547 6e-12 0.00180143 1.8e-11"/>
+        <!-- Part v6_20_j2 -->
+        <geom type="mesh" class="visual" pos="-0.0081 -0 0.036052" quat="0 -1 0 0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <geom type="mesh" class="collision" pos="-0.0081 -0 0.036052" quat="0 -1 0 0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <!-- Part v6_j1_20 -->
+        <geom type="mesh" class="visual" pos="0 -0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <geom type="mesh" class="collision" pos="0 -0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <!-- Part 2020x20cm -->
+        <geom type="mesh" class="visual" pos="0.2253 -0 0.018" quat="0 -0.707107 0 0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <geom type="mesh" class="collision" pos="0.2253 -0 0.018" quat="0 -0.707107 0 0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <!-- Part dm_j4340_2ec_3 -->
+        <geom type="mesh" class="visual" pos="0.1532 -0 -0.139098" quat="0.707107 -0 0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <geom type="mesh" class="collision" pos="0.1532 -0 -0.139098" quat="0.707107 -0 0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <!-- Part 2020x10cm -->
+        <geom type="mesh" class="visual" pos="0.1653 -0 -0.092" quat="0.707107 0 0 0.707107" mesh="2020x10cm" material="2020x10cm_material"/>
+        <geom type="mesh" class="collision" pos="0.1653 -0 -0.092" quat="0.707107 0 0 0.707107" mesh="2020x10cm" material="2020x10cm_material"/>
+        <!-- Link v6_20_j3 -->
+        <body name="v6_20_j3" pos="0.2065 -0 -0.139098" quat="0.5 0.5 0.5 0.5">
+          <!-- Joint from v6_j1_20 to v6_20_j3 -->
+          <joint axis="0 0 1" name="RJ2" type="hinge"/>
+          <inertial pos="0.0816108 2.273e-09 -0.108365" mass="0.6629" fullinertia="0.00347714 0.00399968 0.000810527 1.64e-10 0.00105864 -1.66e-10"/>
+          <!-- Part v6_j2_20 -->
+          <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <!-- Part 2020x20cm_2 -->
+          <geom type="mesh" class="visual" pos="0.127501 -0 -0.135474" quat="0.683013 0.183013 -0.183013 -0.683013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <geom type="mesh" class="collision" pos="0.127501 -0 -0.135474" quat="0.683013 0.183013 -0.183013 -0.683013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <!-- Part v6_20_j3 -->
+          <geom type="mesh" class="visual" pos="-0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <geom type="mesh" class="collision" pos="-0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <!-- Part dm_j4340_2ec_4 -->
+          <geom type="mesh" class="visual" pos="0.114169 0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <geom type="mesh" class="collision" pos="0.114169 0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <!-- Link v6_j3_2020_r -->
+          <body name="v6_j3_2020_r" pos="0.0680103 -0 -0.175034" quat="0.353553 0.612372 -0.612372 -0.353553">
+            <!-- Joint from v6_20_j3 to v6_j3_2020_r -->
+            <joint axis="0 0 1" name="RJ3" type="hinge"/>
+            <inertial pos="-0.0212538 0.197218 0.0388781" mass="0.86248" fullinertia="0.00735386 0.00260883 0.00681043 -0.00104227 0.000890554 0.000819778"/>
+            <!-- Part 2020x20cm_3 -->
+            <geom type="mesh" class="visual" pos="-0.0681202 0.2685 0.078682" quat="0.707107 0.707107 0 -0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <geom type="mesh" class="collision" pos="-0.0681202 0.2685 0.078682" quat="0.707107 0.707107 0 -0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <!-- Part v6_2020_j4_r -->
+            <geom type="mesh" class="visual" pos="-0.0681202 0.2885 0.078682" quat="0.5 0.5 -0.5 0.5" mesh="v6_2020_j4_r" material="v6_2020_j4_r_material"/>
+            <geom type="mesh" class="collision" pos="-0.0681202 0.2885 0.078682" quat="0.5 0.5 -0.5 0.5" mesh="v6_2020_j4_r" material="v6_2020_j4_r_material"/>
+            <!-- Part dm_j4340_2ec_5 -->
+            <geom type="mesh" class="visual" pos="0.00787981 0.26 0.027332" quat="0 -1 0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <geom type="mesh" class="collision" pos="0.00787981 0.26 0.027332" quat="0 -1 0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <!-- Part v6_j3_2020_r -->
+            <geom type="mesh" class="visual" pos="-0.0286202 0.0885 0.028682" quat="0.5 0.5 0.5 -0.5" mesh="v6_j3_2020_r" material="v6_j3_2020_r_material"/>
+            <geom type="mesh" class="collision" pos="-0.0286202 0.0885 0.028682" quat="0.5 0.5 0.5 -0.5" mesh="v6_j3_2020_r" material="v6_j3_2020_r_material"/>
+            <!-- Frame R_shoulder -->
+            <site group="3" name="R_shoulder" pos="-0.000120192 -0 0.078682" quat="0.5 0.5 0.5 0.5"/>
+            <!-- Link v6_j4_j5_r -->
+            <body name="v6_j4_j5_r" pos="0.00787981 0.26 -0.025968" quat="0 0.707107 -0.707107 -0">
+              <!-- Joint from v6_j3_2020_r to v6_j4_j5_r -->
+              <joint axis="0 0 1" name="RJ4" type="hinge"/>
+              <inertial pos="-0.054697 0.00944171 -0.00245461" mass="0.49139" fullinertia="0.00105657 0.00119188 0.000538644 -2.97344e-05 -0.000120659 0.000177465"/>
+              <!-- Part dm_j4310_2ec -->
+              <geom type="mesh" class="visual" pos="-0.0431803 0 0.0244282" quat="0.348175 0.615447 -0.615447 -0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <geom type="mesh" class="collision" pos="-0.0431803 0 0.0244282" quat="0.348175 0.615447 -0.615447 -0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <!-- Part v6_j4_j5_r -->
+              <geom type="mesh" class="visual" pos="-0.0285 0.0285 0" quat="0.5 0.5 0.5 -0.5" mesh="v6_j4_j5_r" material="v6_j4_j5_r_material"/>
+              <geom type="mesh" class="collision" pos="-0.0285 0.0285 0" quat="0.5 0.5 0.5 -0.5" mesh="v6_j4_j5_r" material="v6_j4_j5_r_material"/>
+              <!-- Link v6_wrist_j5_j6 -->
+              <body name="v6_wrist_j5_j6" pos="-0.0825226 -0 0.000785249" quat="0 -0.870373 0 0.492393">
+                <!-- Joint from v6_j4_j5_r to v6_wrist_j5_j6 -->
+                <joint axis="0 0 1" name="RJ5" type="hinge"/>
+                <inertial pos="-0.145145 7.46224e-05 0.168843" mass="0.4484" fullinertia="0.00261478 0.00340573 0.00098092 -2.29433e-06 0.00132169 3.71339e-06"/>
+                <!-- Part v6_wrist_j5_j6 -->
+                <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 -0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 -0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <!-- Part v6_xt30_2_2_50cm -->
+                <geom type="mesh" class="visual" pos="-0.0816102 0.005 0.0643898" quat="0.270598 0.270598 0.653281 -0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <geom type="mesh" class="collision" pos="-0.0816102 0.005 0.0643898" quat="0.270598 0.270598 0.653281 -0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <!-- Part dm_j4310_2ec_2 -->
+                <geom type="mesh" class="visual" pos="-0.146 -0 0.212" quat="0.5 -0.5 -0.5 0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <geom type="mesh" class="collision" pos="-0.146 -0 0.212" quat="0.5 -0.5 -0.5 0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <!-- Link v5_wrist_j6_j7 -->
+                <body name="v5_wrist_j6_j7" pos="-0.1919 0 0.212" quat="0 -0.707107 -0 0.707107">
+                  <!-- Joint from v6_wrist_j5_j6 to v5_wrist_j6_j7 -->
+                  <joint axis="0 0 1" name="RJ6" type="hinge"/>
+                  <inertial pos="-0 0.0975631 -0.161926" mass="0.407" fullinertia="0.00180585 0.00164708 0.000326757 0 -0 0.000247612"/>
+                  <!-- Part v5_wrist_j6_j7 -->
+                  <geom type="mesh" class="visual" pos="0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <geom type="mesh" class="collision" pos="0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <!-- Part dm_j4310_2ec_3 -->
+                  <geom type="mesh" class="visual" pos="0 0.121 -0.191" quat="0.707107 0.707107 0 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <geom type="mesh" class="collision" pos="0 0.121 -0.191" quat="0.707107 0.707107 0 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <!-- Part v6_xt30_2_2_50cm_2 -->
+                  <geom type="mesh" class="visual" pos="-0 0.121 -0.0925" quat="0.707107 0.707107 0 -0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <geom type="mesh" class="collision" pos="-0 0.121 -0.0925" quat="0.707107 0.707107 0 -0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <!-- Link v5_wrist_j7_leader_gripper_r -->
+                  <body name="v5_wrist_j7_leader_gripper_r" pos="-0 0.0751 -0.191" quat="0.5 0.5 -0.5 0.5">
+                    <!-- Joint from v5_wrist_j6_j7 to v5_wrist_j7_leader_gripper_r -->
+                    <joint axis="0 0 1" name="RJ7" type="hinge"/>
+                    <inertial pos="-0.00205487 0.0647313 0.154012" mass="0.4573" fullinertia="0.00200291 0.00193779 0.000443531 -3.45159e-05 2.74273e-06 -0.000439089"/>
+                    <!-- Part v6_xt30_2_2_50cm_3 -->
+                    <geom type="mesh" class="visual" pos="0.025 0.0547059 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <geom type="mesh" class="collision" pos="0.025 0.0547059 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <!-- Part v5_wrist_j7_leader_gripper_r -->
+                    <geom type="mesh" class="visual" pos="0 0 0" quat="1 0 -0 -0" mesh="v5_wrist_j7_leader_gripper_r" material="v5_wrist_j7_leader_gripper_r_material"/>
+                    <geom type="mesh" class="collision" pos="0 0 0" quat="1 0 -0 -0" mesh="v5_wrist_j7_leader_gripper_r" material="v5_wrist_j7_leader_gripper_r_material"/>
+                    <!-- Part dm_j4310_2ec_4 -->
+                    <geom type="mesh" class="visual" pos="0 0.075 0.21" quat="0 -0.707107 0.707107 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <geom type="mesh" class="collision" pos="0 0.075 0.21" quat="0 -0.707107 0.707107 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <!-- Frame R_wrist -->
+                    <site group="3" name="R_wrist" pos="0 0 0.0751" quat="0.707107 -0 -0 0.707107"/>
+                    <!-- Link v5_j8_4finger -->
+                    <body name="v5_j8_4finger" pos="0 0.075 0.1641" quat="0 -0.707107 -0.707107 -0">
+                      <!-- Joint from v5_wrist_j7_leader_gripper_r to v5_j8_4finger -->
+                      <joint axis="0 0 1" name="RJ8" type="hinge"/>
+                      <inertial pos="0.0582262 -2.729e-09 0.031412" mass="0.0384" fullinertia="3.92746e-05 6.14789e-05 2.96973e-05 -1e-12 -1.19931e-05 2e-12"/>
+                      <!-- Part v5_j8_4finger -->
+                      <geom type="mesh" class="visual" pos="0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                      <geom type="mesh" class="collision" pos="0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <!-- Link dm_j4340_2ec -->
+      <body name="dm_j4340_2ec" pos="-0.0383 0.2615 -0" quat="0.183013 -0.683013 -0.183013 0.683013">
+        <!-- Joint from v5_4040_j1 to dm_j4340_2ec -->
+        <joint axis="0 0 1" name="LJ1" type="hinge"/>
+        <inertial pos="0.150386 -5.99e-10 -0.0862657" mass="0.64749" fullinertia="0.0032465 0.00514141 0.00215547 6e-12 0.00180143 1.8e-11"/>
+        <!-- Part v6_j1_20_2 -->
+        <geom type="mesh" class="visual" pos="0 -0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <geom type="mesh" class="collision" pos="0 -0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <!-- Part 2020x10cm_2 -->
+        <geom type="mesh" class="visual" pos="0.1653 0 -0.092" quat="0 0 -0 1" mesh="2020x10cm" material="2020x10cm_material"/>
+        <geom type="mesh" class="collision" pos="0.1653 0 -0.092" quat="0 0 -0 1" mesh="2020x10cm" material="2020x10cm_material"/>
+        <!-- Part dm_j4340_2ec_6 -->
+        <geom type="mesh" class="visual" pos="0.1532 -0 -0.139098" quat="0.707107 -0 0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <geom type="mesh" class="collision" pos="0.1532 -0 -0.139098" quat="0.707107 -0 0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <!-- Part 2020x20cm_4 -->
+        <geom type="mesh" class="visual" pos="0.2253 -0 0.018" quat="0 -0.707107 -0 0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <geom type="mesh" class="collision" pos="0.2253 -0 0.018" quat="0 -0.707107 -0 0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <!-- Part v6_20_j2_2 -->
+        <geom type="mesh" class="visual" pos="-0.0081 -0 0.036052" quat="0 1 0 -0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <geom type="mesh" class="collision" pos="-0.0081 -0 0.036052" quat="0 1 0 -0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <!-- Link dm_j4340_2ec_2 -->
+        <body name="dm_j4340_2ec_2" pos="0.2065 0 -0.139098" quat="0.5 -0.5 0.5 -0.5">
+          <!-- Joint from dm_j4340_2ec to dm_j4340_2ec_2 -->
+          <joint axis="0 0 1" name="LJ2" type="hinge"/>
+          <inertial pos="0.0816108 2.273e-09 -0.108365" mass="0.6629" fullinertia="0.00347714 0.00399968 0.000810527 1.64e-10 0.00105864 -1.66e-10"/>
+          <!-- Part v6_j2_20_2 -->
+          <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <!-- Part v6_20_j3_2 -->
+          <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <!-- Part 2020x20cm_5 -->
+          <geom type="mesh" class="visual" pos="0.0275006 -0 0.0377311" quat="0.183013 0.683013 0.683013 0.183013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <geom type="mesh" class="collision" pos="0.0275006 -0 0.0377311" quat="0.183013 0.683013 0.683013 0.183013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <!-- Part dm_j4340_2ec_7 -->
+          <geom type="mesh" class="visual" pos="0.114169 -0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <geom type="mesh" class="collision" pos="0.114169 -0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <!-- Link v6_j3_2020_l -->
+          <body name="v6_j3_2020_l" pos="0.0680103 -0 -0.175034" quat="0.353553 -0.612372 -0.612372 0.353553">
+            <!-- Joint from dm_j4340_2ec_2 to v6_j3_2020_l -->
+            <joint axis="0 0 1" name="LJ3" type="hinge"/>
+            <inertial pos="-0.0211335 -0.197218 0.0388781" mass="0.86248" fullinertia="0.00735383 0.00260882 0.0068104 0.00104226 0.00089055 -0.00081978"/>
+            <!-- Part v6_j3_2020_l -->
+            <geom type="mesh" class="visual" pos="-0.0285 -0.0885 0.248682" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j3_2020_l" material="v6_j3_2020_l_material"/>
+            <geom type="mesh" class="collision" pos="-0.0285 -0.0885 0.248682" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j3_2020_l" material="v6_j3_2020_l_material"/>
+            <!-- Part dm_j4340_2ec_8 -->
+            <geom type="mesh" class="visual" pos="0.008 -0.26 0.027332" quat="0 -1 -0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <geom type="mesh" class="collision" pos="0.008 -0.26 0.027332" quat="0 -1 -0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <!-- Part 2020x20cm_6 -->
+            <geom type="mesh" class="visual" pos="-0.068 -0.2685 0.078682" quat="0.707107 -0.707107 0 0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <geom type="mesh" class="collision" pos="-0.068 -0.2685 0.078682" quat="0.707107 -0.707107 0 0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <!-- Part v6_2020_j4_l -->
+            <geom type="mesh" class="visual" pos="-0.068 -0.2885 0.214682" quat="0.5 -0.5 0.5 0.5" mesh="v6_2020_j4_l" material="v6_2020_j4_l_material"/>
+            <geom type="mesh" class="collision" pos="-0.068 -0.2885 0.214682" quat="0.5 -0.5 0.5 0.5" mesh="v6_2020_j4_l" material="v6_2020_j4_l_material"/>
+            <!-- Frame L_shoulder -->
+            <site group="3" name="L_shoulder" pos="0 0 0.078682" quat="0.5 -0.5 0.5 -0.5"/>
+            <!-- Link v6_j4_j5_l -->
+            <body name="v6_j4_j5_l" pos="0.008 -0.26 -0.025968" quat="0 0.707107 0.707107 0">
+              <!-- Joint from v6_j3_2020_l to v6_j4_j5_l -->
+              <joint axis="0 0 1" name="LJ4" type="hinge"/>
+              <inertial pos="-0.054697 -0.0094417 -0.00245459" mass="0.49139" fullinertia="0.00105657 0.00119187 0.000538643 2.97343e-05 -0.000120659 -0.000177465"/>
+              <!-- Part v6_j4_j5_l -->
+              <geom type="mesh" class="visual" pos="-0.0285 -0.0285 -0.3253" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j4_j5_l" material="v6_j4_j5_l_material"/>
+              <geom type="mesh" class="collision" pos="-0.0285 -0.0285 -0.3253" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j4_j5_l" material="v6_j4_j5_l_material"/>
+              <!-- Part dm_j4310_2ec_5 -->
+              <geom type="mesh" class="visual" pos="-0.0431803 -0 0.0244282" quat="0.348175 -0.615447 -0.615447 0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <geom type="mesh" class="collision" pos="-0.0431803 -0 0.0244282" quat="0.348175 -0.615447 -0.615447 0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <!-- Link v6_wrist_j5_j6_2 -->
+              <body name="v6_wrist_j5_j6_2" pos="-0.0825226 -0 0.000785249" quat="0 -0.870373 0 0.492393">
+                <!-- Joint from v6_j4_j5_l to v6_wrist_j5_j6_2 -->
+                <joint axis="0 0 1" name="LJ5" type="hinge"/>
+                <inertial pos="-0.145035 -0.000119401 0.168953" mass="0.4484" fullinertia="0.00260463 0.00340197 0.000987304 3.35036e-06 0.00132357 -5.25689e-06"/>
+                <!-- Part v6_wrist_j5_j6_2 -->
+                <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 -0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 -0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <!-- Part dm_j4310_2ec_6 -->
+                <geom type="mesh" class="visual" pos="-0.146 -0 0.212" quat="0.5 0.5 -0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <geom type="mesh" class="collision" pos="-0.146 -0 0.212" quat="0.5 0.5 -0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <!-- Part v6_xt30_2_2_50cm_4 -->
+                <geom type="mesh" class="visual" pos="-0.0787817 -0.005 0.0672183" quat="0.270598 -0.270598 0.653281 0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <geom type="mesh" class="collision" pos="-0.0787817 -0.005 0.0672183" quat="0.270598 -0.270598 0.653281 0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <!-- Link v5_wrist_j6_j7_2 -->
+                <body name="v5_wrist_j6_j7_2" pos="-0.1919 -0 0.212" quat="0.707107 0 -0.707107 -0">
+                  <!-- Joint from v6_wrist_j5_j6_2 to v5_wrist_j6_j7_2 -->
+                  <joint axis="0 0 1" name="LJ6" type="hinge"/>
+                  <inertial pos="-0 0.0975631 -0.161926" mass="0.407" fullinertia="0.00180585 0.00164708 0.000326757 0 -0 0.000247612"/>
+                  <!-- Part v6_xt30_2_2_50cm_5 -->
+                  <geom type="mesh" class="visual" pos="-0 0.116 -0.0925" quat="0.707107 -0.707107 0 -0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <geom type="mesh" class="collision" pos="-0 0.116 -0.0925" quat="0.707107 -0.707107 0 -0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <!-- Part v5_wrist_j6_j7_2 -->
+                  <geom type="mesh" class="visual" pos="-0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <geom type="mesh" class="collision" pos="-0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <!-- Part dm_j4310_2ec_7 -->
+                  <geom type="mesh" class="visual" pos="-0 0.121 -0.191" quat="0.5 0.5 0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <geom type="mesh" class="collision" pos="-0 0.121 -0.191" quat="0.5 0.5 0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <!-- Link dm_j4310_2ec -->
+                  <body name="dm_j4310_2ec" pos="-0 0.0751 -0.191" quat="0.5 0.5 0.5 -0.5">
+                    <!-- Joint from v5_wrist_j6_j7_2 to dm_j4310_2ec -->
+                    <joint axis="0 0 1" name="LJ7" type="hinge"/>
+                    <inertial pos="0.00205488 0.0647314 0.154012" mass="0.4573" fullinertia="0.00200291 0.00193779 0.000443531 3.45159e-05 -2.7424e-06 -0.000439089"/>
+                    <!-- Part dm_j4310_2ec_8 -->
+                    <geom type="mesh" class="visual" pos="0 0.075 0.21" quat="0 -1 0 0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <geom type="mesh" class="collision" pos="0 0.075 0.21" quat="0 -1 0 0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <!-- Part v5_wrist_j7_leader_gripper_l -->
+                    <geom type="mesh" class="visual" pos="-0.25 0 0" quat="1 -0 -0 -0" mesh="v5_wrist_j7_leader_gripper_l" material="v5_wrist_j7_leader_gripper_l_material"/>
+                    <geom type="mesh" class="collision" pos="-0.25 0 0" quat="1 -0 -0 -0" mesh="v5_wrist_j7_leader_gripper_l" material="v5_wrist_j7_leader_gripper_l_material"/>
+                    <!-- Part v6_xt30_2_2_50cm_6 -->
+                    <geom type="mesh" class="visual" pos="-0.03 0.054706 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <geom type="mesh" class="collision" pos="-0.03 0.054706 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <!-- Frame L_wrist -->
+                    <site group="3" name="L_wrist" pos="0 0 0.0751" quat="0.707107 -0 -0 0.707107"/>
+                    <!-- Link v5_j8_4finger_2 -->
+                    <body name="v5_j8_4finger_2" pos="0 0.075 0.1641" quat="0 -0.707107 -0.707107 -0">
+                      <!-- Joint from dm_j4310_2ec to v5_j8_4finger_2 -->
+                      <joint axis="0 0 1" name="LJ8" type="hinge"/>
+                      <inertial pos="0.0582262 -2.729e-09 0.031412" mass="0.0384" fullinertia="3.92746e-05 6.14789e-05 2.96973e-05 -1e-12 -1.19931e-05 2e-12"/>
+                      <!-- Part v5_j8_4finger_2 -->
+                      <geom type="mesh" class="visual" pos="0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                      <geom type="mesh" class="collision" pos="0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <asset>
+    <mesh file="v6_j4_j5_l.stl"/>
+    <mesh file="v6_j3_2020_l.stl"/>
+    <mesh file="2020x20cm.stl"/>
+    <mesh file="4040x30cm.stl"/>
+    <mesh file="v5_wrist_j7_leader_gripper_l.stl"/>
+    <mesh file="v5_wrist_j6_j7.stl"/>
+    <mesh file="v6_j1_20.stl"/>
+    <mesh file="v6_j4_j5_r.stl"/>
+    <mesh file="dm_j4340_2ec.stl"/>
+    <mesh file="2020x10cm.stl"/>
+    <mesh file="v6_wrist_j5_j6.stl"/>
+    <mesh file="v5_wrist_j7_leader_gripper_r.stl"/>
+    <mesh file="v5_j8_4finger.stl"/>
+    <mesh file="v6_20_j2.stl"/>
+    <mesh file="v5_4040_j1.stl"/>
+    <mesh file="v6_xt30_2_2_50cm.stl"/>
+    <mesh file="v6_2020_j4_r.stl"/>
+    <mesh file="v6_2020_j4_l.stl"/>
+    <mesh file="v6_j3_2020_r.stl"/>
+    <mesh file="v6_20_j3.stl"/>
+    <mesh file="dm_j4310_2ec.stl"/>
+    <mesh file="v6_j2_20.stl"/>
+    <material name="dm_j4340_2ec_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="v5_4040_j1_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="4040x30cm_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="v6_20_j2_material" rgba="0.890196 0.698039 0.639216 1"/>
+    <material name="v6_j1_20_material" rgba="0.890196 0.698039 0.639216 1"/>
+    <material name="2020x20cm_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="2020x10cm_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="v6_j2_20_material" rgba="0.890196 0.698039 0.639216 1"/>
+    <material name="v6_20_j3_material" rgba="0.890196 0.698039 0.639216 1"/>
+    <material name="v6_2020_j4_r_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v6_j3_2020_r_material" rgba="0.890196 0.698039 0.639216 1"/>
+    <material name="dm_j4310_2ec_material" rgba="0.941176 0.87451 0.678431 1"/>
+    <material name="v6_j4_j5_r_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v6_wrist_j5_j6_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v6_xt30_2_2_50cm_material" rgba="1 0 0 1"/>
+    <material name="v5_wrist_j6_j7_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v5_wrist_j7_leader_gripper_r_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v5_j8_4finger_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v6_j3_2020_l_material" rgba="0.890196 0.698039 0.639216 1"/>
+    <material name="v6_2020_j4_l_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v6_j4_j5_l_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v5_wrist_j7_leader_gripper_l_material" rgba="0.615686 0.811765 0.929412 1"/>
+  </asset>
+  <actuator>
+    <position class="models" name="RJ1" joint="RJ1"/>
+    <position class="models" name="RJ2" joint="RJ2"/>
+    <position class="models" name="RJ3" joint="RJ3"/>
+    <position class="models" name="RJ4" joint="RJ4"/>
+    <position class="models" name="RJ5" joint="RJ5"/>
+    <position class="models" name="RJ6" joint="RJ6"/>
+    <position class="models" name="RJ7" joint="RJ7"/>
+    <position class="models" name="RJ8" joint="RJ8"/>
+    <position class="models" name="LJ1" joint="LJ1"/>
+    <position class="models" name="LJ2" joint="LJ2"/>
+    <position class="models" name="LJ3" joint="LJ3"/>
+    <position class="models" name="LJ4" joint="LJ4"/>
+    <position class="models" name="LJ5" joint="LJ5"/>
+    <position class="models" name="LJ6" joint="LJ6"/>
+    <position class="models" name="LJ7" joint="LJ7"/>
+    <position class="models" name="LJ8" joint="LJ8"/>
+  </actuator>
+  <equality/>
+</mujoco>

model/ume/v6_imu/robot.xml

@@ -0,0 +1,389 @@
+<?xml version="1.0" ?>
+<mujoco model="models">
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <default>
+    <default class="models">
+      <joint frictionloss="0.1" armature="0.005"/>
+      <position kp="50" dampratio="1"/>
+      <default class="visual">
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
+      </default>
+      <default class="collision">
+        <geom group="3"/>
+      </default>
+    </default>
+  </default>
+  <worldbody>
+    <!-- Link dm_j4340_2ec -->
+    <body name="dm_j4340_2ec" pos="0 0 0" quat="1 0 0 0" childclass="models">
+      <freejoint name="dm_j4340_2ec_freejoint"/>
+      <inertial pos="0.00114692 -0.0251961 -0.012025" mass="0.8837" fullinertia="0.0325559 0.00235183 0.0334678 0.00115048 -0.000622082 0.000713939"/>
+      <!-- Part 4040x50cm -->
+      <geom type="mesh" class="visual" pos="0 0 -0.54" quat="1 0 -0 -0" mesh="4040x50cm" material="4040x50cm_material"/>
+      <geom type="mesh" class="collision" pos="0 0 -0.54" quat="1 0 -0 -0" mesh="4040x50cm" material="4040x50cm_material"/>
+      <!-- Part v6_4040_belt -->
+      <geom type="mesh" class="visual" pos="0.0004 0 -0.33" quat="0 0.707107 0.707107 0" mesh="v6_4040_belt" material="v6_4040_belt_material"/>
+      <geom type="mesh" class="collision" pos="0.0004 0 -0.33" quat="0 0.707107 0.707107 0" mesh="v6_4040_belt" material="v6_4040_belt_material"/>
+      <!-- Part 4040_ybimu -->
+      <geom type="mesh" class="visual" pos="0.0002 -0 -0.23" quat="0.5 -0.5 -0.5 0.5" mesh="4040_ybimu" material="4040_ybimu_material"/>
+      <geom type="mesh" class="collision" pos="0.0002 -0 -0.23" quat="0.5 -0.5 -0.5 0.5" mesh="4040_ybimu" material="4040_ybimu_material"/>
+      <!-- Part v6_4040_j1_loop_r -->
+      <geom type="mesh" class="visual" pos="0 -0.0615 -0.02" quat="1 0 0 0" mesh="v6_4040_j1_loop_r" material="v6_4040_j1_loop_r_material"/>
+      <geom type="mesh" class="collision" pos="0 -0.0615 -0.02" quat="1 0 0 0" mesh="v6_4040_j1_loop_r" material="v6_4040_j1_loop_r_material"/>
+      <!-- Part dm_j4340_2ec -->
+      <geom type="mesh" class="visual" pos="0.015 0.2 -0.02" quat="0 -0.707107 0 0.707107" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <geom type="mesh" class="collision" pos="0.015 0.2 -0.02" quat="0 -0.707107 0 0.707107" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <!-- Part 4040_t_bracket -->
+      <geom type="mesh" class="visual" pos="0.02 -0 -0.02" quat="0.5 0.5 0.5 0.5" mesh="4040_t_bracket" material="4040_t_bracket_material"/>
+      <geom type="mesh" class="collision" pos="0.02 -0 -0.02" quat="0.5 0.5 0.5 0.5" mesh="4040_t_bracket" material="4040_t_bracket_material"/>
+      <!-- Part dm_j4340_2ec_2 -->
+      <geom type="mesh" class="visual" pos="0.015 -0.2 -0.02" quat="0.707107 0 -0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <geom type="mesh" class="collision" pos="0.015 -0.2 -0.02" quat="0.707107 0 -0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+      <!-- Part 4040_t_bracket_2 -->
+      <geom type="mesh" class="visual" pos="-0.02 -0 -0.02" quat="0.5 0.5 -0.5 -0.5" mesh="4040_t_bracket" material="4040_t_bracket_material"/>
+      <geom type="mesh" class="collision" pos="-0.02 -0 -0.02" quat="0.5 0.5 -0.5 -0.5" mesh="4040_t_bracket" material="4040_t_bracket_material"/>
+      <!-- Part v6_4040_j1_loop_l -->
+      <geom type="mesh" class="visual" pos="0 0.0615 -0.02" quat="1 0 0 0" mesh="v6_4040_j1_loop_l" material="v6_4040_j1_loop_l_material"/>
+      <geom type="mesh" class="collision" pos="0 0.0615 -0.02" quat="1 0 0 0" mesh="v6_4040_j1_loop_l" material="v6_4040_j1_loop_l_material"/>
+      <!-- Part 4040x30cm -->
+      <geom type="mesh" class="visual" pos="0 0.15 -0.02" quat="0.707107 0.707107 0 -0" mesh="4040x30cm" material="4040x30cm_material"/>
+      <geom type="mesh" class="collision" pos="0 0.15 -0.02" quat="0.707107 0.707107 0 -0" mesh="4040x30cm" material="4040x30cm_material"/>
+      <!-- Part 4040_cap -->
+      <geom type="mesh" class="visual" pos="0 0 -0.55" quat="1 0 -0 -0" mesh="4040_cap" material="4040_cap_material"/>
+      <geom type="mesh" class="collision" pos="0 0 -0.55" quat="1 0 -0 -0" mesh="4040_cap" material="4040_cap_material"/>
+      <!-- Frame imu -->
+      <site group="3" name="imu" pos="-0.0298 -0 -0.229564" quat="0.707107 0 -0.707107 0"/>
+      <!-- Link v6_j1_20 -->
+      <body name="v6_j1_20" pos="-0.0383 -0.2 -0.02" quat="0.183013 0.683013 -0.183013 -0.683013">
+        <!-- Joint from dm_j4340_2ec to v6_j1_20 -->
+        <joint axis="0 0 1" name="RJ1" type="hinge"/>
+        <inertial pos="0.150386 -5.99e-10 -0.0862657" mass="0.64749" fullinertia="0.0032465 0.00514141 0.00215547 6e-12 0.00180143 1.8e-11"/>
+        <!-- Part v6_20_j2 -->
+        <geom type="mesh" class="visual" pos="-0.0081 -0 0.036052" quat="0 -1 -0 0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <geom type="mesh" class="collision" pos="-0.0081 -0 0.036052" quat="0 -1 -0 0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <!-- Part v6_j1_20 -->
+        <geom type="mesh" class="visual" pos="-0 0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <geom type="mesh" class="collision" pos="-0 0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <!-- Part 2020x20cm -->
+        <geom type="mesh" class="visual" pos="0.2253 -0 0.018" quat="0 0.707107 0 -0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <geom type="mesh" class="collision" pos="0.2253 -0 0.018" quat="0 0.707107 0 -0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <!-- Part dm_j4340_2ec_3 -->
+        <geom type="mesh" class="visual" pos="0.1532 0 -0.139098" quat="0.707107 -0 0.707107 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <geom type="mesh" class="collision" pos="0.1532 0 -0.139098" quat="0.707107 -0 0.707107 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <!-- Part 2020x10cm -->
+        <geom type="mesh" class="visual" pos="0.1653 -0 -0.092" quat="0.707107 0 0 0.707107" mesh="2020x10cm" material="2020x10cm_material"/>
+        <geom type="mesh" class="collision" pos="0.1653 -0 -0.092" quat="0.707107 0 0 0.707107" mesh="2020x10cm" material="2020x10cm_material"/>
+        <!-- Link v6_20_j3 -->
+        <body name="v6_20_j3" pos="0.2065 0 -0.139098" quat="0.5 0.5 0.5 0.5">
+          <!-- Joint from v6_j1_20 to v6_20_j3 -->
+          <joint axis="0 0 1" name="RJ2" type="hinge"/>
+          <inertial pos="0.0816108 2.273e-09 -0.108365" mass="0.6629" fullinertia="0.00347714 0.00399968 0.000810527 1.64e-10 0.00105864 -1.66e-10"/>
+          <!-- Part v6_j2_20 -->
+          <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <!-- Part 2020x20cm_2 -->
+          <geom type="mesh" class="visual" pos="0.127501 -0 -0.135474" quat="0.683013 0.183013 -0.183013 -0.683013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <geom type="mesh" class="collision" pos="0.127501 -0 -0.135474" quat="0.683013 0.183013 -0.183013 -0.683013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <!-- Part v6_20_j3 -->
+          <geom type="mesh" class="visual" pos="-0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <geom type="mesh" class="collision" pos="-0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <!-- Part dm_j4340_2ec_4 -->
+          <geom type="mesh" class="visual" pos="0.114169 0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <geom type="mesh" class="collision" pos="0.114169 0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <!-- Link v6_j3_2020_r -->
+          <body name="v6_j3_2020_r" pos="0.0680103 0 -0.175034" quat="0.353553 0.612372 -0.612372 -0.353553">
+            <!-- Joint from v6_20_j3 to v6_j3_2020_r -->
+            <joint axis="0 0 1" name="RJ3" type="hinge"/>
+            <inertial pos="-0.0212538 0.197218 0.0388781" mass="0.86248" fullinertia="0.00735386 0.00260883 0.00681043 -0.00104227 0.000890554 0.000819778"/>
+            <!-- Part 2020x20cm_3 -->
+            <geom type="mesh" class="visual" pos="-0.0681202 0.2685 0.078682" quat="0.707107 0.707107 0 -0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <geom type="mesh" class="collision" pos="-0.0681202 0.2685 0.078682" quat="0.707107 0.707107 0 -0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <!-- Part v6_2020_j4_r -->
+            <geom type="mesh" class="visual" pos="-0.0681202 0.2885 0.078682" quat="0.5 0.5 -0.5 0.5" mesh="v6_2020_j4_r" material="v6_2020_j4_r_material"/>
+            <geom type="mesh" class="collision" pos="-0.0681202 0.2885 0.078682" quat="0.5 0.5 -0.5 0.5" mesh="v6_2020_j4_r" material="v6_2020_j4_r_material"/>
+            <!-- Part dm_j4340_2ec_5 -->
+            <geom type="mesh" class="visual" pos="0.00787981 0.26 0.027332" quat="0 -1 0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <geom type="mesh" class="collision" pos="0.00787981 0.26 0.027332" quat="0 -1 0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <!-- Part v6_j3_2020_r -->
+            <geom type="mesh" class="visual" pos="-0.0286202 0.0885 0.028682" quat="0.5 0.5 0.5 -0.5" mesh="v6_j3_2020_r" material="v6_j3_2020_r_material"/>
+            <geom type="mesh" class="collision" pos="-0.0286202 0.0885 0.028682" quat="0.5 0.5 0.5 -0.5" mesh="v6_j3_2020_r" material="v6_j3_2020_r_material"/>
+            <!-- Frame R_shoulder -->
+            <site group="3" name="R_shoulder" pos="-0.000120192 -0 0.078682" quat="0.5 0.5 0.5 0.5"/>
+            <!-- Link v6_j4_j5_r -->
+            <body name="v6_j4_j5_r" pos="0.00787981 0.26 -0.025968" quat="0 0.707107 -0.707107 -0">
+              <!-- Joint from v6_j3_2020_r to v6_j4_j5_r -->
+              <joint axis="0 0 1" name="RJ4" type="hinge"/>
+              <inertial pos="-0.054697 0.00944171 -0.00245461" mass="0.49139" fullinertia="0.00105657 0.00119188 0.000538644 -2.97344e-05 -0.000120659 0.000177465"/>
+              <!-- Part dm_j4310_2ec -->
+              <geom type="mesh" class="visual" pos="-0.0431803 0 0.0244282" quat="0.348175 0.615447 -0.615447 -0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <geom type="mesh" class="collision" pos="-0.0431803 0 0.0244282" quat="0.348175 0.615447 -0.615447 -0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <!-- Part v6_j4_j5_r -->
+              <geom type="mesh" class="visual" pos="-0.0285 0.0285 0" quat="0.5 0.5 0.5 -0.5" mesh="v6_j4_j5_r" material="v6_j4_j5_r_material"/>
+              <geom type="mesh" class="collision" pos="-0.0285 0.0285 0" quat="0.5 0.5 0.5 -0.5" mesh="v6_j4_j5_r" material="v6_j4_j5_r_material"/>
+              <!-- Link v6_wrist_j5_j6 -->
+              <body name="v6_wrist_j5_j6" pos="-0.0825226 -0 0.000785249" quat="0 -0.870373 0 0.492393">
+                <!-- Joint from v6_j4_j5_r to v6_wrist_j5_j6 -->
+                <joint axis="0 0 1" name="RJ5" type="hinge"/>
+                <inertial pos="-0.145145 7.46224e-05 0.168843" mass="0.4484" fullinertia="0.00261478 0.00340573 0.00098092 -2.29433e-06 0.00132169 3.71339e-06"/>
+                <!-- Part v6_wrist_j5_j6 -->
+                <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 -0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 -0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <!-- Part v6_xt30_2_2_50cm -->
+                <geom type="mesh" class="visual" pos="-0.0816102 0.005 0.0643898" quat="0.270598 0.270598 0.653281 -0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <geom type="mesh" class="collision" pos="-0.0816102 0.005 0.0643898" quat="0.270598 0.270598 0.653281 -0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <!-- Part dm_j4310_2ec_2 -->
+                <geom type="mesh" class="visual" pos="-0.146 -0 0.212" quat="0.5 -0.5 -0.5 0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <geom type="mesh" class="collision" pos="-0.146 -0 0.212" quat="0.5 -0.5 -0.5 0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <!-- Link v5_wrist_j6_j7 -->
+                <body name="v5_wrist_j6_j7" pos="-0.1919 0 0.212" quat="0 -0.707107 -0 0.707107">
+                  <!-- Joint from v6_wrist_j5_j6 to v5_wrist_j6_j7 -->
+                  <joint axis="0 0 1" name="RJ6" type="hinge"/>
+                  <inertial pos="-0 0.0975631 -0.161926" mass="0.407" fullinertia="0.00180585 0.00164708 0.000326757 0 -0 0.000247612"/>
+                  <!-- Part v5_wrist_j6_j7 -->
+                  <geom type="mesh" class="visual" pos="0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <geom type="mesh" class="collision" pos="0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <!-- Part dm_j4310_2ec_3 -->
+                  <geom type="mesh" class="visual" pos="-0 0.121 -0.191" quat="0.707107 0.707107 0 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <geom type="mesh" class="collision" pos="-0 0.121 -0.191" quat="0.707107 0.707107 0 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <!-- Part v6_xt30_2_2_50cm_2 -->
+                  <geom type="mesh" class="visual" pos="0 0.121 -0.0925" quat="0.707107 0.707107 0 -0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <geom type="mesh" class="collision" pos="0 0.121 -0.0925" quat="0.707107 0.707107 0 -0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <!-- Link v5_wrist_j7_leader_gripper_r -->
+                  <body name="v5_wrist_j7_leader_gripper_r" pos="-0 0.0751 -0.191" quat="0.5 0.5 -0.5 0.5">
+                    <!-- Joint from v5_wrist_j6_j7 to v5_wrist_j7_leader_gripper_r -->
+                    <joint axis="0 0 1" name="RJ7" type="hinge"/>
+                    <inertial pos="-0.00205487 0.0647313 0.154012" mass="0.4573" fullinertia="0.00200291 0.00193779 0.000443531 -3.45159e-05 2.74273e-06 -0.000439089"/>
+                    <!-- Part v6_xt30_2_2_50cm_3 -->
+                    <geom type="mesh" class="visual" pos="0.025 0.0547059 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <geom type="mesh" class="collision" pos="0.025 0.0547059 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <!-- Part v5_wrist_j7_leader_gripper_r -->
+                    <geom type="mesh" class="visual" pos="-0 0 0" quat="1 0 -0 -0" mesh="v5_wrist_j7_leader_gripper_r" material="v5_wrist_j7_leader_gripper_r_material"/>
+                    <geom type="mesh" class="collision" pos="-0 0 0" quat="1 0 -0 -0" mesh="v5_wrist_j7_leader_gripper_r" material="v5_wrist_j7_leader_gripper_r_material"/>
+                    <!-- Part dm_j4310_2ec_4 -->
+                    <geom type="mesh" class="visual" pos="0 0.075 0.21" quat="0 -0.707107 0.707107 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <geom type="mesh" class="collision" pos="0 0.075 0.21" quat="0 -0.707107 0.707107 -0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <!-- Frame R_wrist -->
+                    <site group="3" name="R_wrist" pos="0 0 0.0751" quat="0.707107 -0 -0 0.707107"/>
+                    <!-- Link v5_j8_4finger -->
+                    <body name="v5_j8_4finger" pos="-0 0.075 0.1641" quat="0 -0.707107 -0.707107 -0">
+                      <!-- Joint from v5_wrist_j7_leader_gripper_r to v5_j8_4finger -->
+                      <joint axis="0 0 1" name="RJ8" type="hinge"/>
+                      <inertial pos="0.0582262 -2.729e-09 0.031412" mass="0.0384" fullinertia="3.92746e-05 6.14789e-05 2.96973e-05 -1e-12 -1.19931e-05 2e-12"/>
+                      <!-- Part v5_j8_4finger -->
+                      <geom type="mesh" class="visual" pos="-0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                      <geom type="mesh" class="collision" pos="-0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <!-- Link dm_j4340_2ec_2 -->
+      <body name="dm_j4340_2ec_2" pos="-0.0383 0.2 -0.02" quat="0.183013 -0.683013 -0.183013 0.683013">
+        <!-- Joint from dm_j4340_2ec to dm_j4340_2ec_2 -->
+        <joint axis="0 0 1" name="LJ1" type="hinge"/>
+        <inertial pos="0.150386 -5.99e-10 -0.0862657" mass="0.64749" fullinertia="0.0032465 0.00514141 0.00215547 6e-12 0.00180143 1.8e-11"/>
+        <!-- Part v6_j1_20_2 -->
+        <geom type="mesh" class="visual" pos="-0 -0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <geom type="mesh" class="collision" pos="-0 -0 0.036" quat="0 1 0 0" mesh="v6_j1_20" material="v6_j1_20_material"/>
+        <!-- Part 2020x10cm_2 -->
+        <geom type="mesh" class="visual" pos="0.1653 -0 -0.092" quat="0 0 -0 1" mesh="2020x10cm" material="2020x10cm_material"/>
+        <geom type="mesh" class="collision" pos="0.1653 -0 -0.092" quat="0 0 -0 1" mesh="2020x10cm" material="2020x10cm_material"/>
+        <!-- Part dm_j4340_2ec_6 -->
+        <geom type="mesh" class="visual" pos="0.1532 -0 -0.139098" quat="0.707107 -0 0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <geom type="mesh" class="collision" pos="0.1532 -0 -0.139098" quat="0.707107 -0 0.707107 0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+        <!-- Part 2020x20cm_4 -->
+        <geom type="mesh" class="visual" pos="0.2253 -0 0.018" quat="0 -0.707107 -0 0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <geom type="mesh" class="collision" pos="0.2253 -0 0.018" quat="0 -0.707107 -0 0.707107" mesh="2020x20cm" material="2020x20cm_material"/>
+        <!-- Part v6_20_j2_2 -->
+        <geom type="mesh" class="visual" pos="-0.0081 -0 0.036052" quat="0 1 0 -0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <geom type="mesh" class="collision" pos="-0.0081 -0 0.036052" quat="0 1 0 -0" mesh="v6_20_j2" material="v6_20_j2_material"/>
+        <!-- Link dm_j4340_2ec_3 -->
+        <body name="dm_j4340_2ec_3" pos="0.2065 -0 -0.139098" quat="0.5 -0.5 0.5 -0.5">
+          <!-- Joint from dm_j4340_2ec_2 to dm_j4340_2ec_3 -->
+          <joint axis="0 0 1" name="LJ2" type="hinge"/>
+          <inertial pos="0.0816108 2.273e-09 -0.108365" mass="0.6629" fullinertia="0.00347714 0.00399968 0.000810527 1.64e-10 0.00105864 -1.66e-10"/>
+          <!-- Part v6_j2_20_2 -->
+          <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_j2_20" material="v6_j2_20_material"/>
+          <!-- Part v6_20_j3_2 -->
+          <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.683013 -0.683013 -0.183013 -0.183013" mesh="v6_20_j3" material="v6_20_j3_material"/>
+          <!-- Part 2020x20cm_5 -->
+          <geom type="mesh" class="visual" pos="0.0275006 -0 0.0377311" quat="0.183013 0.683013 0.683013 0.183013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <geom type="mesh" class="collision" pos="0.0275006 -0 0.0377311" quat="0.183013 0.683013 0.683013 0.183013" mesh="2020x20cm" material="2020x20cm_material"/>
+          <!-- Part dm_j4340_2ec_7 -->
+          <geom type="mesh" class="visual" pos="0.114169 0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <geom type="mesh" class="collision" pos="0.114169 0 -0.148384" quat="0.353553 0.612372 -0.612372 -0.353553" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+          <!-- Link v6_j3_2020_l -->
+          <body name="v6_j3_2020_l" pos="0.0680103 -0 -0.175034" quat="0.353553 -0.612372 -0.612372 0.353553">
+            <!-- Joint from dm_j4340_2ec_3 to v6_j3_2020_l -->
+            <joint axis="0 0 1" name="LJ3" type="hinge"/>
+            <inertial pos="-0.0211335 -0.197218 0.0388781" mass="0.86248" fullinertia="0.00735383 0.00260882 0.0068104 0.00104226 0.00089055 -0.00081978"/>
+            <!-- Part v6_j3_2020_l -->
+            <geom type="mesh" class="visual" pos="-0.0285 -0.0885 0.248682" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j3_2020_l" material="v6_j3_2020_l_material"/>
+            <geom type="mesh" class="collision" pos="-0.0285 -0.0885 0.248682" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j3_2020_l" material="v6_j3_2020_l_material"/>
+            <!-- Part dm_j4340_2ec_8 -->
+            <geom type="mesh" class="visual" pos="0.008 -0.26 0.027332" quat="0 -1 -0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <geom type="mesh" class="collision" pos="0.008 -0.26 0.027332" quat="0 -1 -0 -0" mesh="dm_j4340_2ec" material="dm_j4340_2ec_material"/>
+            <!-- Part 2020x20cm_6 -->
+            <geom type="mesh" class="visual" pos="-0.068 -0.2685 0.078682" quat="0.707107 -0.707107 0 0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <geom type="mesh" class="collision" pos="-0.068 -0.2685 0.078682" quat="0.707107 -0.707107 0 0" mesh="2020x20cm" material="2020x20cm_material"/>
+            <!-- Part v6_2020_j4_l -->
+            <geom type="mesh" class="visual" pos="-0.068 -0.2885 0.214682" quat="0.5 -0.5 0.5 0.5" mesh="v6_2020_j4_l" material="v6_2020_j4_l_material"/>
+            <geom type="mesh" class="collision" pos="-0.068 -0.2885 0.214682" quat="0.5 -0.5 0.5 0.5" mesh="v6_2020_j4_l" material="v6_2020_j4_l_material"/>
+            <!-- Frame L_shoulder -->
+            <site group="3" name="L_shoulder" pos="0 -0 0.078682" quat="0.5 -0.5 0.5 -0.5"/>
+            <!-- Link v6_j4_j5_l -->
+            <body name="v6_j4_j5_l" pos="0.008 -0.26 -0.025968" quat="0 0.707107 0.707107 0">
+              <!-- Joint from v6_j3_2020_l to v6_j4_j5_l -->
+              <joint axis="0 0 1" name="LJ4" type="hinge"/>
+              <inertial pos="-0.054697 -0.0094417 -0.00245459" mass="0.49139" fullinertia="0.00105657 0.00119187 0.000538643 2.97343e-05 -0.000120659 -0.000177465"/>
+              <!-- Part v6_j4_j5_l -->
+              <geom type="mesh" class="visual" pos="-0.0285 -0.0285 -0.3253" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j4_j5_l" material="v6_j4_j5_l_material"/>
+              <geom type="mesh" class="collision" pos="-0.0285 -0.0285 -0.3253" quat="0.5 -0.5 -0.5 -0.5" mesh="v6_j4_j5_l" material="v6_j4_j5_l_material"/>
+              <!-- Part dm_j4310_2ec_5 -->
+              <geom type="mesh" class="visual" pos="-0.0431803 -0 0.0244282" quat="0.348175 -0.615447 -0.615447 0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <geom type="mesh" class="collision" pos="-0.0431803 -0 0.0244282" quat="0.348175 -0.615447 -0.615447 0.348175" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+              <!-- Link v6_wrist_j5_j6_2 -->
+              <body name="v6_wrist_j5_j6_2" pos="-0.0825226 -0 0.000785249" quat="0 -0.870373 0 0.492393">
+                <!-- Joint from v6_j4_j5_l to v6_wrist_j5_j6_2 -->
+                <joint axis="0 0 1" name="LJ5" type="hinge"/>
+                <inertial pos="-0.145035 -0.000119401 0.168953" mass="0.4484" fullinertia="0.00260463 0.00340197 0.000987304 3.35036e-06 0.00132357 -5.25689e-06"/>
+                <!-- Part v6_wrist_j5_j6_2 -->
+                <geom type="mesh" class="visual" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <geom type="mesh" class="collision" pos="0 -0.0285 0.0075" quat="0.707107 -0.707107 -0 0" mesh="v6_wrist_j5_j6" material="v6_wrist_j5_j6_material"/>
+                <!-- Part dm_j4310_2ec_6 -->
+                <geom type="mesh" class="visual" pos="-0.146 0 0.212" quat="0.5 0.5 -0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <geom type="mesh" class="collision" pos="-0.146 0 0.212" quat="0.5 0.5 -0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                <!-- Part v6_xt30_2_2_50cm_4 -->
+                <geom type="mesh" class="visual" pos="-0.0787817 -0.005 0.0672183" quat="0.270598 -0.270598 0.653281 0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <geom type="mesh" class="collision" pos="-0.0787817 -0.005 0.0672183" quat="0.270598 -0.270598 0.653281 0.653281" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                <!-- Link v5_wrist_j6_j7_2 -->
+                <body name="v5_wrist_j6_j7_2" pos="-0.1919 -0 0.212" quat="0.707107 0 -0.707107 -0">
+                  <!-- Joint from v6_wrist_j5_j6_2 to v5_wrist_j6_j7_2 -->
+                  <joint axis="0 0 1" name="LJ6" type="hinge"/>
+                  <inertial pos="-0 0.0975631 -0.161926" mass="0.407" fullinertia="0.00180585 0.00164708 0.000326757 0 -0 0.000247612"/>
+                  <!-- Part v6_xt30_2_2_50cm_5 -->
+                  <geom type="mesh" class="visual" pos="0 0.116 -0.0925" quat="0.707107 -0.707107 0 0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <geom type="mesh" class="collision" pos="0 0.116 -0.0925" quat="0.707107 -0.707107 0 0" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                  <!-- Part v5_wrist_j6_j7_2 -->
+                  <geom type="mesh" class="visual" pos="0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <geom type="mesh" class="collision" pos="0 0.131 -0.0955" quat="0.5 0.5 0.5 -0.5" mesh="v5_wrist_j6_j7" material="v5_wrist_j6_j7_material"/>
+                  <!-- Part dm_j4310_2ec_7 -->
+                  <geom type="mesh" class="visual" pos="-0 0.121 -0.191" quat="0.5 0.5 0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <geom type="mesh" class="collision" pos="-0 0.121 -0.191" quat="0.5 0.5 0.5 -0.5" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                  <!-- Link dm_j4310_2ec -->
+                  <body name="dm_j4310_2ec" pos="-0 0.0751 -0.191" quat="0.5 0.5 0.5 -0.5">
+                    <!-- Joint from v5_wrist_j6_j7_2 to dm_j4310_2ec -->
+                    <joint axis="0 0 1" name="LJ7" type="hinge"/>
+                    <inertial pos="0.00205488 0.0647314 0.154012" mass="0.4573" fullinertia="0.00200291 0.00193779 0.000443531 3.45159e-05 -2.7424e-06 -0.000439089"/>
+                    <!-- Part dm_j4310_2ec_8 -->
+                    <geom type="mesh" class="visual" pos="0 0.075 0.21" quat="0 -1 0 0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <geom type="mesh" class="collision" pos="0 0.075 0.21" quat="0 -1 0 0" mesh="dm_j4310_2ec" material="dm_j4310_2ec_material"/>
+                    <!-- Part v5_wrist_j7_leader_gripper_l -->
+                    <geom type="mesh" class="visual" pos="-0.25 0 0" quat="1 0 0 0" mesh="v5_wrist_j7_leader_gripper_l" material="v5_wrist_j7_leader_gripper_l_material"/>
+                    <geom type="mesh" class="collision" pos="-0.25 0 0" quat="1 0 0 0" mesh="v5_wrist_j7_leader_gripper_l" material="v5_wrist_j7_leader_gripper_l_material"/>
+                    <!-- Part v6_xt30_2_2_50cm_6 -->
+                    <geom type="mesh" class="visual" pos="-0.03 0.054706 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <geom type="mesh" class="collision" pos="-0.03 0.054706 0.0643213" quat="0.5 -0.5 0.5 -0.5" mesh="v6_xt30_2_2_50cm" material="v6_xt30_2_2_50cm_material"/>
+                    <!-- Frame L_wrist -->
+                    <site group="3" name="L_wrist" pos="0 -0 0.0751" quat="0.707107 -0 0 0.707107"/>
+                    <!-- Link v5_j8_4finger_2 -->
+                    <body name="v5_j8_4finger_2" pos="0 0.075 0.1641" quat="0 -0.707107 -0.707107 -0">
+                      <!-- Joint from dm_j4310_2ec to v5_j8_4finger_2 -->
+                      <joint axis="0 0 1" name="LJ8" type="hinge"/>
+                      <inertial pos="0.0582262 -2.729e-09 0.031412" mass="0.0384" fullinertia="3.92746e-05 6.14789e-05 2.96973e-05 -1e-12 -1.19931e-05 2e-12"/>
+                      <!-- Part v5_j8_4finger_2 -->
+                      <geom type="mesh" class="visual" pos="0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                      <geom type="mesh" class="collision" pos="0 0 0.01" quat="0 1 0 0" mesh="v5_j8_4finger" material="v5_j8_4finger_material"/>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <asset>
+    <mesh file="v6_j1_20.stl"/>
+    <mesh file="v6_20_j3.stl"/>
+    <mesh file="2020x10cm.stl"/>
+    <mesh file="v6_xt30_2_2_50cm.stl"/>
+    <mesh file="dm_j4310_2ec.stl"/>
+    <mesh file="4040_ybimu.stl"/>
+    <mesh file="v6_j3_2020_r.stl"/>
+    <mesh file="v6_wrist_j5_j6.stl"/>
+    <mesh file="v5_wrist_j7_leader_gripper_l.stl"/>
+    <mesh file="v6_20_j2.stl"/>
+    <mesh file="v6_j3_2020_l.stl"/>
+    <mesh file="v6_j4_j5_l.stl"/>
+    <mesh file="v6_j4_j5_r.stl"/>
+    <mesh file="v5_wrist_j7_leader_gripper_r.stl"/>
+    <mesh file="2020x20cm.stl"/>
+    <mesh file="4040x50cm.stl"/>
+    <mesh file="v6_2020_j4_r.stl"/>
+    <mesh file="v6_4040_j1_loop_r.stl"/>
+    <mesh file="v5_j8_4finger.stl"/>
+    <mesh file="v6_4040_j1_loop_l.stl"/>
+    <mesh file="4040_t_bracket.stl"/>
+    <mesh file="dm_j4340_2ec.stl"/>
+    <mesh file="v6_2020_j4_l.stl"/>
+    <mesh file="v6_4040_belt.stl"/>
+    <mesh file="v6_j2_20.stl"/>
+    <mesh file="4040x30cm.stl"/>
+    <mesh file="4040_cap.stl"/>
+    <mesh file="v5_wrist_j6_j7.stl"/>
+    <material name="4040x50cm_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="v6_4040_belt_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="4040_ybimu_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v6_4040_j1_loop_r_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="dm_j4340_2ec_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="4040_t_bracket_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="v6_4040_j1_loop_l_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="4040x30cm_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="4040_cap_material" rgba="0.615686 0.811765 0.929412 1"/>
+    <material name="v6_20_j2_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_j1_20_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="2020x20cm_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="2020x10cm_material" rgba="0.901961 0.901961 0.901961 1"/>
+    <material name="v6_j2_20_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_20_j3_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_2020_j4_r_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_j3_2020_r_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="dm_j4310_2ec_material" rgba="0.796078 0.905882 0.745098 1"/>
+    <material name="v6_j4_j5_r_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_wrist_j5_j6_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_xt30_2_2_50cm_material" rgba="1 0 0 1"/>
+    <material name="v5_wrist_j6_j7_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v5_wrist_j7_leader_gripper_r_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v5_j8_4finger_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_j3_2020_l_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_2020_j4_l_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v6_j4_j5_l_material" rgba="0.972549 0.529412 0.00392157 1"/>
+    <material name="v5_wrist_j7_leader_gripper_l_material" rgba="0.972549 0.529412 0.00392157 1"/>
+  </asset>
+  <actuator>
+    <position class="models" name="RJ1" joint="RJ1"/>
+    <position class="models" name="RJ2" joint="RJ2"/>
+    <position class="models" name="RJ3" joint="RJ3"/>
+    <position class="models" name="RJ4" joint="RJ4"/>
+    <position class="models" name="RJ5" joint="RJ5"/>
+    <position class="models" name="RJ6" joint="RJ6"/>
+    <position class="models" name="RJ7" joint="RJ7"/>
+    <position class="models" name="RJ8" joint="RJ8"/>
+    <position class="models" name="LJ1" joint="LJ1"/>
+    <position class="models" name="LJ2" joint="LJ2"/>
+    <position class="models" name="LJ3" joint="LJ3"/>
+    <position class="models" name="LJ4" joint="LJ4"/>
+    <position class="models" name="LJ5" joint="LJ5"/>
+    <position class="models" name="LJ6" joint="LJ6"/>
+    <position class="models" name="LJ7" joint="LJ7"/>
+    <position class="models" name="LJ8" joint="LJ8"/>
+  </actuator>
+  <equality/>
+</mujoco>

protos/cmvr/api/arm_teleop_v1.proto

@@ -0,0 +1,133 @@
+syntax = "proto3";
+
+package cmvr.api.armteleop.v1;
+
+// Versioned, session-oriented protocol for wired arm teleoperation. Existing
+// unary ArmService RPCs intentionally remain unchanged.
+service ArmTeleopService {
+  rpc Teleoperate(stream ClientFrame) returns (stream ServerFrame);
+}
+
+enum SessionPhase {
+  SESSION_PHASE_UNSPECIFIED = 0;
+  SESSION_PHASE_OPENED = 1;
+  SESSION_PHASE_READY = 2;
+  SESSION_PHASE_ACTIVE = 3;
+  SESSION_PHASE_HOLDING = 4;
+  SESSION_PHASE_STOPPED = 5;
+  SESSION_PHASE_WATCHDOG_EXPIRED = 6;
+  SESSION_PHASE_LEASE_LOST = 7;
+  SESSION_PHASE_REJECTED = 8;
+  SESSION_PHASE_FAILED = 9;
+}
+
+enum StopReason {
+  STOP_REASON_UNSPECIFIED = 0;
+  STOP_REASON_OPERATOR_REQUEST = 1;
+  STOP_REASON_CLIENT_SHUTDOWN = 2;
+  STOP_REASON_WATCHDOG = 3;
+  STOP_REASON_LEASE_REVOKED = 4;
+  STOP_REASON_ROBOT_FAULT = 5;
+  STOP_REASON_EMERGENCY_STOP = 6;
+  STOP_REASON_PROTOCOL_ERROR = 7;
+}
+
+enum EffortSource {
+  EFFORT_SOURCE_UNSPECIFIED = 0;
+  EFFORT_SOURCE_MOTOR_ESTIMATE = 1;
+  EFFORT_SOURCE_JOINT_SENSOR = 2;
+  EFFORT_SOURCE_FORCE_TORQUE_SENSOR = 3;
+  EFFORT_SOURCE_OBSERVER = 4;
+}
+
+message RobotManifest {
+  string robot_id = 1;
+  string model_sha256 = 2;
+  string calibration_sha256 = 3;
+  repeated string joint_names = 4;
+  string position_unit = 5;
+  string velocity_unit = 6;
+  string effort_unit = 7;
+  string base_frame = 8;
+  string tool_frame = 9;
+}
+
+message OpenSession {
+  uint32 protocol_major = 1;
+  uint32 protocol_minor = 2;
+  string client_instance_id = 3;
+  RobotManifest expected_robot = 4;
+  uint32 requested_command_rate_hz = 5;
+  uint32 requested_state_rate_hz = 6;
+  uint32 watchdog_timeout_ms = 7;
+  uint32 requested_lease_ms = 8;
+  bool request_force_feedback = 9;
+}
+
+message JointSetpoint {
+  // Strictly increasing and non-zero within a session.
+  uint64 sequence = 1;
+  repeated double position_rad = 2;
+  repeated double velocity_rad_s = 3;
+  // The receiver computes its deadline from local arrival time plus this
+  // duration. Zero is invalid for an active setpoint.
+  uint32 valid_for_us = 4;
+}
+
+message ClientHeartbeat {
+  uint64 sequence = 1;
+}
+
+message StopSession {
+  StopReason reason = 1;
+  string detail = 2;
+}
+
+message ClientFrame {
+  oneof payload {
+    OpenSession open = 1;
+    JointSetpoint setpoint = 2;
+    ClientHeartbeat heartbeat = 3;
+    StopSession stop = 4;
+  }
+}
+
+message JointState {
+  uint64 sample_sequence = 1;
+  repeated double position_rad = 2;
+  repeated double velocity_rad_s = 3;
+  repeated double effort_nm = 4;
+  bool position_valid = 5;
+  bool velocity_valid = 6;
+  bool effort_valid = 7;
+  EffortSource effort_source = 8;
+  uint64 sample_age_us = 9;
+}
+
+message SessionStatus {
+  string session_id = 1;
+  SessionPhase phase = 2;
+  uint64 received_sequence = 3;
+  uint64 applied_sequence = 4;
+  uint64 dropped_setpoints = 5;
+  uint64 rejected_setpoints = 6;
+  uint32 negotiated_watchdog_ms = 7;
+  uint32 lease_remaining_ms = 8;
+  StopReason stop_reason = 9;
+  string detail = 10;
+}
+
+message RobotSafetyState {
+  bool connected = 1;
+  bool powered_on = 2;
+  bool protective_stopped = 3;
+  bool emergency_stopped = 4;
+  bool fault = 5;
+  string fault_detail = 6;
+}
+
+message ServerFrame {
+  SessionStatus status = 1;
+  JointState joint_state = 2;
+  RobotSafetyState safety = 3;
+}

protos/cmvr/config/arm_config/arm_config.proto

@@ -6,6 +6,7 @@ import "cmvr/config/pinocchio_dls_ik_config.proto";
 import "cmvr/config/pinocchio_qp_ik_config.proto";
 import "cmvr/config/srs_ik_config.proto";
 import "cmvr/config/cartesian_motion_validation_config.proto";
+import "cmvr/config/motor_config/motor_config.proto";
 
 enum ToppraPathType {
   TOPPRA_PATH_TYPE_UNKNOWN = 0;
@@ -24,6 +25,10 @@ message MotorRobotArmBackendConfig {
   double default_vel = 7;
   double default_acc = 8;
   repeated string motor_group_ids = 9;
+  // Reserved opt-in for a future atomic/timed group position-servo primitive.
+  // MotorRobotArm's current sequential per-joint servoJ implementation
+  // deliberately rejects this capability even if this field is true.
+  bool enable_teleop_group_servo = 10;
 }
 
 enum VendorRobotArmBrand {
@@ -45,6 +50,57 @@ message VendorRobotArmBackendConfig {
   string password = 10;
 }
 
+enum DamiaoMotorModel {
+  DAMIAO_MOTOR_MODEL_UNKNOWN = 0;
+  DAMIAO_MOTOR_MODEL_DM4310 = 1;
+  DAMIAO_MOTOR_MODEL_DM4310_48V = 2;
+  DAMIAO_MOTOR_MODEL_DM4340 = 3;
+  DAMIAO_MOTOR_MODEL_DM4340_48V = 4;
+  DAMIAO_MOTOR_MODEL_DM6006 = 5;
+  DAMIAO_MOTOR_MODEL_DM8006 = 6;
+  DAMIAO_MOTOR_MODEL_DM8009 = 7;
+  DAMIAO_MOTOR_MODEL_DM10010L = 8;
+  DAMIAO_MOTOR_MODEL_DM10010 = 9;
+  DAMIAO_MOTOR_MODEL_DMH3510 = 10;
+  DAMIAO_MOTOR_MODEL_DMH6215 = 11;
+  DAMIAO_MOTOR_MODEL_DMG6220 = 12;
+}
+
+message DamiaoJointConfig {
+  string joint_name = 1;
+  uint32 command_id = 2;
+  uint32 feedback_id = 3;
+  uint32 reported_motor_id = 4;
+  DamiaoMotorModel model = 5;
+  // Only +1 and -1 are accepted.
+  int32 direction = 6;
+  // q_joint = direction * q_motor + zero_offset_rad.
+  double zero_offset_rad = 7;
+  double joint_lower_rad = 8;
+  double joint_upper_rad = 9;
+  double max_velocity_rad_s = 10;
+  double max_torque_nm = 11;
+  // Explicit whitelist for the four-bit status nibble in MIT feedback.
+  // The code intentionally does not guess vendor/firmware meanings. At least
+  // one reviewed value is required before hardware_enabled may be true.
+  repeated uint32 healthy_feedback_status = 12;
+  // Raw byte thresholds, interpreted only as ordered protocol values. Nonzero
+  // reviewed limits are required before hardware_enabled may be true.
+  uint32 max_driver_temperature_raw = 13;
+  uint32 max_motor_temperature_raw = 14;
+}
+
+message UmeRobotArmBackendConfig {
+  SocketCanConfig can = 1;
+  repeated DamiaoJointConfig joints = 2;
+  uint32 control_frequency_hz = 3;
+  uint32 cycle_deadline_us = 4;
+  uint32 feedback_watchdog_ms = 5;
+  uint32 shutdown_timeout_ms = 6;
+  // This is deliberately false in every checked-in configuration.
+  bool hardware_enabled = 7;
+}
+
 message SpeedLPlannerConfig {
   double linear_velocity_max = 1;
   double linear_acceleration_max = 2;
@@ -133,6 +189,7 @@ message RobotArmConfig {
   oneof backend {
     MotorRobotArmBackendConfig motor = 10;
     VendorRobotArmBackendConfig vendor = 11;
+    UmeRobotArmBackendConfig ume = 12;
   }
 
   ArmKinematicsConfig kinematics = 20;

protos/cmvr/config/grpc_server_config/grpc_server_config.proto

@@ -1,6 +1,29 @@
 syntax = "proto3";
 package cmvr.config;
 
+message ArmTeleopBackendConfig {
+  // Two independent gates are required: this service-level switch and the
+  // RobotArm implementation's teleop group-servo capability.
+  bool enable = 1;
+  // Also becomes RobotManifest.robot_id and the process-wide control lease
+  // resource. It must exactly match RobotArm.id().
+  string device_id = 2;
+  string model_sha256 = 3;
+  string calibration_sha256 = 4;
+  string base_frame = 5;
+  string tool_frame = 6;
+  double servo_period_s = 7;
+  // Maximum wall time allowed for one RobotArm::servoJ call.
+  uint32 max_apply_duration_us = 8;
+  // Omission is interpreted as true by the backend. Explicit false is intended
+  // only for simulation and independently supervised commissioning.
+  optional bool require_powered = 9;
+  // Bounds the first target relative to the cached measured position.
+  double max_initial_position_step_rad = 10;
+  // Bounds every later target relative to the last accepted target.
+  double max_position_step_rad = 11;
+}
+
 message GRPCServerConfig {
   string host = 1;
   string port = 2;
@@ -12,6 +35,7 @@ message GRPCServerConfig {
   // Frames older than this monotonic age are not sent. Zero uses the service
   // default so configurations written before these fields remain low-latency.
   uint32 camera_stream_max_frame_age_ms = 6;
+  ArmTeleopBackendConfig arm_teleop_backend = 7;
 }
 message GRPCServerRootConfig {
   GRPCServerConfig grpc_server = 1;

protos/cmvr/config/motor_config/motor_config.proto

@@ -45,6 +45,21 @@ message EtherCATDcConfig {
 message SocketCanConfig {
   string dev_id = 1;
   int32 channel_id = 2;
+  // Explicit Linux interface name (for example can0 or vcan0). When empty,
+  // the legacy channel_id based naming remains in use.
+  optional string interface_name = 3;
+  // Allows CAN-FD frames on the raw socket. This does not configure the
+  // physical interface bitrate or bring the interface up.
+  optional bool enable_fd = 4;
+  // Default BRS flag for callers that explicitly construct an FD frame.
+  optional bool bitrate_switch = 5;
+  // Bounded receive wait. Zero selects the implementation safety default.
+  optional uint32 receive_timeout_us = 6;
+  optional bool receive_own_messages = 7;
+  optional bool enable_error_frames = 8;
+  // Total bounded wait for one send() batch. Zero selects the implementation
+  // safety default. UME profiles set this below their control-cycle deadline.
+  optional uint32 send_timeout_us = 9;
 }
 
 message EtherCATConfig {

protos/cmvr/config/task_manager_config/task_manager_config.proto

@@ -8,6 +8,7 @@ message TaskConfigEntry {
     TASK_TYPE_GRPC_SERVER = 3;
     TASK_TYPE_SELF_COLLISION = 4;
     TASK_TYPE_QUIC_EDGE = 5;
+    TASK_TYPE_UME_TELEOP = 6;
   }
 
   enum TaskRunMode {

protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto

@@ -0,0 +1,30 @@
+syntax = "proto3";
+
+package cmvr.config;
+
+import "cmvr/api/arm_teleop_v1.proto";
+
+message UmeTeleopReconnectConfig {
+  uint32 initial_delay_ms = 1;
+  uint32 maximum_delay_ms = 2;
+  double multiplier = 3;
+}
+
+// Transport/session configuration only. Robot kinematics, SEW, FK and IK stay
+// in UME and publish already-computed joint setpoints through the client API.
+message UmeTeleopConfig {
+  string id = 1;
+  string server_address = 2;
+
+  // M6 deliberately supports only an explicitly opted-in insecure channel.
+  // Keep the task disabled until the endpoint and deployment security policy
+  // are configured. TLS credentials can be added without changing the v1 API.
+  bool allow_insecure = 3;
+
+  cmvr.api.armteleop.v1.OpenSession open_session = 4;
+  UmeTeleopReconnectConfig reconnect = 5;
+}
+
+message UmeTeleopRootConfig {
+  UmeTeleopConfig ume_teleop = 1;
+}