fix(arm): reconcile protective recovery interface

# Conflicts:
#	CMakeLists.txt
#	cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt
#	cmvr-es/devices/arm/aubo_arm/aubo_arm.h
#	cmvr-es/devices/arm/huayan_arm/huayan_arm.h
#	cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
#	cmvr-es/main.cpp

CMakeLists.txt

@@ -9,7 +9,29 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 #set(CMAKE_CXX_STANDARD_REQUIRED True)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+# Preserve the project's production-build behavior: tests are opt-in via
+# -DBUILD_TESTING=ON, while still registering them with CTest when requested.
+option(BUILD_TESTING "Build the test targets" OFF)
+include(CTest)
 
+if(BUILD_TESTING AND UNIX AND NOT APPLE)
+    # Test executables can still inherit the AUBO imported target's build-tree
+    # RUNPATH. Keep the active toolchain runtime ahead of that vendor path.
+    execute_process(
+            COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6
+            OUTPUT_VARIABLE CMVR_TEST_SYSTEM_LIBSTDCXX
+            OUTPUT_STRIP_TRAILING_WHITESPACE
+    )
+    if(EXISTS "${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+        get_filename_component(
+                CMVR_TEST_SYSTEM_LIBSTDCXX
+                "${CMVR_TEST_SYSTEM_LIBSTDCXX}"
+                REALPATH
+        )
+    else()
+        unset(CMVR_TEST_SYSTEM_LIBSTDCXX)
+    endif()
+endif()
 
 # Install to <source>/output
 set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE)
@@ -48,6 +70,9 @@ install(
         DESTINATION lib/dri
 )
 
+if(BUILD_TESTING AND CMVR_EXTERNAL_LIBRARY_DIRS)
+    list(JOIN CMVR_EXTERNAL_LIBRARY_DIRS ":" CMVR_TEST_EXTERNAL_LIBRARY_PATH)
+endif()
 # 在调用 setup_external_libs 之后
 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}")
 message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}")
@@ -65,8 +90,9 @@ file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto)
 
 
 set(Protobuf_PROTOC_EXECUTABLE "${CMAKE_INSTALL_PREFIX}/bin/protoc" CACHE FILEPATH "" FORCE)
-set_property(TARGET gRPC::grpc_cpp_plugin
+set_target_properties(gRPC::grpc_cpp_plugin PROPERTIES
-        PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
+        IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
+        IMPORTED_LOCATION_RELEASE "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
 )
 
 # 1) 先做 OBJECT：只负责生成/编译 pb.cc

README.md

@@ -138,3 +138,14 @@ $IGH_ETHERCAT_ROOT/bin/ethercat pdos
 sudo script/ethercat/stop_ethercat.sh eno1
 sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
 ```
+
+## 组件文档
+
+具体能力、配置、协议和安全边界由对应代码目录下的 README 维护：
+
+- [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice)
+- [电机设备模块](cmvr-es/devices/motor/README.md)
+- [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md)
+- [配置与部署规则](cmvr-es/config/README.md)
+
+AUBO JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。

cmake/FindExternalLib.cmake

@@ -139,6 +139,7 @@ function(setup_external_libs ARCH)
         list(REMOVE_DUPLICATES LIBRARY_DIRS)
         link_directories(${LIBRARY_DIRS})
     endif()
+    set(CMVR_EXTERNAL_LIBRARY_DIRS "${LIBRARY_DIRS}" PARENT_SCOPE)
 
     # ---- install third-party shared libs into <prefix>/lib ----
     if(INSTALL_SO_FILES)

cmvr-es/CMakeLists.txt

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

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

@@ -20,12 +20,80 @@ const std::vector<std::string> kRightArmJoints{
     "R_WRIST_R",
 };
 
+const std::vector<std::string> kGen2RightArmJoints{
+    "right_arm_J1",
+    "right_arm_J2",
+    "right_arm_J3",
+    "right_arm_J4",
+    "right_arm_J5",
+    "right_arm_J6",
+    "right_arm_J7",
+};
+
+const std::vector<double> kGen2SetupPose{
+    0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0,
+};
+
+const std::vector<double> kGen2WarningPose{
+    2.45028525340088,
+    0.413065394330014,
+    -1.78610031118294,
+    2.3232081721811,
+    -2.96828882895788,
+    -1.59350098130002,
+    0.582912411114367,
+};
+
+const std::vector<double> kGen2StopPose{
+    2.13758633436379,
+    1.61835160165575,
+    -2.3836142221041,
+    0.964538527544213,
+    -0.00382525077004825,
+    1.74586899135531,
+    -0.336868659266887,
+};
+
+const std::vector<double> kGen2CollisionPose{
+    -0.42656969579233,
+    1.41426471041774,
+    -2.67949400419915,
+    2.45814854129954,
+    -2.35907388079205,
+    1.14125209449898,
+    1.53232912981414,
+};
+
+const std::vector<double> kGen2TorsoCollisionPose{
+    1.57607137794121,
+    2.06613762981425,
+    -1.76915077905899,
+    0.959251437141443,
+    -0.725973209527894,
+    1.79390262120717,
+    0.2223354372144,
+};
+
 std::string collisionUrdfPath()
 {
     return std::string(CMVR_ES_SOURCE_DIR) +
            "/model/xiaoyan_description/dual_arm_collision.urdf";
 }
 
+std::string gen2CollisionUrdfPath()
+{
+    return std::string(CMVR_ES_SOURCE_DIR) +
+           "/model/gen2/collision/robot_collision.urdf";
+}
+
+SelfCollisionOptions gen2CollisionOptions()
+{
+    SelfCollisionOptions options;
+    options.ignored_pairs.push_back({"arm_link_5_2", "arm_link_7_2"});
+    options.ignored_pairs.push_back({"body_link", "arm_link_2_2"});
+    return options;
+}
+
 CollisionGeometrySnapshot singleObjectSnapshot(double x,
                                                 double angle,
                                                 double radius)
@@ -82,6 +150,64 @@ TEST(SelfCollisionCheckerTest, RemovesConfiguredIgnoredPair)
     EXPECT_EQ(filtered.activePairCount() + 1, baseline.activePairCount());
 }
 
+TEST(SelfCollisionCheckerTest, LoadsGen2RightArmCollisionModel)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(
+        gen2CollisionUrdfPath(),
+        kGen2RightArmJoints,
+        gen2CollisionOptions(),
+        &error)) << error;
+    EXPECT_EQ(checker.dof(), 7U);
+    EXPECT_EQ(checker.activePairCount(), 19U);
+
+    CollisionGeometrySnapshot snapshot;
+    ASSERT_TRUE(checker.makeSnapshot(kGen2SetupPose, &snapshot, &error)) << error;
+    EXPECT_EQ(snapshot.objects.size(), 8U);
+
+    const SelfCollisionResult setup_result = checker.check(snapshot);
+    ASSERT_TRUE(setup_result.valid) << setup_result.error;
+    EXPECT_FALSE(setup_result.in_collision);
+    EXPECT_GT(setup_result.minimum_distance_m, 0.02);
+}
+
+TEST(SelfCollisionCheckerTest, ClassifiesGen2SafetyDistances)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(
+        gen2CollisionUrdfPath(),
+        kGen2RightArmJoints,
+        gen2CollisionOptions(),
+        &error)) << error;
+
+    const SelfCollisionResult warning_result = checker.check(kGen2WarningPose);
+    ASSERT_TRUE(warning_result.valid) << warning_result.error;
+    EXPECT_FALSE(warning_result.in_collision);
+    EXPECT_GT(warning_result.minimum_distance_m, 0.005);
+    EXPECT_LE(warning_result.minimum_distance_m, 0.02);
+
+    const SelfCollisionResult stop_result = checker.check(kGen2StopPose);
+    ASSERT_TRUE(stop_result.valid) << stop_result.error;
+    EXPECT_FALSE(stop_result.in_collision);
+    EXPECT_GT(stop_result.minimum_distance_m, 0.0);
+    EXPECT_LE(stop_result.minimum_distance_m, 0.005);
+
+    const SelfCollisionResult collision_result = checker.check(kGen2CollisionPose);
+    ASSERT_TRUE(collision_result.valid) << collision_result.error;
+    EXPECT_TRUE(collision_result.in_collision);
+    EXPECT_LE(collision_result.minimum_distance_m, 0.0);
+
+    const SelfCollisionResult torso_result =
+        checker.check(kGen2TorsoCollisionPose);
+    ASSERT_TRUE(torso_result.valid) << torso_result.error;
+    EXPECT_TRUE(torso_result.in_collision);
+    EXPECT_LE(torso_result.minimum_distance_m, 0.0);
+    EXPECT_TRUE(torso_result.first == "body_link" ||
+                torso_result.second == "body_link");
+}
+
 TEST(DistanceSamplingPolicyTest, SamplesByAccumulatedGeometryDisplacement)
 {
     DistanceSamplingPolicy policy;

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/algorithms/motion_planner/arm_motion/CMakeLists.txt

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -2,6 +2,7 @@
 #define CMVR_ES_AGV_TYPES_H
 
 #include <cstdint>
+#include <functional>
 #include <optional>
 #include <string>
 #include <unordered_map>
@@ -90,6 +91,14 @@ enum class AgvTaskType {
     Custom
 };
 
+/**
+ * @brief 固定距离平移使用的距离参考模式。
+ */
+enum class AgvTranslationMode {
+    Odometry = 0,
+    Localization
+};
+
 /**
  * @brief AGV 车体坐标系下的平面速度。
  *
@@ -101,6 +110,20 @@ struct AgvVelocity {
     double wz{0.0};
 };
 
+
+
+/**
+ * @brief AGV 车体坐标系下的固定距离平移参数。
+ */
+struct AgvTranslation {
+    double distance{0.0};
+    double vx{0.0};
+    double vy{0.0};
+    AgvTranslationMode mode{AgvTranslationMode::Odometry};
+};
+
+
+
 /**
  * @brief 导航通用运动约束和执行选项。
  *
@@ -114,7 +137,13 @@ struct AgvMotionOptions {
     double reach_distance{0.0};
     double reach_angle{0.0};
     double speed_ratio{1.0};
-    bool asynchronous{true};
+    // 导航默认同步阻塞；调用方只有显式设为 true 才在任务接受后立即返回。
+    bool asynchronous{false};
+    int wait_timeout_ms{0};
+    int poll_interval_ms{0};
+    // 不带 RPC 框架依赖的取消检查。同步导航等待期间可由
+    // 上层绑定 deadline/cancel；驱动不得在函数返回后保留该回调。
+    std::function<bool()> cancellation_requested;
 };
 
 /**
@@ -220,7 +249,7 @@ struct AgvPathSegment {
 /**
  * @brief AGV 扫图过程中产生的数据文件。
  *
- * content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。
+ * content 可保存控制器返回的二进制内容，例如 SEER Robokit 的 rawmap zip 包。
  */
 struct AgvMappingDataFile {
     std::string name;

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

@@ -2,6 +2,7 @@
 #define CMVR_ES_ARM_TYPES_H
 
 #include <cstdint>
+#include <functional>
 #include <string>
 #include <vector>
 
@@ -103,6 +104,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
     {
@@ -112,6 +118,14 @@ struct JointGroupState {
     }
 };
 
+struct JointTrajectoryPoint {
+    double time_s{0.0};
+    std::vector<double> position;
+    std::vector<double> velocity;
+};
+
+using JointTrajectory = std::vector<JointTrajectoryPoint>;
+
 struct JointPositionCommand {
     std::vector<double> position;
 
@@ -157,6 +171,11 @@ struct MotionOptions {
     double jerk{5.0};
     std::vector<double> joint_velocity_limits;
     bool asynchronous{false};
+    // Framework-independent cancellation check used by queued synchronous
+    // motion. Cancellation after device acceptance retains a typed motion
+    // barrier; the owner must call stopMotion() to confirm physical idle.
+    // Drivers must not retain this callback after moveJ/moveL returns.
+    std::function<bool()> cancellation_requested;
 };
 
 struct ServoOptions {
@@ -165,6 +184,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 +224,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

@@ -29,10 +29,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 +109,49 @@ output/bin/protoc \
 
 该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。
 
+## 双边遥操配置
+
+源码仓库保持唯一根入口 [`cmvr_es.pb.txt`](cmvr_es.pb.txt)。统一的
+[`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) 已声明
+`ume_left`、`ume_right`、`ti5_motors` 和 `right_arm`；统一的
+[`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) 已声明
+`ume_teleop` 和 gRPC server。角色差异不通过增加新的源码根配置文件表达，而由
+两台机器各自的外部部署配置决定。
+
+UME 主端部署配置应只启用本机需要的 UME 设备和 `ume_teleop` Task：
+
+- `ume_left`、`ume_right` 在 DeviceManager 层默认关闭；
+- [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的
+  `hardware_enabled` 也默认关闭；
+- 两层硬件门必须在完成 CAN 映射、限位标定和安全验收后分别启用；
+- 当前 Task 只实现会话、重连、心跳和 latest-only 指令邮箱，尚无生产算法调用
+  `submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。
+
+机器人从端部署配置应只启用经过验收的机械臂设备以及所需的 gRPC server：
+
+- `ti5_motors`、`right_arm` 默认关闭；
+- `ArmTeleop` 服务后端默认关闭，模型哈希必须由部署配置明确给出；
+- 当前 `MotorRobotArm::servoJ()` 仍是逐关节顺序写，不满足遥操作组伺服能力门；
+- 启动 gRPC server 不代表允许遥操作执行，也不能绕过设备层硬件门。
+
+部署时应把完整配置树分别复制到两台机器的外部目录，并继续使用相同的标准文件名：
+
+```text
+/etc/cmvr-es/ume/cmvr_es.pb.txt
+/etc/cmvr-es/robot/cmvr_es.pb.txt
+```
+
+两套根配置都继续引用各自目录下同名的
+`manager/device_manager.pb.txt` 和 `manager/task_manager.pb.txt`。运行命令为：
+
+```bash
+./output/bin/cmvr_es --config /etc/cmvr-es/ume/cmvr_es.pb.txt
+./output/bin/cmvr_es --config /etc/cmvr-es/robot/cmvr_es.pb.txt
+```
+
+UME 主端需要填写从端地址、会话 manifest 和认证配置；机器人从端需要填写现场
+机械臂配置。不要把生产 IP、token、私钥或设备标定值提交到仓库默认配置。
+
 ## 生产配置
 
 `cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。

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

@@ -8,8 +8,9 @@ agv {
   }
 
   agvs {
+    # 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。
     id: "src1100"
-    src1100_agv {
+    seer_robokit_agv {
       ip: "192.168.192.5"
       port_status: 19204
       port_control: 19205
@@ -18,6 +19,7 @@ agv {
       port_other: 19210
       port_push: 19301
       recv_timeout_ms: 1000
+      control_nick_name: "cmvr-es"
       enable_state_push: true
       state_push_interval_ms: 200
       state_push_included_fields: "x"

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

@@ -0,0 +1,151 @@
+arm {
+  robot_arms {
+    id: "mujoco_right_arm"
+
+    motor {
+      motor_system_id: "mujoco_motors"
+      motor_group_ids: "mujoco_right_arm"
+      dof: 7
+      joint_names: "right_arm_J1"
+      joint_names: "right_arm_J2"
+      joint_names: "right_arm_J3"
+      joint_names: "right_arm_J4"
+      joint_names: "right_arm_J5"
+      joint_names: "right_arm_J6"
+      joint_names: "right_arm_J7"
+      upd_freq: 1000
+      buffer_size: 50
+      default_vel: 0.6
+      default_acc: 2.0
+    }
+
+    kinematics {
+      pinocchio_dls_ik_solver {
+        urdf_path: "model/gen2/robot.urdf"
+        base_frame_name: "body_link"
+        flange_frame_name: "arm_link_7_2"
+        max_iters: 200
+        pos_eps: 1e-6
+        rot_eps: 1e-6
+        damping: 1e-5
+        joint_limit_policy {
+          limits {
+            enable: true
+            source: JOINT_LIMIT_SOURCE_CUSTOM
+            joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.15
+            max_push: 0.25
+            weight: 2.0
+          }
+        }
+      }
+    }
+
+    motion {
+      move_j {
+        toppra_joint_motion_planner {
+          path_type: TOPPRA_PATH_TYPE_QUINTIC
+          sample_period_s: 0.001
+          grid_size: 150
+          high_grid_size: 300
+        }
+      }
+
+      move_l {
+        pinocchio_cartesian_motion_planner {
+          sample_period_s: 0.001
+          position_gain: 4.0
+          rotation_gain: 4.0
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.005
+          }
+          joint_continuity_check {
+            enable: true
+            max_joint_delta_rad: 0.05
+            max_joint_velocity_rad_s: 4.0
+            max_joint_acceleration_rad_s2: 100.0
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 10.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 10.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+      }
+
+      speed_l {
+        pinocchio_cartesian_motion_planner {
+          linear_velocity_max: 0.5
+          linear_acceleration_max: 2.0
+          linear_jerk_max: 10.0
+          angular_velocity_max: 1.0
+          angular_acceleration_max: 5.0
+          angular_jerk_max: 12.0
+          linear_target_replan_threshold: 1e-4
+          angular_target_replan_threshold: 1e-4
+          linear_reverse_cos_threshold: -0.8660254037844386
+          linear_reverse_switch_speed_threshold: 1e-3
+          enforce_joint_acceleration_limits: true
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.005
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 4.0
+            max_joint_acceleration_rad_s2: 100.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 10.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 10.0
+            min_desired_linear_speed: 0.01
+            min_desired_angular_speed: 1e-4
+          }
+        }
+
+        speed_l_controller {
+          cartesian_velocity_controller {
+            control_period_s: 0.001
+            stop_twist_norm: 1e-9
+            stop_command_velocity_norm: 1e-3
+            stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 2.0
+          }
+        }
+      }
+    }
+  }
+}

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

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

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

@@ -0,0 +1,35 @@
+motor {
+  id: "mujoco_motors"
+
+  motor_groups {
+    id: "mujoco_right_arm"
+    bus_type: MOTOR_BUS_MUJOCO
+    vendor: MOTOR_VENDOR_MUJOCO
+    protocol: MOTOR_PROTOCOL_MUJOCO
+    mujoco {
+      world_id: "mujoco_world"
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+      joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "right_arm_J1" }
+      motors { id: 2 joint_name: "right_arm_J2" }
+      motors { id: 3 joint_name: "right_arm_J3" }
+      motors { id: 4 joint_name: "right_arm_J4" }
+      motors { id: 5 joint_name: "right_arm_J5" }
+      motors { id: 6 joint_name: "right_arm_J6" }
+      motors { id: 7 joint_name: "right_arm_J7" }
+    }
+  }
+}

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

@@ -100,7 +100,7 @@ device_manager {
     id: "aubo_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/aubo_arm.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
@@ -110,6 +110,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
@@ -118,10 +135,11 @@ device_manager {
   }
   
   devices {
+    # 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。
     id: "src1100"
     type: DEVICE_TYPE_AGV
-    config_file: "devices/agv/src1100.pb.txt"
+    config_file: "devices/agv/seer_robokit.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {

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: false
   }
+  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/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/quic_edge_task/quic_edge_task.pb.txt

@@ -3,11 +3,11 @@ quic_edge {
 
   # Task enablement is controlled by manager/task_manager.pb.txt. Configure a
   # reachable QUIC Gateway and TLS policy before enabling the task there.
-  server_host: "192.168.0.118"
+  server_host: "192.168.0.222"
   server_port: 4433
   alpn: "cmvr-quic-edge/1"
   node_id: "cmvr-edge"
-  robot_id: "CN-CMVR-MBLRV1-CHAGAN-20260731-001"
+  robot_id: "CN-CMVR-MBLRV1-AIMA-20260806-001"
   software_version: "0.1"
 
   # The existing cmvr-es gRPC server remains the robot-control endpoint. "auto"
@@ -24,8 +24,8 @@ quic_edge {
   control_response_timeout_ms: 1000
 
   tls {
-    ca_file: "certs/quic-test-ca.crt"
+    ca_file: "certs/cmvr-quic-ca.crt"
-    server_name: "192.168.0.118"
+    server_name: "192.168.0.222"
     allow_insecure: false
   }
 

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

@@ -21,4 +21,14 @@ self_collision_task {
     warning_distance_m: 0.02
     stop_distance_m: 0.005
   }
+
+
+  recovery {
+    clear_distance_m: 0.025
+    stable_period_s: 0.1
+    max_joint_velocity_rad_s: 0.15
+    max_joint_acceleration_rad_s2: 0.3
+    history_duration_s: 10.0
+    max_distance_regression_m: 0.001
+  }
 }

cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt

@@ -0,0 +1,37 @@
+self_collision_task {
+  id: "gen2_right_arm_self_collision"
+  arm_id: "mujoco_right_arm"
+
+  checker {
+    urdf_path: "model/gen2/collision/robot_collision.urdf"
+
+    # These second-neighbor mounting bodies overlap in normal assembled poses.
+    ignored_pairs {
+      first: "arm_link_5_2"
+      second: "arm_link_7_2"
+    }
+    ignored_pairs {
+      first: "body_link"
+      second: "arm_link_2_2"
+    }
+  }
+
+  sampling {
+    max_geometry_displacement_m: 0.002
+    max_check_period_s: 0.01
+  }
+
+  safety {
+    warning_distance_m: 0.02
+    stop_distance_m: 0.005
+  }
+
+  recovery {
+    clear_distance_m: 0.05
+    stable_period_s: 0.1
+    max_joint_velocity_rad_s: 0.3
+    max_joint_acceleration_rad_s2: 5.0
+    history_duration_s: 10.0
+    max_distance_regression_m: 0.001
+  }
+}

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

@@ -36,13 +36,13 @@ config/cmvr_es.pb.txt
 | 大类 | 抽象接口 | 类别工厂 | 当前可选后端 |
 | --- | --- | --- | --- |
 | Camera | [`camera/abstract_camera.h`](camera/abstract_camera.h) | [`camera/camera_factory.h`](camera/camera_factory.h) | UVC、RealSense、Hikvision |
-| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SRC1100 |
+| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SEER Robokit |
-| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、AUBO、Huayan |
+| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、[AUBO](arm/aubo_arm/README.md)、Huayan、UME |
 | DexHand | [`dexhand/abstract_dexhand.h`](dexhand/abstract_dexhand.h) | [`dexhand/dexhand_factory.h`](dexhand/dexhand_factory.h) | RH56DFTP、PX6AXGen3 |
 | Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg |
 | Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg |
 | BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot |
-| MotorSystem | `motor/motor_system/` | DeviceFactory 直接创建 | CAN/MuJoCo motor group |
+| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT |
 
 代码目录存在不等于已经接入配置创建链：
 

cmvr-es/devices/agv/CMakeLists.txt

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

cmvr-es/devices/agv/abstract_agv.h

@@ -15,6 +15,12 @@
 
 namespace cmvr::device {
 
+enum class AgvActionKind {
+    NavigateToPose,
+    NavigateToStation,
+    FollowPath,
+};
+
 /**
  * @brief AGV/移动底盘设备抽象基类。
  *
@@ -28,6 +34,16 @@ public:
 
     DeviceKind kind() const noexcept override { return DeviceKind::AGV; }
 
+    /**
+     * @brief Whether this backend provides terminal-state and stopped-motion
+     *        confirmation plus bounded cancellation suitable for synchronous
+     *        Action execution.
+     */
+    virtual bool supportsSynchronousAction(AgvActionKind) const noexcept
+    {
+        return false;
+    }
+
     /**
      * @brief 获取 AGV 运行状态快照。
      */
@@ -85,12 +101,41 @@ public:
     /**
      * @brief 发起显式站点到站点路径导航任务。
      */
-    virtual AgvResult followPath(const std::vector<AgvPathSegment>& path)
+    virtual AgvResult followPath(
+        const std::vector<AgvPathSegment>& path)
     {
         (void)path;
         return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented");
     }
 
+
+
+    /**
+     * @brief 按指定速度执行固定距离平移。
+     *
+     * 返回成功表示控制器已经接受命令，不表示运动已经完成。
+     */
+    virtual AgvResult translate(const AgvTranslation& translation)
+    {
+        (void)translation;
+        return AgvResult::failure(
+            AgvErrorCode::UnsupportedCommand,
+            "translate not implemented");
+    }
+
+    /**
+     * @brief 发起显式站点到站点路径导航任务，并指定同步/异步选项。
+     *
+     * 保留单参数虚函数以兼容已有派生类；旧实现会由本重载转发。
+     */
+    virtual AgvResult followPath(
+        const std::vector<AgvPathSegment>& path,
+        const AgvMotionOptions& options)
+    {
+        (void)options;
+        return followPath(path);
+    }
+    
     /**
      * @brief 暂停当前导航任务，如果设备支持。
      */
@@ -138,6 +183,19 @@ public:
         return setVelocity(AgvVelocity{});
     }
 
+    /**
+     * @brief 确认 AGV 已进入可安全释放控制权的停止状态。
+     *
+     * 该接口只在导航任务已终止且底盘速度经过连续采样确认为零后返回
+     * 成功；仅收到取消、停止或零速度命令的应答不构成成功。
+     */
+    virtual AgvResult confirmMotionStopped()
+    {
+        return AgvResult::failure(
+            AgvErrorCode::UnsupportedCommand,
+            "confirmMotionStopped not implemented");
+    }
+
     /**
      * @brief 查询 AGV 可用地图名称列表。
      */

cmvr-es/devices/agv/agv_factory.h

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

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

@@ -0,0 +1,57 @@
+add_library(seer_robokit_agv SHARED
+        src/seer_robokit_agv.cpp
+        src/seer_robokit_transport.cpp
+        src/seer_robokit_control.cpp
+        src/seer_robokit_status.cpp
+        src/seer_robokit_navigation.cpp
+        src/seer_robokit_navigation_wait.cpp
+        src/seer_robokit_map.cpp
+        include/seer_robokit_agv.h
+        include/seer_robokit_protocol.h
+        include/seer_robokit_utils.h
+        include/seer_robokit_navigation_utils.h
+        include/seer_robokit_pgv_utils.h
+)
+
+target_include_directories(seer_robokit_agv
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+        ${PROJECT_SOURCE_DIR}/cmvr-es
+)
+
+target_link_libraries(seer_robokit_agv
+        PUBLIC
+        cmvr_es::proto
+        jsoncpp
+)
+
+add_library(cmvr_es::device::seer_robokit_agv ALIAS seer_robokit_agv)
+install(TARGETS seer_robokit_agv LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(seer_robokit_control_authority_test
+            tests/seer_robokit_control_authority_test.cpp
+    )
+    target_link_libraries(seer_robokit_control_authority_test
+            PRIVATE
+            cmvr_es::device::seer_robokit_agv
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME seer_robokit_control_authority_test
+            COMMAND seer_robokit_control_authority_test
+    )
+    set(_seer_robokit_control_authority_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _seer_robokit_control_authority_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(seer_robokit_control_authority_test PROPERTIES
+            TIMEOUT 180
+            ENVIRONMENT "${_seer_robokit_control_authority_test_environment}"
+    )
+endif()

cmvr-es/devices/agv/seer_robokit/README.md

@@ -0,0 +1,395 @@
+# 仙工 SEER Robokit AGV 适配器
+
+`SeerRobokitAgv` 将仙工 SEER Robokit TCP/IP API 适配为 CMVR 的通用
+`AbstractAGV`/`cmvr.api.AgvService`。厂商命令号、端口、抢占控制权、状态轮询、
+地图格式转换和错误码解析都封装在本目录内。
+
+本项目现场使用的控制器型号仍是 SRC1100，所以设备实例 ID 保持为
+`src1100`；它只用于配置关联和 gRPC 路由，不再作为驱动实现名称。后端配置字段
+使用 `seer_robokit_agv`，目录、类、库和测试统一使用 `seer_robokit` /
+`SeerRobokitAgv` 命名。
+
+从旧版本升级时，外部部署配置必须同步使用 `seer_robokit_agv { ... }`，并把
+配置路径更新为 `devices/agv/seer_robokit.pb.txt`；设备实例 ID 保持不变。程序、
+外部配置和部署脚本需要原子升级，不能把旧字段或旧路径与新二进制混用。
+
+返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。
+
+## 代码与配置
+
+所有驱动头文件统一放在 `include/`，实现文件统一放在 `src/`；测试源码独立放在
+`tests/`。除 `seer_robokit_agv.h` 外，其余头文件均为驱动内部实现细节。
+
+- 公共类声明：[`include/seer_robokit_agv.h`](include/seer_robokit_agv.h)
+- 导航轮询工具：
+  [`include/seer_robokit_navigation_utils.h`](include/seer_robokit_navigation_utils.h)
+- PGV 参数转换：
+  [`include/seer_robokit_pgv_utils.h`](include/seer_robokit_pgv_utils.h)
+- 协议常量：[`include/seer_robokit_protocol.h`](include/seer_robokit_protocol.h)
+- 通用解析工具：[`include/seer_robokit_utils.h`](include/seer_robokit_utils.h)
+- 生命周期和连接：[`src/seer_robokit_agv.cpp`](src/seer_robokit_agv.cpp)
+- TCP 帧与收发：[`src/seer_robokit_transport.cpp`](src/seer_robokit_transport.cpp)
+- 控制权与受控命令：[`src/seer_robokit_control.cpp`](src/seer_robokit_control.cpp)
+- 状态与推送缓存：[`src/seer_robokit_status.cpp`](src/seer_robokit_status.cpp)
+- 导航命令：[`src/seer_robokit_navigation.cpp`](src/seer_robokit_navigation.cpp)
+- 阻塞等待与停车确认：
+  [`src/seer_robokit_navigation_wait.cpp`](src/seer_robokit_navigation_wait.cpp)
+- 地图和建图：[`src/seer_robokit_map.cpp`](src/seer_robokit_map.cpp)
+- 假控制器测试：
+  [`tests/seer_robokit_control_authority_test.cpp`](tests/seer_robokit_control_authority_test.cpp)
+- 设备配置：
+  [`../../../config/devices/agv/seer_robokit.pb.txt`](../../../config/devices/agv/seer_robokit.pb.txt)
+- DeviceManager 配置：
+  [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
+- gRPC API：
+  [`../../../../protos/cmvr/api/agv_service.proto`](../../../../protos/cmvr/api/agv_service.proto)、
+  [`../../../../protos/cmvr/api/agv_command.proto`](../../../../protos/cmvr/api/agv_command.proto)、
+  [`../../../../protos/cmvr/api/agv_utils.proto`](../../../../protos/cmvr/api/agv_utils.proto)
+
+## 配置和启动
+
+现场配置至少需要修改控制器 IP；端口通常保持仙工默认值：
+
+```textproto
+agv {
+  agvs {
+    id: "src1100"
+    seer_robokit_agv {
+      ip: "192.168.192.5"
+      port_status: 19204
+      port_control: 19205
+      port_nav: 19206
+      port_config: 19207
+      port_other: 19210
+      port_push: 19301
+      recv_timeout_ms: 1000
+      control_nick_name: "cmvr-es"
+      enable_state_push: true
+      state_push_interval_ms: 200
+      enable_map_update: true
+      map_update_interval_ms: 1000
+      map_update_history_size: 8
+    }
+  }
+}
+```
+
+还要在 `device_manager.pb.txt` 中确认同一个设备 id，并在完成现场安全检查后把
+`enable` 改为 `true`。源码默认配置故意保持关闭。
+
+```textproto
+devices {
+  id: "src1100"
+  type: DEVICE_TYPE_AGV
+  config_file: "devices/agv/seer_robokit.pb.txt"
+  enable: true
+}
+```
+
+构建、安装并启动：
+
+```bash
+cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
+cmake --build build -j2
+cmake --install build
+./output/bin/cmvr_es
+```
+
+`output/bin/cmvr_es` 默认读取 `output/bin/config/`。修改源码配置后需要重新安装，
+或通过程序支持的外部配置入口启动，不能只修改源码文件后继续使用旧的
+`output/` 配置。
+
+## 控制器端口和命令
+
+| 端口 | 主要用途 | 当前使用的命令 |
+| --- | --- | --- |
+| `19204` | 状态、站点、地图和建图文件 | `1004`、`1007`、`1020`、`1101`、`1110`、`1300`、`1301`、`1780`、`1800` |
+| `19205` | 底盘控制 | `2000`、`2010`、`2022` |
+| `19206` | 导航任务 | `3001`、`3002`、`3003`、`3051`、`3066`、`3067` |
+| `19207` | 控制权、地图上传下载 | `4005`、`4010`、`4011` |
+| `19210` | 开始/停止建图 | `6100`、`6101` |
+| `19301` | 机器人状态推送 | `9300`/`19300` 配置，`19301` 推送 |
+
+所有会改变机器人或控制器状态的调用都在 SEER Robokit 子类内部先通过 `4005`
+抢权，负载为稳定的 `nick_name`，成功后才发送实际命令。普通命令集中走
+`sendControlledCommand_`；`emergencyStop` 为保证 `2000` 和导航取消之间不被
+插入其他命令，会在同一个控制序列锁内只抢一次权。只读查询不抢权。不要在
+gRPC 客户端另做一套租约逻辑。
+
+## gRPC 接口概览
+
+默认示例端点为 `127.0.0.1:50052`；远程部署时替换为 CMVR 服务所在主机，
+不是 SEER Robokit 原生 TCP 端口。
+
+| gRPC 方法 | SEER Robokit 行为 | 说明 |
+| --- | --- | --- |
+| `getRuntimeState` | 推送缓存，缺失时查询 `1004/1007/1300` | 只读 |
+| `getNavigationStatus` | 跟踪任务查询 `1110`，无精确上下文时回退 `1020` | 只读；同步等待另用 `1101` 确认停车 |
+| `emergencyStop` | `2000`，再执行 `3003` 或 `3067` | 软件停止，不替代硬件急停 |
+| `clearFault` | 未实现 | 返回 `UnsupportedCommand` |
+| `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿，仅双轮差速底盘 |
+| `navigateToStation` | `3051` | 站点路径导航；PGV 二次定位也使用此方法 |
+| `followPath` | `3066` | 仙工“指定路径导航”，与 `3051` 不同 |
+| `pauseNavigation` / `resumeNavigation` | `3001` / `3002` | 导航控制 |
+| `cancelNavigation` | `3003`，路径队列使用 `3067` | 取消当前跟踪任务 |
+| `setVelocity` / `stopVelocityControl` | `2010` | 车体速度；停止时发送全零速度 |
+| `listMaps` / `listStations` | `1300` / `1301` | 只读 |
+| `switchMap` | `2022` | 会改变定位所用地图 |
+| `uploadMap` / `downloadMap` | `4010` / `4011` | 上传会抢权，下载只读 |
+| `startMapping` / `stopMapping` | `6100` / `6101` | 建图控制 |
+| `streamMap` | `1780/1800` 加内部解析和缓存 | 对外发送统一 2D/3D 地图，不暴露 `.smap` 原始格式 |
+
+查询运行状态：
+
+```bash
+grpcurl -plaintext \
+  -d '{"header":{"deviceId":"src1100"}}' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/getRuntimeState
+```
+
+查询导航状态：
+
+```bash
+grpcurl -plaintext \
+  -d '{"header":{"deviceId":"src1100"}}' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/getNavigationStatus
+```
+
+列出地图和当前地图站点：
+
+```bash
+grpcurl -plaintext \
+  -d '{"header":{"deviceId":"src1100"}}' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/listMaps
+
+grpcurl -plaintext \
+  -d '{"header":{"deviceId":"src1100"}}' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/listStations
+```
+
+## 导航的同步语义
+
+`navigateToPose`、`navigateToStation` 和 `followPath` 默认同步阻塞。控制器接受
+命令后，适配器继续轮询精确任务状态，并结合 `1101` 状态确认底盘已经停车；
+到达、失败、取消、遇障停止或超时后才返回。`waitTimeoutMs` 为 `0` 时使用
+适配器默认值，当前为 10 分钟；`pollIntervalMs` 为 `0` 时当前使用 200 ms。
+
+连续观察到障碍阻挡且底盘已经停止后，适配器会主动取消该导航；清理结果不明确
+时还可能发送软件停止。任务不会在障碍消失后由本次调用自动恢复。等待超时、
+RPC cancel 和 deadline 到期也会进入安全取消及停车确认，因此函数返回时间可能
+晚于最初发现障碍或取消请求的时刻。
+
+调用方的 gRPC deadline 必须大于预计行程时间和 `waitTimeoutMs`。RPC 被取消或
+deadline 到期时，适配器会进入安全取消/停车确认流程。显式设置
+`"asynchronous":true` 后不会等待任务终态：站点导航和指定路径导航在控制器
+接受后返回；自由导航仍会做最长约 1.5 秒的启动确认。异步成功不代表已经到点。
+
+`AgvMotionOptions` 中，SEER Robokit 的 `3051` 导航当前支持：
+
+| gRPC 字段 | 控制器字段 | 单位 |
+| --- | --- | --- |
+| `maxSpeed` | `max_speed` | m/s |
+| `maxAngularSpeed` | `max_wspeed` | rad/s |
+| `maxAcceleration` | `max_acc` | m/s² |
+| `maxAngularAcceleration` | `max_wacc` | rad/s² |
+| `reachDistance` | `reach_dist` | m |
+| `reachAngle` | `reach_angle` | rad |
+
+`asynchronous`、`waitTimeoutMs` 和 `pollIntervalMs` 由适配器本地执行。
+`speedRatio` 当前没有对应的 SEER Robokit 序列化字段。`followPath` 的运动选项当前只
+控制同步/异步等待、超时和轮询；在没有确认 `3066` 的速度字段前，不会猜测性地
+写入每个路径段。
+
+## 固定路径导航的 PGV 二次定位
+
+仙工文档 [“路径导航 / 2. 固定路径导航 PGV 二次定位调整”](https://seer-group.feishu.cn/wiki/Q26SwaNoGisuLWk2vCxcPfVWn2e)
+说明 PGV 参数是 `3051 / robot_task_gotarget_req` 的顶层可选字段。因此在 CMVR
+中应调用 `navigateToStation`，不是 `followPath`。后者对应另一条
+`3066 / 指定路径导航` 协议，现有仙工资料和仓库历史都没有证明 `3066` 支持
+PGV 字段。
+
+PGV 参数通过 `adapterParams.values` 传入。protobuf map 的值是字符串，
+SEER Robokit 适配器会在任何状态查询、抢权和运动命令之前完成校验，再转换为控制器
+要求的 JSON `bool`/`number`：
+
+| `adapterParams.values` 键 | 输出 JSON 类型 | 含义 |
+| --- | --- | --- |
+| `use_pgv` | `bool` | 使用上视 PGV |
+| `use_down_pgv` | `bool` | 使用下视 PGV |
+| `pgv_adjust_dist` | `number` | 最大调整半径，必须为有限非负数；用于仙工第 3/4 种调整方式 |
+| `pgv_adjust_cx` | `number` | 调整范围圆心在二维码坐标系下的 X 偏移；用于第 4 种方式 |
+| `pgv_adjust_cy` | `number` | 调整范围圆心在二维码坐标系下的 Y 偏移；用于第 4 种方式 |
+| `pgv_x_adjust` | `number` | 仅调整小车 X 方向误差；用于第 2 种方式 |
+
+所有数字都必须是完整、有限的数字字符串；偏移量允许正负。适配器不臆造
+调整半径上限，也不假定上视和下视一定互斥，这些约束应由实际 PGV 安装、标定和
+当前控制器版本确定。显式的 `"false"` 和 `"0"` 仍会作为原生布尔值和数值
+发给控制器；没有给出的字段不会发送。第 2/3/4 种方式由控制器和站点配置决定，
+本接口只传递与所选方式匹配的调整参数。
+
+一旦请求中出现任意 PGV 键，适配器只允许同时出现 `source_id`、`task_id` 和
+上述 PGV 字段；`operation`、`jack_height`、脚本名或未知扩展字段都会在状态
+查询和抢权前被拒绝，避免一次 PGV 导航意外夹带顶升、货叉、IO 或脚本动作。
+没有 PGV 键的既有站点导航扩展语义保持不变。
+
+上视 PGV 示例。该命令会让机器人导航到 `AP1`，只能在确认地图、站点、PGV
+标定、行驶区域和急停人员后执行：
+
+```bash
+grpcurl -plaintext \
+  -d '{
+    "header":{"deviceId":"src1100"},
+    "stationId":"AP1",
+    "options":{
+      "maxSpeed":0.15,
+      "maxAcceleration":0.15,
+      "asynchronous":false,
+      "waitTimeoutMs":300000,
+      "pollIntervalMs":200
+    },
+    "adapterParams":{"values":{
+      "use_pgv":"true",
+      "pgv_adjust_dist":"0.3",
+      "pgv_adjust_cx":"-0.3",
+      "pgv_adjust_cy":"0"
+    }}
+  }' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/navigateToStation
+```
+
+下视 PGV 使用同一接口，把 `use_down_pgv` 设为字符串 `"true"`；其他调整
+字段是否需要传入取决于现场定位方案。如果控制器版本要求明确起点，可在同一个
+map 中增加 `"source_id":"实际起点站点"`；默认起点为 `SELF_POSITION`。
+
+仙工在线文档当前有两处拼写不一致：
+
+- 代码块出现了损坏字段 `pgv_adjustuse_pgv_dist`；适配器会拒绝它，正确字段是
+  `pgv_adjust_dist`；
+- 表格写成 `pgv_ajdust_cy`，而示例和仓库旧版序列化代码使用
+  `pgv_adjust_cy`。适配器兼容接收前者，但只向控制器输出规范字段
+  `pgv_adjust_cy`；两个拼写同时出现会因歧义被拒绝。
+
+C++ 调用同样复用通用扩展参数：
+
+```cpp
+cmvr::device::AgvMotionOptions options;
+options.max_speed = 0.15;
+options.max_acceleration = 0.15;
+
+cmvr::device::AgvAdapterParams adapter;
+adapter.values["use_pgv"] = "true";
+adapter.values["pgv_adjust_dist"] = "0.3";
+adapter.values["pgv_adjust_cx"] = "-0.3";
+adapter.values["pgv_adjust_cy"] = "0";
+
+const auto result = agv.navigateToStation("AP1", options, adapter);
+```
+
+## 其他导航和控制示例
+
+自由导航使用地图绝对坐标，不是“相对当前位置移动多少米”。示例只展示请求
+结构，发送前必须读取当前位姿并确认目标在同一地图的安全区域：
+
+```bash
+grpcurl -plaintext \
+  -d '{
+    "header":{"deviceId":"src1100"},
+    "pose":{"x":1.0,"y":0.0,"theta":0.0},
+    "options":{"maxSpeed":0.15,"maxAcceleration":0.15}
+  }' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/navigateToPose
+```
+
+显式站点路径使用 `3066`：
+
+```bash
+grpcurl -plaintext \
+  -d '{
+    "header":{"deviceId":"src1100"},
+    "path":[
+      {"sourceStation":"LM1","targetStation":"LM2"},
+      {"sourceStation":"LM2","targetStation":"AP1"}
+    ]
+  }' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/followPath
+```
+
+暂停、继续和取消的请求体直接是 `CommandHeader.Request`，没有外层 `header`：
+
+```bash
+grpcurl -plaintext -d '{"deviceId":"src1100"}' \
+  127.0.0.1:50052 cmvr.api.AgvService/pauseNavigation
+
+grpcurl -plaintext -d '{"deviceId":"src1100"}' \
+  127.0.0.1:50052 cmvr.api.AgvService/resumeNavigation
+
+grpcurl -plaintext -d '{"deviceId":"src1100"}' \
+  127.0.0.1:50052 cmvr.api.AgvService/cancelNavigation
+```
+
+差速底盘的 `vy` 应保持 `0`。低层速度控制不等价于导航，并可能与已有任务
+冲突；只应在专门的速度控制测试流程中使用：
+
+```bash
+grpcurl -plaintext \
+  -d '{"header":{"deviceId":"src1100"},"velocity":{"vx":0.05,"vy":0,"wz":0}}' \
+  127.0.0.1:50052 \
+  cmvr.api.AgvService/setVelocity
+
+grpcurl -plaintext -d '{"deviceId":"src1100"}' \
+  127.0.0.1:50052 cmvr.api.AgvService/stopVelocityControl
+```
+
+## 错误返回
+
+控制器响应中的非零 `ret_code` 和 `err_msg` 会保留在 `AgvResult.message`，并由
+gRPC 同时写入 transport status message 和反馈头的 `errorMessage`。非 OK RPC
+下，标准客户端通常不会交付响应体，因此跨客户端应以 transport status message
+为准，不要依赖反馈头仍然可见。例如：
+
+```text
+SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose
+```
+
+控制器仅返回“已接收”不等于导航完成；同步接口仍要等待精确任务终态和停车
+确认。若发送后连接中断且控制器是否执行已无法确定，错误会明确提示 outcome
+unknown，调用方不能自动重发运动命令，应先查询状态并取消或停止。
+
+## 安全边界
+
+- 仙工文档明确把 `3051` 定位为任务链或验证测试等单车场景接口；不要把它当作
+  多车调度接口，否则可能出现路径/速度不连续等危险行为。
+- `emergencyStop` 是控制器软件停止，不是功能安全急停；真实系统必须保留可达的
+  硬件急停、安全激光、碰撞条和独立安全链。
+- 首次 PGV 测试应在低速、空载、隔离区域进行，并先核对二维码坐标系、传感器
+  上/下视方向、调整半径和中心偏移的标定值。
+- PGV 同步成功目前能证明精确 `3051` 任务进入终态，并连续确认两次零速度；
+  仙工文档没有明确 `Completed` 是否一定覆盖 PGV 二次调整的全部阶段，仍需实机
+  验证后才能据此联动机械臂。异步成功更不代表 PGV 调整完成。
+- 地图切换、地图上传和开始建图会改变控制器状态，也会先抢占控制权；不要和
+  现场调度系统并行操作。
+- 本目录的假控制器测试验证软件协议、错误路径和并发逻辑，不代表真实 SEER Robokit、
+  底盘、PGV、地图或安全链已经验收。
+
+## 测试
+
+```bash
+cmake --build build \
+  --target seer_robokit_control_authority_test grpc_agv_service_test \
+  -j2
+
+ctest --test-dir build \
+  -R '^(seer_robokit_control_authority_test|grpc_agv_service_test)$' \
+  --output-on-failure
+```
+
+`seer_robokit_control_authority_test` 使用本机回环 TCP 假控制器，需要允许本地
+bind/listen。受限沙箱若禁止创建 socket，只能证明编译通过，不能把未执行的
+fake-controller 场景报告为测试通过。测试过程不会连接真实 AGV。

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h

@@ -0,0 +1,354 @@
+#ifndef CMVR_ES_SEER_ROBOKIT_AGV_H
+#define CMVR_ES_SEER_ROBOKIT_AGV_H
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <json/json.h>
+
+#include "cmvr/config/agv_config/agv_config.pb.h"
+#include "devices/agv/abstract_agv.h"
+
+namespace cmvr::device {
+
+class SeerRobokitAgvTestPeer;
+
+class SeerRobokitAgv final : public AbstractAGV {
+public:
+    explicit SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg);
+    ~SeerRobokitAgv() override;
+
+    std::string typeName() const override { return "SeerRobokitAgv"; }
+
+    bool supportsSynchronousAction(AgvActionKind kind) const noexcept override
+    {
+        switch (kind) {
+            case AgvActionKind::NavigateToPose:
+            case AgvActionKind::NavigateToStation:
+            case AgvActionKind::FollowPath:
+                return true;
+        }
+        return false;
+    }
+
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+    bool update() override;
+
+    AgvRuntimeState runtimeState() const override;
+    AgvNavigationStatus navigationStatus() const override;
+
+    AgvResult emergencyStop() override;
+    AgvResult clearFault() override;
+
+    AgvResult navigateToPose(
+        const math::Pose2d& pose,
+        const AgvMotionOptions& options = {},
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
+    AgvResult navigateToStation(
+        const std::string& station_id,
+        const AgvMotionOptions& options = {},
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
+    AgvResult followPath(
+        const std::vector<AgvPathSegment>& path) override;
+    AgvResult followPath(
+        const std::vector<AgvPathSegment>& path,
+        const AgvMotionOptions& options) override;
+    AgvResult translate(const AgvTranslation& translation) override;
+    AgvResult pauseNavigation() override;
+    AgvResult resumeNavigation() override;
+    AgvResult cancelNavigation() override;
+
+    AgvResult setVelocity(const AgvVelocity& velocity) override;
+    AgvResult confirmMotionStopped() override;
+
+    AgvResult listMaps(std::vector<std::string>& maps) const override;
+    AgvResult listStations(std::vector<AgvStation>& stations) const override;
+    AgvResult switchMap(const std::string& map_name) override;
+    AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
+    AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
+    AgvResult startMapping(const AgvMappingOptions& options = {}) override;
+    AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
+    AgvResult getUnifiedMapUpdate(
+        std::uint64_t after_sequence,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const override;
+    AgvResult stopMapping() override;
+
+private:
+    friend class SeerRobokitAgvTestPeer;
+
+    struct Ports {
+        int status{19204};
+        int control{19205};
+        int navigation{19206};
+        int config{19207};
+        int other{19210};
+        int push{19301};
+    };
+
+    struct PoseTaskStatus {
+        bool found{false};
+        int state{0};
+        int type{0};
+        bool type_present{false};
+        double progress{0.0};
+        std::string detail;
+    };
+
+    struct NavigationSnapshot {
+        int task_status{0};
+        int task_type{0};
+        bool task_status_present{false};
+        bool task_type_present{false};
+        bool blocked{false};
+        bool blocked_present{false};
+        int block_reason{-1};
+        std::string block_reason_raw;
+        bool velocity_present{false};
+        double vx{0.0};
+        double vy{0.0};
+        double w{0.0};
+        bool emergency{false};
+        std::string target_id;
+        std::string active_faults;
+        std::string detail;
+    };
+
+    enum class CommandTransmissionState {
+        NotSent,
+        PossiblySent,
+    };
+
+    struct TrackedNavigationContext {
+        std::string token;
+        std::vector<std::string> task_ids;
+        AgvTaskType type{AgvTaskType::None};
+        std::string target_id;
+        std::vector<std::string> target_ids;
+        std::uint64_t navigation_generation{0};
+        std::chrono::steady_clock::time_point accepted_at{};
+        bool synchronous_wait{false};
+    };
+
+    struct PoseTaskContext {
+        std::string task_id;
+        math::Pose2d target{};
+        double reach_distance{0.0};
+        double reach_angle{0.0};
+        std::uint64_t navigation_generation{0};
+        std::uint64_t controller_fault_sequence_at_start{0};
+        std::uint64_t control_attempt_sequence_at_start{0};
+        std::uint64_t controller_fault_channel_epoch_at_start{0};
+    };
+
+    AgvResult connect_();
+    AgvResult disconnect_();
+    AgvResult emergencyStopTrackedNavigation_(
+        const TrackedNavigationContext* expected_navigation);
+    AgvResult connectSocket_(int& sock, int port);
+    AgvResult ensureOtherSocket_();
+    void closeSocket_(int& sock) const;
+    bool connected_() const;
+
+    AgvResult acquireControl_() const;
+    AgvResult confirmPoseNavigationStarted_(
+        const PoseTaskContext& context,
+        bool accept_paused,
+        const AgvMotionOptions& options) const;
+    AgvResult waitForPoseNavigationTerminal_(
+        const PoseTaskContext& pose_context,
+        const TrackedNavigationContext& navigation_context,
+        const AgvMotionOptions& options);
+    AgvResult waitForTrackedNavigationTerminal_(
+        const TrackedNavigationContext& context,
+        const AgvMotionOptions& options);
+    AgvResult queryNavigationSnapshot_(NavigationSnapshot& snapshot) const;
+    AgvResult cancelTrackedNavigation_(
+        const TrackedNavigationContext& context,
+        std::uint64_t& accepted_generation);
+    AgvResult waitForCanceledTaskToStop_(
+        const TrackedNavigationContext& context,
+        const AgvMotionOptions& options,
+        const std::string& reason,
+        bool require_global_stopped = false);
+    AgvResult failAndCancelTrackedNavigation_(
+        const TrackedNavigationContext& context,
+        const AgvMotionOptions& options,
+        AgvErrorCode error_code,
+        const std::string& reason);
+    AgvResult queryPoseTaskStatus_(
+        const std::string& task_id,
+        PoseTaskStatus& status) const;
+    AgvResult queryTaskStatuses_(
+        const std::vector<std::string>& task_ids,
+        std::vector<PoseTaskStatus>& statuses) const;
+    bool poseTargetReached_(
+        const PoseTaskContext& context,
+        std::string& detail) const;
+    std::string cachedControllerFaultDetail_(
+        std::uint64_t after_sequence = 0,
+        int wait_ms = 0,
+        std::uint64_t* associated_control_attempt = nullptr) const;
+    int controllerFaultCaptureGraceMs_() const;
+    int controllerFaultStateMaxAgeMs_() const;
+    std::string freeNavigationFaultStateUnavailableDetail_() const;
+    void rememberPoseTask_(const PoseTaskContext& context) const;
+    void advancePoseTaskGeneration_(
+        std::uint64_t navigation_generation,
+        std::uint64_t control_attempt_sequence) const;
+    void advancePoseTaskControlAttempt_(
+        std::uint64_t control_attempt_sequence) const;
+    void clearPoseTask_(std::uint64_t navigation_generation) const;
+    void clearPoseTaskIfTaskId_(const std::string& task_id) const;
+    bool currentPoseTask_(PoseTaskContext& context) const;
+    void rememberTrackedNavigation_(
+        const TrackedNavigationContext& context) const;
+    void advanceTrackedNavigationGeneration_(
+        std::uint64_t navigation_generation) const;
+    void clearTrackedNavigation_(std::uint64_t navigation_generation) const;
+    void clearTrackedNavigationIfToken_(const std::string& token) const;
+    bool currentTrackedNavigation_(
+        TrackedNavigationContext& context) const;
+    AgvResult sendControlledCommand_(int sock,
+                                     std::uint16_t command,
+                                     const Json::Value& payload,
+                                     Json::Value* response,
+                                     std::uint64_t* accepted_navigation_generation = nullptr,
+                                     std::uint64_t* controller_fault_sequence_at_attempt = nullptr,
+                                     std::uint64_t* control_attempt_sequence = nullptr,
+                                     PoseTaskContext* pose_context_to_publish = nullptr,
+                                     bool reject_if_active_controller_fault = false,
+                                     TrackedNavigationContext* navigation_context_to_publish = nullptr,
+                                     bool preserve_tracked_navigation = false,
+                                     const std::string* expected_navigation_token = nullptr,
+                                     const std::function<bool()>* cancellation_requested = nullptr,
+                                     const TrackedNavigationContext* expected_active_navigation = nullptr) const;
+    AgvResult sendCommand_(int sock,
+                           std::uint16_t command,
+                           const Json::Value& payload,
+                           Json::Value* response,
+                           CommandTransmissionState* transmission_state = nullptr) const;
+    AgvResult sendCommandRaw_(int sock,
+                              std::uint16_t command,
+                              const Json::Value& payload,
+                              std::string* response_payload,
+                              CommandTransmissionState* transmission_state = nullptr) const;
+    AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
+    AgvResult configurePush_();
+    void startPushThread_();
+    void stopPushThread_();
+    void pushLoop_();
+    void invalidateControllerFaultState_();
+    AgvRuntimeState queryRuntimeState_() const;
+    void updateCachedRuntimeState_(const Json::Value& payload);
+    void startMapUpdateThread_();
+    void stopMapUpdateThread_();
+    void mapUpdateLoop_();
+    AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
+    AgvResult parseMapFileToUpdates_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        std::vector<AgvUnifiedMapUpdate>& updates) const;
+    AgvResult parseSeerRobokitMapArchive_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        std::vector<AgvUnifiedMapUpdate>& updates) const;
+    AgvResult parseSeerRobokitMap2D_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const;
+    AgvResult parseSeerRobokitMap3D_(
+        const std::string& file_name,
+        const std::string& content,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const;
+    void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
+    bool findCachedMapUpdate_(
+        std::uint64_t after_sequence,
+        const AgvMapStreamOptions& options,
+        AgvUnifiedMapUpdate& update) const;
+    bool mapUpdateMatches_(
+        const AgvUnifiedMapUpdate& update,
+        const AgvMapStreamOptions& options) const;
+
+    static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
+    static std::string toJsonString_(const Json::Value& value);
+    static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
+    static std::string extractJson_(const std::string& raw);
+    static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
+    static void applyMotionOptions_(
+        Json::Value& payload,
+        const AgvMotionOptions& options,
+        bool include_reach_options = true);
+    static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
+    static AgvResult resultFromResponse_(const Json::Value& response);
+
+    config::SeerRobokitAgvConfig config_;
+    std::string ip_;
+    std::string control_nick_name_;
+    int recv_timeout_ms_{1000};
+    Ports ports_;
+    bool state_push_enabled_{false};
+    bool map_update_enabled_{false};
+    int map_update_interval_ms_{1000};
+    std::size_t map_update_history_size_{8};
+
+    mutable std::mutex mutex_;
+    mutable std::mutex status_io_mutex_;
+    mutable std::mutex control_sequence_mutex_;
+    mutable std::atomic<std::uint64_t> navigation_generation_{0};
+    mutable std::atomic<std::uint64_t> pose_task_sequence_{0};
+    mutable std::atomic<std::uint64_t> control_attempt_sequence_{0};
+    mutable std::atomic<std::uint64_t> controller_fault_channel_epoch_{0};
+    mutable std::mutex pose_task_mutex_;
+    mutable PoseTaskContext pose_task_context_;
+    mutable std::mutex tracked_navigation_mutex_;
+    mutable TrackedNavigationContext tracked_navigation_context_;
+    mutable int sock_status_{-1};
+    mutable int sock_control_{-1};
+    mutable int sock_navigation_{-1};
+    mutable int sock_config_{-1};
+    mutable int sock_other_{-1};
+    mutable int sock_push_{-1};
+    std::string last_error_;
+
+    std::atomic<bool> push_running_{false};
+    std::thread push_thread_;
+    mutable std::mutex runtime_state_mutex_;
+    mutable std::condition_variable runtime_state_cv_;
+    AgvRuntimeState cached_runtime_state_;
+    bool cached_runtime_state_valid_{false};
+    std::uint64_t controller_fault_sequence_{0};
+    bool controller_fault_state_observed_{false};
+    std::chrono::steady_clock::time_point controller_fault_state_observed_at_{};
+    std::string active_controller_fault_detail_;
+    double last_controller_fault_timestamp_{0.0};
+    std::string last_controller_fault_detail_;
+    std::uint64_t last_controller_fault_control_attempt_{0};
+
+    mutable std::atomic<bool> map_update_running_{false};
+    mutable std::thread map_update_thread_;
+    mutable std::mutex map_update_mutex_;
+    mutable std::condition_variable map_update_cv_;
+    mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
+    mutable std::uint64_t map_sequence_{0};
+    mutable int next_mapping_index_{0};
+    mutable std::size_t last_map_content_hash_{0};
+    mutable std::string map_session_id_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_SEER_ROBOKIT_AGV_H

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h

@@ -0,0 +1,257 @@
+#ifndef CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
+#define CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <string>
+#include <thread>
+
+#include "devices/agv/abstract_agv.h"
+
+namespace cmvr::device::seer_robokit::navigation {
+
+constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500);
+constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50);
+constexpr int kPoseNavigationRequiredRunningSamples = 2;
+constexpr auto kDefaultNavigationWaitTimeout =
+    std::chrono::milliseconds(600000);
+constexpr auto kDefaultNavigationPollInterval =
+    std::chrono::milliseconds(200);
+constexpr auto kMaximumNavigationPollInterval =
+    std::chrono::milliseconds(5000);
+constexpr auto kNavigationCancellationCheckInterval =
+    std::chrono::milliseconds(50);
+constexpr auto kNavigationCancelPollInterval =
+    std::chrono::milliseconds(100);
+constexpr auto kNavigationCancelConfirmationTimeout =
+    std::chrono::milliseconds(3000);
+constexpr int kRequiredBlockedStopSamples = 2;
+constexpr int kRequiredCompletedStopSamples = 2;
+constexpr double kNavigationStopVelocityTolerance = 0.005;
+constexpr int kMinimumControllerFaultCaptureGraceMs = 250;
+constexpr int kMaximumControllerFaultCaptureGraceMs = 5000;
+constexpr int kDefaultControllerFaultPushIntervalMs = 1000;
+constexpr int kControllerFaultPushJitterMs = 100;
+constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000;
+constexpr int kControllerFaultStateMaxAgeIntervals = 5;
+constexpr double kDefaultPoseReachDistance = 0.05;
+constexpr double kDefaultPoseReachAngle = 0.10;
+constexpr double kTwoPi = 6.28318530717958647692;
+
+static inline bool exactTaskStateIsActive(const int state)
+{
+    return state >= 1 && state <= 3;
+}
+
+static inline bool exactTaskStateIsKnownTerminal(const int state)
+{
+    return state >= 4 && state <= 7;
+}
+
+static inline bool globalTaskStateIsKnownTerminal(const int state)
+{
+    return state == 0 || exactTaskStateIsKnownTerminal(state);
+}
+
+static inline double angleDistance(const double lhs, const double rhs)
+{
+    return std::abs(std::remainder(lhs - rhs, kTwoPi));
+}
+
+static inline AgvResult withUnknownControllerOutcome(AgvResult result)
+{
+    const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code;
+    std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message;
+    detail +=
+        "; SEER Robokit controller outcome is unknown after the command attempt; "
+        "the command may already have taken effect; do not issue another motion "
+        "command automatically; query status and cancel or stop first";
+    return AgvResult::failure(code, detail);
+}
+
+static inline std::string makePoseTaskId(
+    const std::string& device_id,
+    const std::uint64_t task_sequence)
+{
+    const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
+    return prefix + "_pose_" + std::to_string(timestamp)
+        + "_" + std::to_string(task_sequence);
+}
+
+static inline std::string makeNavigationTaskId(
+    const std::string& device_id,
+    const char* kind,
+    const std::uint64_t task_sequence)
+{
+    const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
+    return prefix + "_" + kind + "_" + std::to_string(timestamp)
+        + "_" + std::to_string(task_sequence);
+}
+
+static inline const char* blockReasonName(const int reason)
+{
+    switch (reason) {
+        case 0:
+            return "ultrasonic";
+        case 1:
+            return "laser";
+        case 2:
+            return "fallingdown";
+        case 3:
+            return "collision";
+        case 4:
+            return "infrared";
+        case 5:
+            return "locked";
+        default:
+            return "unknown";
+    }
+}
+
+static inline std::string invalidMotionOption(const AgvMotionOptions& options)
+{
+    const auto non_negative_error = [](const double value, const char* field) {
+        if (!std::isfinite(value)) {
+            return std::string(field) + " must be finite";
+        }
+        if (value < 0.0) {
+            return std::string(field) + " must be non-negative";
+        }
+        return std::string{};
+    };
+
+    if (auto error = non_negative_error(options.max_speed, "max_speed");
+        !error.empty()) return error;
+    if (auto error = non_negative_error(
+            options.max_angular_speed,
+            "max_angular_speed");
+        !error.empty()) return error;
+    if (auto error = non_negative_error(
+            options.max_acceleration,
+            "max_acceleration");
+        !error.empty()) return error;
+    if (auto error = non_negative_error(
+            options.max_angular_acceleration,
+            "max_angular_acceleration");
+        !error.empty()) return error;
+    if (auto error = non_negative_error(
+            options.reach_distance,
+            "reach_distance");
+        !error.empty()) return error;
+    if (auto error = non_negative_error(options.reach_angle, "reach_angle");
+        !error.empty()) return error;
+    if (auto error = non_negative_error(options.speed_ratio, "speed_ratio");
+        !error.empty()) return error;
+    if (options.wait_timeout_ms < 0) {
+        return "wait_timeout_ms must be non-negative";
+    }
+    if (options.poll_interval_ms < 0) {
+        return "poll_interval_ms must be non-negative";
+    }
+    if (options.poll_interval_ms
+        > kMaximumNavigationPollInterval.count()) {
+        return "poll_interval_ms must not exceed "
+            + std::to_string(kMaximumNavigationPollInterval.count());
+    }
+    if (options.wait_timeout_ms > 0
+        && options.poll_interval_ms > options.wait_timeout_ms) {
+        return "poll_interval_ms must not exceed wait_timeout_ms";
+    }
+    return {};
+}
+
+static inline std::chrono::milliseconds navigationWaitTimeout(
+    const AgvMotionOptions& options)
+{
+    return options.wait_timeout_ms > 0
+        ? std::chrono::milliseconds(options.wait_timeout_ms)
+        : kDefaultNavigationWaitTimeout;
+}
+
+static inline std::chrono::milliseconds navigationPollInterval(
+    const AgvMotionOptions& options)
+{
+    if (options.poll_interval_ms <= 0) {
+        return kDefaultNavigationPollInterval;
+    }
+    return std::chrono::milliseconds(
+        std::max(options.poll_interval_ms, 20));
+}
+
+static inline bool navigationCancellationRequested(const AgvMotionOptions& options)
+{
+    return options.cancellation_requested
+        && options.cancellation_requested();
+}
+
+static inline void sleepForNavigationPoll(
+    const std::chrono::milliseconds poll_interval,
+    const std::chrono::steady_clock::time_point overall_deadline,
+    const AgvMotionOptions& options)
+{
+    const auto poll_deadline = std::min(
+        overall_deadline,
+        std::chrono::steady_clock::now() + poll_interval);
+    while (std::chrono::steady_clock::now() < poll_deadline
+           && !navigationCancellationRequested(options)) {
+        const auto remaining = std::chrono::duration_cast<std::chrono::milliseconds>(
+            poll_deadline - std::chrono::steady_clock::now());
+        if (remaining <= std::chrono::milliseconds::zero()) {
+            break;
+        }
+        std::this_thread::sleep_for(std::min(
+            kNavigationCancellationCheckInterval,
+            remaining));
+    }
+}
+
+template <typename Snapshot>
+static inline bool navigationStopped(const Snapshot& snapshot)
+{
+    return snapshot.velocity_present
+        && std::abs(snapshot.vx) <= kNavigationStopVelocityTolerance
+        && std::abs(snapshot.vy) <= kNavigationStopVelocityTolerance
+        && std::abs(snapshot.w) <= kNavigationStopVelocityTolerance;
+}
+
+static inline AgvResult reconciledNavigationResult(
+    const AgvResult& command_result,
+    AgvResult terminal_result)
+{
+    if (command_result.ok()) {
+        return terminal_result;
+    }
+    if (terminal_result.ok()) {
+        terminal_result.message =
+            "SEER Robokit navigation completed after an indeterminate command "
+            "acknowledgment; initial_detail=" + command_result.message;
+        return terminal_result;
+    }
+    terminal_result.message =
+        "SEER Robokit navigation command acknowledgment was indeterminate: "
+        + command_result.message + "; status reconciliation: "
+        + terminal_result.message;
+    return terminal_result;
+}
+
+static inline bool parseFiniteDouble(const std::string& value, double& parsed)
+{
+    std::size_t consumed = 0;
+    try {
+        parsed = std::stod(value, &consumed);
+    } catch (...) {
+        return false;
+    }
+    return consumed == value.size() && std::isfinite(parsed);
+}
+
+} // namespace cmvr::device::seer_robokit::navigation
+
+#endif // CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h

@@ -0,0 +1,141 @@
+#ifndef CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
+#define CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
+
+#include <string>
+
+#include <json/json.h>
+
+#include "devices/agv/abstract_agv.h"
+#include "seer_robokit_navigation_utils.h"
+#include "seer_robokit_utils.h"
+
+namespace cmvr::device::seer_robokit::pgv {
+
+constexpr char kUsePgv[] = "use_pgv";
+constexpr char kPgvAdjustDist[] = "pgv_adjust_dist";
+constexpr char kPgvAdjustCx[] = "pgv_adjust_cx";
+constexpr char kPgvAdjustCy[] = "pgv_adjust_cy";
+constexpr char kPgvXAdjust[] = "pgv_x_adjust";
+constexpr char kUseDownPgv[] = "use_down_pgv";
+
+// These spellings currently appear in the vendor document, but conflict with
+// its own field table/example and the repository's older working serializer.
+constexpr char kMalformedAdjustDist[] = "pgv_adjustuse_pgv_dist";
+constexpr char kAdjustCyDocumentAlias[] = "pgv_ajdust_cy";
+
+static inline bool isPgvAdjustmentKey(const std::string& key)
+{
+    return key == kUsePgv
+        || key == kPgvAdjustDist
+        || key == kPgvAdjustCx
+        || key == kPgvAdjustCy
+        || key == kPgvXAdjust
+        || key == kUseDownPgv
+        || key == kMalformedAdjustDist
+        || key == kAdjustCyDocumentAlias;
+}
+
+static inline bool hasPgvAdjustmentParams(const AgvAdapterParams& params)
+{
+    for (const auto& [key, value] : params.values) {
+        (void)value;
+        if (isPgvAdjustmentKey(key)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+/**
+ * Parse the string-valued generic adapter parameters into the native JSON
+ * types required by SEER Robokit API 3051. Returns an error string without
+ * modifying controller state; an empty string means success.
+ */
+static inline std::string applyPgvAdjustmentParams(
+    Json::Value& payload,
+    const AgvAdapterParams& params)
+{
+    if (params.getString(kMalformedAdjustDist)) {
+        return std::string(kMalformedAdjustDist)
+            + " is a vendor-document typo; use " + kPgvAdjustDist;
+    }
+    if (hasPgvAdjustmentParams(params)) {
+        for (const auto& [key, value] : params.values) {
+            (void)value;
+            if (!isPgvAdjustmentKey(key)
+                && key != "source_id"
+                && key != "task_id") {
+                return "PGV adjustment must not be combined with adapter "
+                    "field " + key;
+            }
+        }
+    }
+    const auto adjust_cy = params.getString(kPgvAdjustCy);
+    const auto adjust_cy_alias = params.getString(kAdjustCyDocumentAlias);
+    if (adjust_cy && adjust_cy_alias) {
+        return std::string(kPgvAdjustCy) + " and its vendor-document alias "
+            + kAdjustCyDocumentAlias + " must not both be set";
+    }
+
+    const auto apply_bool = [&payload, &params](const char* key) {
+        if (!params.getString(key)) {
+            return std::string{};
+        }
+        const auto parsed = params.getBool(key);
+        if (!parsed) {
+            return std::string(key)
+                + " must be a boolean string such as true or false";
+        }
+        detail::jsonMember(payload, key) = *parsed;
+        return std::string{};
+    };
+    if (auto error = apply_bool(kUsePgv); !error.empty()) {
+        return error;
+    }
+    if (auto error = apply_bool(kUseDownPgv); !error.empty()) {
+        return error;
+    }
+
+    const auto apply_number = [&payload, &params](
+                                  const char* key,
+                                  const bool non_negative) {
+        const auto raw = params.getString(key);
+        if (!raw) {
+            return std::string{};
+        }
+        double parsed = 0.0;
+        if (!navigation::parseFiniteDouble(*raw, parsed)) {
+            return std::string(key) + " must be a complete finite number";
+        }
+        if (non_negative && parsed < 0.0) {
+            return std::string(key) + " must be non-negative";
+        }
+        detail::jsonMember(payload, key) = parsed;
+        return std::string{};
+    };
+    if (auto error = apply_number(kPgvAdjustDist, true); !error.empty()) {
+        return error;
+    }
+    if (auto error = apply_number(kPgvAdjustCx, false); !error.empty()) {
+        return error;
+    }
+    if (adjust_cy_alias) {
+        double parsed = 0.0;
+        if (!navigation::parseFiniteDouble(*adjust_cy_alias, parsed)) {
+            return std::string(kAdjustCyDocumentAlias)
+                + " must be a complete finite number";
+        }
+        detail::jsonMember(payload, kPgvAdjustCy) = parsed;
+    } else if (auto error = apply_number(kPgvAdjustCy, false);
+               !error.empty()) {
+        return error;
+    }
+    if (auto error = apply_number(kPgvXAdjust, false); !error.empty()) {
+        return error;
+    }
+    return {};
+}
+
+} // namespace cmvr::device::seer_robokit::pgv
+
+#endif // CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h

@@ -0,0 +1,39 @@
+#ifndef CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
+#define CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
+
+#include <cstdint>
+
+namespace cmvr::device::seer_robokit::protocol {
+
+constexpr std::uint16_t kRobotStatusLoc = 1004;
+constexpr std::uint16_t kRobotStatusBattery = 1007;
+constexpr std::uint16_t kRobotStatusAll2 = 1101;
+constexpr std::uint16_t kRobotStatusTask = 1020;
+constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
+constexpr std::uint16_t kRobotStatusMap = 1300;
+constexpr std::uint16_t kRobotStatusStation = 1301;
+constexpr std::uint16_t kRobotStatusMappingFileList = 1780;
+constexpr std::uint16_t kRobotStatusDownloadFile = 1800;
+constexpr std::uint16_t kRobotControlStop = 2000;
+constexpr std::uint16_t kRobotControlMotion = 2010;
+constexpr std::uint16_t kRobotControlLoadMap = 2022;
+constexpr std::uint16_t kRobotTaskPause = 3001;
+constexpr std::uint16_t kRobotTaskResume = 3002;
+constexpr std::uint16_t kRobotTaskCancel = 3003;
+constexpr std::uint16_t kRobotTaskGoTarget = 3051;
+constexpr std::uint16_t kRobotTaskTranslate = 3055;
+constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
+constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
+constexpr std::uint16_t kRobotConfigLock = 4005;
+constexpr std::uint16_t kRobotConfigUploadMap = 4010;
+constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
+constexpr std::uint16_t kRobotOtherStartMapping = 6100;
+constexpr std::uint16_t kRobotOtherStopMapping = 6101;
+constexpr std::uint16_t kRobotPushConfigReq = 9300;
+constexpr std::uint16_t kRobotPushConfigRes = 19300;
+constexpr std::uint16_t kRobotPush = 19301;
+constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U;
+
+} // namespace cmvr::device::seer_robokit::protocol
+
+#endif // CMVR_ES_SEER_ROBOKIT_PROTOCOL_H

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h

@@ -0,0 +1,92 @@
+#ifndef CMVR_ES_SEER_ROBOKIT_UTILS_H
+#define CMVR_ES_SEER_ROBOKIT_UTILS_H
+
+#include <cerrno>
+#include <chrono>
+#include <cstring>
+#include <string>
+
+#include <json/json.h>
+
+namespace cmvr::device::seer_robokit::detail {
+
+static inline std::string systemError()
+{
+    return std::strerror(errno);
+}
+
+static inline Json::Value& jsonMember(
+    Json::Value& value,
+    const char* key)
+{
+    return *value.demand(key, key + std::strlen(key));
+}
+
+static inline Json::Value& jsonMember(
+    Json::Value& value,
+    const std::string& key)
+{
+    return *value.demand(key.data(), key.data() + key.size());
+}
+
+static inline const Json::Value* jsonFind(
+    const Json::Value& value,
+    const char* key)
+{
+    return value.find(key, key + std::strlen(key));
+}
+
+static inline Json::Value jsonGet(
+    const Json::Value& value,
+    const char* key,
+    const Json::Value& fallback)
+{
+    const auto* found = jsonFind(value, key);
+    return found ? *found : fallback;
+}
+
+static inline double nowSeconds()
+{
+    const auto now = std::chrono::system_clock::now().time_since_epoch();
+    return std::chrono::duration<double>(now).count();
+}
+
+static inline bool jsonHas(
+    const Json::Value& value,
+    const char* key)
+{
+    return jsonFind(value, key) != nullptr;
+}
+
+static inline bool hasNumericControllerRetCode(
+    const Json::Value& response)
+{
+    const auto* ret_code = jsonFind(response, "ret_code");
+    return ret_code
+        && (ret_code->isInt()
+            || ret_code->isUInt()
+            || ret_code->isInt64()
+            || ret_code->isUInt64());
+}
+
+static inline std::string jsonValueToString(const Json::Value& value)
+{
+    if (value.isString()) return value.asString();
+    if (value.isBool()) return value.asBool() ? "true" : "false";
+    if (value.isInt64() || value.isInt()) {
+        return std::to_string(value.asInt64());
+    }
+    if (value.isUInt64() || value.isUInt()) {
+        return std::to_string(value.asUInt64());
+    }
+    if (value.isDouble()) return std::to_string(value.asDouble());
+    if (value.isNull()) return {};
+
+    Json::StreamWriterBuilder builder;
+    builder["indentation"] = "";
+    return Json::writeString(builder, value);
+}
+
+} // namespace cmvr::device::seer_robokit::detail
+
+#endif // CMVR_ES_SEER_ROBOKIT_UTILS_H

cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp

@@ -0,0 +1,175 @@
+#include "seer_robokit_agv.h"
+
+#include <atomic>
+#include <cstddef>
+#include <mutex>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+
+namespace {
+
+constexpr int kDefaultMapUpdateIntervalMs = 1000;
+constexpr std::size_t kDefaultMapUpdateHistorySize = 8;
+
+} // namespace
+
+SeerRobokitAgv::SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg)
+    : config_(cfg),
+      ip_(cfg.ip()),
+      control_nick_name_(
+          cfg.control_nick_name().empty()
+              ? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id())
+              : cfg.control_nick_name()),
+      recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000),
+      state_push_enabled_(cfg.enable_state_push()),
+      map_update_enabled_(cfg.enable_map_update()),
+      map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs),
+      map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize)
+{
+    id_ = cfg.id();
+    if (cfg.port_status() > 0) ports_.status = cfg.port_status();
+    if (cfg.port_control() > 0) ports_.control = cfg.port_control();
+    if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav();
+    if (cfg.port_config() > 0) ports_.config = cfg.port_config();
+    if (cfg.port_other() > 0) ports_.other = cfg.port_other();
+    if (cfg.port_push() > 0) ports_.push = cfg.port_push();
+
+    const auto result = connect_();
+    if (!result.ok()) {
+        CMVR_LOG(ERROR) << "[SeerRobokitAgv] Auto connect failed"
+                        << ", id=" << id_
+                        << ", ip=" << ip_
+                        << ", error=" << result.message;
+    }
+}
+
+SeerRobokitAgv::~SeerRobokitAgv()
+{
+    (void)disconnect_();
+}
+
+bool SeerRobokitAgv::init()
+{
+    return !id_.empty() && !ip_.empty();
+}
+
+bool SeerRobokitAgv::start()
+{
+    return true;
+}
+
+bool SeerRobokitAgv::stop()
+{
+    return true;
+}
+
+bool SeerRobokitAgv::update()
+{
+    return true;
+}
+
+AgvResult SeerRobokitAgv::connect_()
+{
+    const auto lifecycle_generation =
+        navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
+    clearPoseTask_(lifecycle_generation);
+    clearTrackedNavigation_(lifecycle_generation);
+    stopPushThread_();
+    stopMapUpdateThread_();
+
+    {
+        // Status requests may wait for a controller receive timeout without
+        // holding mutex_. Serialize lifecycle changes with that channel before
+        // replacing or closing its descriptor.
+        std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
+        std::lock_guard<std::mutex> lock(mutex_);
+        closeSocket_(sock_status_);
+        closeSocket_(sock_control_);
+        closeSocket_(sock_navigation_);
+        closeSocket_(sock_config_);
+        closeSocket_(sock_other_);
+        closeSocket_(sock_push_);
+
+        if (ip_.empty()) {
+            return AgvResult::failure(AgvErrorCode::InvalidArgument, "SEER Robokit AGV ip is empty");
+        }
+
+        const auto close_all = [this]() {
+            closeSocket_(sock_status_);
+            closeSocket_(sock_control_);
+            closeSocket_(sock_navigation_);
+            closeSocket_(sock_config_);
+            closeSocket_(sock_other_);
+            closeSocket_(sock_push_);
+        };
+
+        if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) {
+            close_all();
+            return result;
+        }
+        if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) {
+            close_all();
+            return result;
+        }
+        if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) {
+            close_all();
+            return result;
+        }
+        if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) {
+            close_all();
+            return result;
+        }
+
+        if (state_push_enabled_) {
+            const auto result = connectSocket_(sock_push_, ports_.push);
+            if (!result.ok()) {
+                CMVR_LOG(ERROR) << "[SeerRobokitAgv] Connect push port failed"
+                                << ", id=" << id_
+                                << ", port=" << ports_.push
+                                << ", error=" << result.message;
+                closeSocket_(sock_push_);
+            }
+        }
+        last_error_.clear();
+    }
+
+    if (state_push_enabled_ && sock_push_ >= 0) {
+        const auto result = configurePush_();
+        if (result.ok()) {
+            startPushThread_();
+        } else {
+            CMVR_LOG(ERROR) << "[SeerRobokitAgv] Configure push failed"
+                            << ", id=" << id_
+                            << ", error=" << result.message;
+            std::lock_guard<std::mutex> lock(mutex_);
+            closeSocket_(sock_push_);
+        }
+    }
+    if (map_update_enabled_) {
+        startMapUpdateThread_();
+    }
+    return AgvResult::success();
+}
+
+AgvResult SeerRobokitAgv::disconnect_()
+{
+    const auto lifecycle_generation =
+        navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
+    clearPoseTask_(lifecycle_generation);
+    clearTrackedNavigation_(lifecycle_generation);
+    stopMapUpdateThread_();
+    stopPushThread_();
+    std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
+    std::lock_guard<std::mutex> lock(mutex_);
+    closeSocket_(sock_status_);
+    closeSocket_(sock_control_);
+    closeSocket_(sock_navigation_);
+    closeSocket_(sock_config_);
+    closeSocket_(sock_other_);
+    closeSocket_(sock_push_);
+    return AgvResult::success();
+}
+
+} // namespace cmvr::device

cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp

@@ -0,0 +1,378 @@
+#include "seer_robokit_agv.h"
+#include "seer_robokit_navigation_utils.h"
+#include "seer_robokit_protocol.h"
+#include "seer_robokit_utils.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cstdint>
+#include <functional>
+#include <mutex>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace cmvr::device {
+
+using namespace seer_robokit::navigation;
+using namespace seer_robokit::protocol;
+using namespace seer_robokit::detail;
+
+AgvResult SeerRobokitAgv::acquireControl_() const
+{
+    Json::Value payload(Json::objectValue);
+    jsonMember(payload, "nick_name") = control_nick_name_;
+
+    Json::Value response;
+    auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response);
+    return result.ok() ? resultFromResponse_(response) : result;
+}
+
+AgvResult SeerRobokitAgv::sendControlledCommand_(
+    const int sock,
+    const std::uint16_t command,
+    const Json::Value& payload,
+    Json::Value* response,
+    std::uint64_t* accepted_navigation_generation,
+    std::uint64_t* controller_fault_sequence_at_attempt,
+    std::uint64_t* control_attempt_sequence,
+    PoseTaskContext* pose_context_to_publish,
+    const bool reject_if_active_controller_fault,
+    TrackedNavigationContext* navigation_context_to_publish,
+    const bool preserve_tracked_navigation,
+    const std::string* expected_navigation_token,
+    const std::function<bool()>* cancellation_requested,
+    const TrackedNavigationContext* expected_active_navigation) const
+{
+    const auto canceled_before_send = [cancellation_requested]() {
+        return cancellation_requested
+            && *cancellation_requested
+            && (*cancellation_requested)();
+    };
+    if (canceled_before_send()) {
+        return AgvResult::failure(
+            AgvErrorCode::TaskCanceled,
+            "SEER Robokit command was not sent because the caller canceled the "
+            "operation before control authority was acquired");
+    }
+
+    const auto expected_context_is_current =
+        [this, expected_active_navigation]() {
+            if (!expected_active_navigation) {
+                return true;
+            }
+            TrackedNavigationContext active_context;
+            return currentTrackedNavigation_(active_context)
+                && active_context.token
+                    == expected_active_navigation->token
+                && active_context.navigation_generation
+                    == expected_active_navigation->navigation_generation
+                && active_context.type
+                    == expected_active_navigation->type
+                && navigation_generation_.load(std::memory_order_relaxed)
+                    == expected_active_navigation->navigation_generation;
+        };
+
+    // Conditional cancel ownership checks are deliberately performed without
+    // the control sequencing mutex. A slow 1110/1101 response must never
+    // prevent emergencyStop() from acquiring authority and sending 2000.
+    // The exact local token/generation/type is revalidated under the control
+    // lock both before and after authority acquisition below.
+    if (expected_active_navigation) {
+        if (!expected_context_is_current()) {
+            return AgvResult::failure(
+                AgvErrorCode::TaskCanceled,
+                "SEER Robokit did not start conditional navigation cancel "
+                "preflight because the tracked task was already replaced or "
+                "ended");
+        }
+
+        std::vector<PoseTaskStatus> statuses;
+        const auto exact_result = queryTaskStatuses_(
+            expected_active_navigation->task_ids,
+            statuses);
+        if (!exact_result.ok()) {
+            return AgvResult::failure(
+                exact_result.code,
+                "SEER Robokit did not send the conditional navigation cancel "
+                "because exact task ownership preflight failed: "
+                    + exact_result.message);
+        }
+        const bool all_exact_tasks_terminal = !statuses.empty()
+            && std::all_of(
+                statuses.begin(),
+                statuses.end(),
+                [](const PoseTaskStatus& status) {
+                    return status.found
+                        && exactTaskStateIsKnownTerminal(status.state);
+                });
+        if (all_exact_tasks_terminal) {
+            return AgvResult::success();
+        }
+
+        const bool exact_task_still_active = std::any_of(
+            statuses.begin(),
+            statuses.end(),
+            [](const PoseTaskStatus& status) {
+                return status.found
+                    && exactTaskStateIsActive(status.state);
+            });
+
+        NavigationSnapshot snapshot;
+        const auto snapshot_result = queryNavigationSnapshot_(snapshot);
+        if (!snapshot_result.ok()) {
+            return AgvResult::failure(
+                snapshot_result.code,
+                "SEER Robokit did not send the conditional navigation cancel "
+                "because 1101 ownership preflight was unavailable: "
+                    + snapshot_result.message);
+        }
+        const int expected_type = expected_active_navigation->type
+                == AgvTaskType::NavigateToPose
+            ? 1
+            : (expected_active_navigation->type
+                    == AgvTaskType::NavigateToStation
+                ? 2
+                : 3);
+        const bool global_active =
+            exactTaskStateIsActive(snapshot.task_status);
+        const bool target_conflicts = global_active
+            && !snapshot.target_id.empty()
+            && !expected_active_navigation->target_ids.empty()
+            && std::find(
+                   expected_active_navigation->target_ids.begin(),
+                   expected_active_navigation->target_ids.end(),
+                   snapshot.target_id)
+                == expected_active_navigation->target_ids.end();
+        if (global_active
+            && (snapshot.task_type != expected_type
+                || target_conflicts)) {
+            return AgvResult::failure(
+                AgvErrorCode::TaskCanceled,
+                "SEER Robokit did not send the conditional navigation cancel "
+                "because 1101 reports another active task: "
+                    + snapshot.detail);
+        }
+
+        if (!exact_task_still_active) {
+            const bool clearing_path_queue =
+                expected_active_navigation->type
+                == AgvTaskType::FollowPath;
+            const bool terminal_target_matches =
+                expected_active_navigation->type
+                    != AgvTaskType::NavigateToStation
+                || (!snapshot.target_id.empty()
+                    && snapshot.target_id
+                        == expected_active_navigation->target_id);
+            if (globalTaskStateIsKnownTerminal(snapshot.task_status)
+                && snapshot.task_status != 0
+                && !clearing_path_queue
+                && snapshot.task_type == expected_type
+                && terminal_target_matches) {
+                return AgvResult::success();
+            }
+        }
+    }
+
+    // Keep the permission acquisition and the following write ordered with
+    // respect to other control RPCs in this process. Channel I/O serialization
+    // is separate, so this must remain a distinct lock.
+    std::lock_guard<std::mutex> sequence_lock(control_sequence_mutex_);
+    if (expected_navigation_token) {
+        TrackedNavigationContext active_context;
+        const bool has_active_context =
+            currentTrackedNavigation_(active_context);
+        const bool expected_context_matches = expected_active_navigation
+            ? (has_active_context
+                && active_context.token
+                    == expected_active_navigation->token
+                && active_context.navigation_generation
+                    == expected_active_navigation->navigation_generation
+                && active_context.type
+                    == expected_active_navigation->type
+                && navigation_generation_.load(std::memory_order_relaxed)
+                    == expected_active_navigation->navigation_generation)
+            : (has_active_context
+                && active_context.token == *expected_navigation_token);
+        if (!expected_context_matches) {
+            return AgvResult::failure(
+                AgvErrorCode::TaskCanceled,
+                "SEER Robokit did not send the conditional navigation cancel "
+                "because the tracked task was already replaced or ended; "
+                "expected_token=" + *expected_navigation_token
+                    + (active_context.token.empty()
+                           ? std::string(", active_token=<none>")
+                           : ", active_token=" + active_context.token));
+        }
+    }
+    const auto attempt_sequence =
+        control_attempt_sequence_.fetch_add(
+            1,
+            std::memory_order_relaxed) + 1;
+    if (control_attempt_sequence) {
+        *control_attempt_sequence = attempt_sequence;
+    }
+    if (pose_context_to_publish) {
+        pose_context_to_publish->control_attempt_sequence_at_start =
+            attempt_sequence;
+    }
+
+    const auto authority = acquireControl_();
+    if (!authority.ok()) {
+        const std::string detail = authority.message.empty() ? "unknown error" : authority.message;
+        return AgvResult::failure(
+            authority.code,
+            "SEER Robokit acquire control authority failed: " + detail);
+    }
+    if (expected_active_navigation && !expected_context_is_current()) {
+        return AgvResult::failure(
+            AgvErrorCode::TaskCanceled,
+            "SEER Robokit did not send the conditional navigation cancel because "
+            "the tracked token, generation, or type changed while control "
+            "authority was being acquired");
+    }
+    if (canceled_before_send()) {
+        return AgvResult::failure(
+            AgvErrorCode::TaskCanceled,
+            "SEER Robokit command was not sent because the caller canceled the "
+            "operation while control authority was being acquired");
+    }
+    std::string controller_fault_gate_error;
+    if (controller_fault_sequence_at_attempt
+        || pose_context_to_publish
+        || reject_if_active_controller_fault) {
+        std::lock_guard<std::mutex> lock(runtime_state_mutex_);
+        if (controller_fault_sequence_at_attempt) {
+            *controller_fault_sequence_at_attempt =
+                controller_fault_sequence_;
+        }
+        if (pose_context_to_publish) {
+            pose_context_to_publish->controller_fault_sequence_at_start =
+                controller_fault_sequence_;
+            pose_context_to_publish
+                ->controller_fault_channel_epoch_at_start =
+                controller_fault_channel_epoch_.load(
+                    std::memory_order_relaxed);
+        }
+        if (reject_if_active_controller_fault) {
+            if (!state_push_enabled_) {
+                controller_fault_gate_error =
+                    "controller fault state is unavailable because state push "
+                    "is disabled";
+            } else if (!active_controller_fault_detail_.empty()) {
+                controller_fault_gate_error =
+                    "the controller reported a fault or invalid fault state: "
+                    + active_controller_fault_detail_;
+            } else if (!controller_fault_state_observed_) {
+                controller_fault_gate_error =
+                    "no state push containing fatals/errors has been observed";
+            } else {
+                const auto fault_state_age =
+                    std::chrono::duration_cast<std::chrono::milliseconds>(
+                        std::chrono::steady_clock::now()
+                            - controller_fault_state_observed_at_)
+                        .count();
+                if (fault_state_age > controllerFaultStateMaxAgeMs_()) {
+                    controller_fault_gate_error =
+                        "the most recent fatals/errors state push is stale "
+                        "(age_ms=" + std::to_string(fault_state_age)
+                        + ", max_age_ms="
+                        + std::to_string(controllerFaultStateMaxAgeMs_())
+                        + ")";
+                }
+            }
+        }
+    }
+    if (!controller_fault_gate_error.empty()) {
+        return AgvResult::failure(
+            AgvErrorCode::Fault,
+            "SEER Robokit free-navigation command was not sent because "
+                + controller_fault_gate_error);
+    }
+    if (canceled_before_send()) {
+        return AgvResult::failure(
+            AgvErrorCode::TaskCanceled,
+            "SEER Robokit command was not sent because the caller canceled the "
+            "operation before the controller command write");
+    }
+    if (canceled_before_send()) {
+        return AgvResult::failure(
+            AgvErrorCode::TaskCanceled,
+            "SEER Robokit command was not sent because the caller canceled the "
+            "operation immediately before the controller command write");
+    }
+    const auto publish_navigation_generation =
+        [this,
+         accepted_navigation_generation,
+         pose_context_to_publish,
+         navigation_context_to_publish,
+         preserve_tracked_navigation]() {
+            const auto generation =
+                navigation_generation_.fetch_add(
+                    1,
+                    std::memory_order_relaxed) + 1;
+            *accepted_navigation_generation = generation;
+            if (pose_context_to_publish) {
+                pose_context_to_publish->navigation_generation = generation;
+                rememberPoseTask_(*pose_context_to_publish);
+            }
+            if (navigation_context_to_publish) {
+                navigation_context_to_publish->navigation_generation =
+                    generation;
+                navigation_context_to_publish->accepted_at =
+                    std::chrono::steady_clock::now();
+                rememberTrackedNavigation_(
+                    *navigation_context_to_publish);
+            } else if (preserve_tracked_navigation) {
+                advanceTrackedNavigationGeneration_(generation);
+            } else {
+                clearTrackedNavigation_(generation);
+            }
+        };
+    CommandTransmissionState transmission_state =
+        CommandTransmissionState::NotSent;
+    auto result = sendCommand_(
+        sock,
+        command,
+        payload,
+        response,
+        &transmission_state);
+    if (!result.ok()) {
+        if (accepted_navigation_generation
+            && transmission_state
+                == CommandTransmissionState::PossiblySent) {
+            // Once the control write has been attempted, a timeout, disconnect,
+            // wrong response opcode, or malformed JSON cannot prove rejection:
+            // the controller may already have executed the command.
+            publish_navigation_generation();
+            return withUnknownControllerOutcome(std::move(result));
+        }
+        return result;
+    }
+    if (!accepted_navigation_generation) {
+        return result;
+    }
+    if (!response) {
+        publish_navigation_generation();
+        return withUnknownControllerOutcome(AgvResult::failure(
+            AgvErrorCode::CommandFailed,
+            "SEER Robokit cannot confirm navigation command without a response"));
+    }
+    if (!hasNumericControllerRetCode(*response)) {
+        publish_navigation_generation();
+        return withUnknownControllerOutcome(resultFromResponse_(*response));
+    }
+    result = resultFromResponse_(*response);
+    if (!result.ok()) {
+        return result;
+    }
+
+    // Advance only after the controller accepted the command, and do it before
+    // releasing control_sequence_mutex_. This prevents a failed cancel/pause or
+    // failed authority acquisition from falsely reporting a pose task canceled,
+    // while preserving the controller's actual command order under concurrency.
+    publish_navigation_generation();
+    return result;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp

@@ -0,0 +1,725 @@
+#include "seer_robokit_agv.h"
+#include "seer_robokit_protocol.h"
+#include "seer_robokit_utils.h"
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <mutex>
+#include <sstream>
+#include <string>
+#include <sys/socket.h>
+#include <thread>
+
+#include <google/protobuf/repeated_ptr_field.h>
+
+namespace cmvr::device {
+
+using namespace seer_robokit::protocol;
+using namespace seer_robokit::detail;
+
+namespace {
+
+bool hasFaultArray(const Json::Value& value, const char* key)
+{
+    const auto* found = jsonFind(value, key);
+    return found && found->isArray() && !found->empty();
+}
+
+void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField<std::string>& strings)
+{
+    if (strings.empty()) {
+        return;
+    }
+    Json::Value array(Json::arrayValue);
+    for (const auto& item : strings) {
+        array.append(item);
+    }
+    jsonMember(value, key) = array;
+}
+
+AgvMode modeFromTaskState(const int state)
+{
+    switch (state) {
+        case 2:
+            return AgvMode::Auto;
+        case 3:
+            return AgvMode::Paused;
+        case 5:
+            return AgvMode::Fault;
+        case 6:
+            return AgvMode::Stopped;
+        default:
+            return AgvMode::Idle;
+    }
+}
+
+AgvTaskState toTaskState(const int value)
+{
+    switch (value) {
+        case 1:
+            return AgvTaskState::Waiting;
+        case 2:
+            return AgvTaskState::Running;
+        case 3:
+            return AgvTaskState::Paused;
+        case 4:
+            return AgvTaskState::Completed;
+        case 5:
+        case 7:
+            return AgvTaskState::Failed;
+        case 6:
+            return AgvTaskState::Canceled;
+        case 0:
+        default:
+            return AgvTaskState::None;
+    }
+}
+
+AgvTaskType toTaskType(const int value)
+{
+    switch (value) {
+        case 1:
+            return AgvTaskType::NavigateToPose;
+        case 2:
+            return AgvTaskType::NavigateToStation;
+        case 3:
+            return AgvTaskType::FollowPath;
+        case 100:
+            return AgvTaskType::Custom;
+        default:
+            return AgvTaskType::None;
+    }
+}
+
+} // namespace
+
+AgvRuntimeState SeerRobokitAgv::runtimeState() const
+{
+    AgvRuntimeState cached_state;
+    bool has_cached_state = false;
+    if (state_push_enabled_) {
+        std::lock_guard<std::mutex> lock(runtime_state_mutex_);
+        if (cached_runtime_state_valid_) {
+            cached_state = cached_runtime_state_;
+            has_cached_state = true;
+        }
+    }
+
+    if (has_cached_state) {
+        std::string adapter_error;
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            cached_state.connected = connected_();
+            adapter_error = last_error_;
+        }
+        if (!adapter_error.empty()) {
+            if (cached_state.last_error.empty()) {
+                cached_state.last_error = adapter_error;
+            } else if (cached_state.last_error != adapter_error) {
+                cached_state.last_error += "; adapter_error=" + adapter_error;
+            }
+        }
+        if (!cached_state.connected) {
+            cached_state.mode = AgvMode::Disconnected;
+        }
+        return cached_state;
+    }
+
+    return queryRuntimeState_();
+}
+
+AgvRuntimeState SeerRobokitAgv::queryRuntimeState_() const
+{
+    AgvRuntimeState state;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        state.connected = connected_();
+        state.last_error = last_error_;
+    }
+    state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected;
+
+    Json::Value loc;
+    if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) {
+        state.pose.x = jsonGet(loc, "x", 0.0).asDouble();
+        state.pose.y = jsonGet(loc, "y", 0.0).asDouble();
+        state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble();
+        state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0;
+        state.current_station = jsonGet(loc, "current_station", "").asString();
+    }
+
+    Json::Value battery;
+    if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) {
+        state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble();
+        state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble();
+        state.battery.charging = jsonGet(battery, "charging", false).asBool();
+        state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble();
+        state.battery.current = jsonGet(battery, "current", 0.0).asDouble();
+    }
+
+    Json::Value map;
+    if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) {
+        state.current_map = jsonGet(map, "current_map", "").asString();
+    }
+
+    const auto nav = navigationStatus();
+    state.moving = nav.state == AgvTaskState::Running;
+    state.fault = nav.state == AgvTaskState::Failed;
+    state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast<int>(nav.state));
+    return state;
+}
+
+AgvNavigationStatus SeerRobokitAgv::navigationStatus() const
+{
+    AgvNavigationStatus status;
+    std::string missing_pose_task_detail;
+    for (int attempt = 0; attempt < 2; ++attempt) {
+        PoseTaskContext pose_context;
+        if (!currentPoseTask_(pose_context)) {
+            break;
+        }
+        const auto observed_navigation_generation =
+            navigation_generation_.load(std::memory_order_relaxed);
+        if (pose_context.navigation_generation
+            != observed_navigation_generation) {
+            continue;
+        }
+
+        PoseTaskStatus task_status;
+        const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status);
+        PoseTaskContext latest_context;
+        if (navigation_generation_.load(std::memory_order_relaxed)
+                != observed_navigation_generation
+            || !currentPoseTask_(latest_context)
+            || latest_context.navigation_generation
+                != pose_context.navigation_generation
+            || latest_context.task_id != pose_context.task_id) {
+            continue;
+        }
+
+        status.type = AgvTaskType::NavigateToPose;
+        const auto fault_monitoring_unavailable =
+            [this, &pose_context]() {
+                if (controller_fault_channel_epoch_.load(
+                        std::memory_order_relaxed)
+                    != pose_context
+                        .controller_fault_channel_epoch_at_start) {
+                    return std::string(
+                        "the controller fault push channel changed or was "
+                        "invalidated after the free-navigation command was "
+                        "accepted");
+                }
+                return freeNavigationFaultStateUnavailableDetail_();
+            };
+        if (!result.ok()) {
+            status.state = AgvTaskState::Failed;
+            status.message = result.message;
+            return status;
+        }
+        if (!task_status.found || task_status.state == 404) {
+            std::uint64_t missing_task_fault_control_attempt = 0;
+            const std::string missing_task_fault =
+                cachedControllerFaultDetail_(
+                    pose_context.controller_fault_sequence_at_start,
+                    controllerFaultCaptureGraceMs_(),
+                    &missing_task_fault_control_attempt);
+            PoseTaskContext post_missing_context;
+            if (navigation_generation_.load(std::memory_order_relaxed)
+                    != observed_navigation_generation
+                || !currentPoseTask_(post_missing_context)
+                || post_missing_context.navigation_generation
+                    != pose_context.navigation_generation
+                || post_missing_context.task_id != pose_context.task_id) {
+                continue;
+            }
+            if (!missing_task_fault.empty()) {
+                std::string attribution;
+                if (missing_task_fault_control_attempt != 0
+                    && missing_task_fault_control_attempt
+                        != pose_context.control_attempt_sequence_at_start) {
+                    attribution =
+                        "controller_fault_attribution=ambiguous because the "
+                        "fault was observed after another control command "
+                        "attempt had begun, ";
+                }
+                status.state = AgvTaskState::Failed;
+                status.message =
+                    "SEER Robokit tracked free-navigation task disappeared from "
+                    "1110 task_status_package while a new controller fault "
+                    "was observed: " + task_status.detail + ", "
+                    + attribution + missing_task_fault;
+                clearPoseTask_(pose_context.navigation_generation);
+                return status;
+            }
+            if (const std::string unavailable =
+                    fault_monitoring_unavailable();
+                !unavailable.empty()) {
+                status.state = AgvTaskState::Failed;
+                status.message =
+                    "SEER Robokit tracked free-navigation status is unsafe to "
+                    "accept because controller fault monitoring is "
+                    "unavailable: " + unavailable
+                    + "; query the controller and cancel or stop before "
+                      "another motion command";
+                return status;
+            }
+            missing_pose_task_detail = task_status.detail;
+            clearPoseTask_(pose_context.navigation_generation);
+            break;
+        }
+        if (task_status.type_present && task_status.type != 1) {
+            status.state = AgvTaskState::Failed;
+            status.type = toTaskType(task_status.type);
+            status.message =
+                "SEER Robokit returned an unexpected task type for the tracked "
+                "free-navigation task: " + task_status.detail;
+            clearPoseTask_(pose_context.navigation_generation);
+            return status;
+        }
+        status.state = toTaskState(task_status.state);
+        status.progress = task_status.progress;
+        status.message = task_status.detail;
+        const auto controller_reported_state = status.state;
+        const bool controller_state_terminal =
+            controller_reported_state == AgvTaskState::Completed
+            || controller_reported_state == AgvTaskState::Failed
+            || controller_reported_state == AgvTaskState::Canceled;
+        std::uint64_t fault_control_attempt = 0;
+        const std::string fault = cachedControllerFaultDetail_(
+            pose_context.controller_fault_sequence_at_start,
+            controller_reported_state == AgvTaskState::Completed
+                || controller_reported_state == AgvTaskState::Failed
+                ? controllerFaultCaptureGraceMs_()
+                : 0,
+            &fault_control_attempt);
+        PoseTaskContext post_fault_context;
+        if (navigation_generation_.load(std::memory_order_relaxed)
+                != observed_navigation_generation
+            || !currentPoseTask_(post_fault_context)
+            || post_fault_context.navigation_generation
+                != pose_context.navigation_generation
+            || post_fault_context.task_id != pose_context.task_id) {
+            continue;
+        }
+        const std::string unavailable =
+            fault_monitoring_unavailable();
+        if (!fault.empty()) {
+            std::string attribution;
+            if (fault_control_attempt != 0
+                && fault_control_attempt
+                    != pose_context.control_attempt_sequence_at_start) {
+                attribution =
+                    "controller_fault_attribution=ambiguous because the "
+                    "fault was observed after another control command "
+                    "attempt had begun, ";
+            }
+            status.state = AgvTaskState::Failed;
+            status.message =
+                "SEER Robokit reported a new controller fault while the tracked "
+                "free-navigation task had controller_task_state="
+                + std::to_string(task_status.state) + ": "
+                + task_status.detail + ", " + attribution + fault;
+            if (!unavailable.empty()) {
+                status.message +=
+                    ", controller_fault_monitoring_unavailable="
+                    + unavailable;
+            }
+        } else if (!unavailable.empty()) {
+            if (controller_reported_state == AgvTaskState::Failed
+                || controller_reported_state == AgvTaskState::Canceled) {
+                status.message +=
+                    ", controller_fault_monitoring_unavailable="
+                    + unavailable;
+            } else {
+                status.state = AgvTaskState::Failed;
+                status.message =
+                    "SEER Robokit tracked free-navigation state is unsafe to accept "
+                    "because controller fault monitoring became unavailable: "
+                    + unavailable
+                    + "; query the controller and cancel or stop before "
+                      "another motion command";
+                return status;
+            }
+        }
+        if (status.state == AgvTaskState::Completed) {
+            std::string pose_detail;
+            const bool target_reached =
+                poseTargetReached_(pose_context, pose_detail);
+            PoseTaskContext post_pose_context;
+            if (navigation_generation_.load(std::memory_order_relaxed)
+                    != observed_navigation_generation
+                || !currentPoseTask_(post_pose_context)
+                || post_pose_context.navigation_generation
+                    != pose_context.navigation_generation
+                || post_pose_context.task_id != pose_context.task_id) {
+                continue;
+            }
+            std::uint64_t post_pose_fault_control_attempt = 0;
+            const std::string post_pose_fault =
+                cachedControllerFaultDetail_(
+                    pose_context.controller_fault_sequence_at_start,
+                    0,
+                    &post_pose_fault_control_attempt);
+            if (!post_pose_fault.empty()) {
+                status.state = AgvTaskState::Failed;
+                std::string attribution;
+                if (post_pose_fault_control_attempt != 0
+                    && post_pose_fault_control_attempt
+                        != pose_context
+                            .control_attempt_sequence_at_start) {
+                    attribution =
+                        "controller_fault_attribution=ambiguous because the "
+                        "fault was observed after another control command "
+                        "attempt had begun, ";
+                }
+                status.message =
+                    "SEER Robokit reported the tracked free-navigation task "
+                    "Completed, but a new controller fault was observed during "
+                    "target verification: " + task_status.detail + ", "
+                    + attribution + post_pose_fault;
+            } else if (const std::string post_pose_unavailable =
+                           fault_monitoring_unavailable();
+                       !post_pose_unavailable.empty()) {
+                status.state = AgvTaskState::Failed;
+                status.message =
+                    "SEER Robokit tracked free-navigation completion is unsafe to "
+                    "accept because controller fault monitoring became "
+                    "unavailable: " + post_pose_unavailable
+                    + "; query the controller and cancel or stop before "
+                      "another motion command";
+                return status;
+            } else if (!target_reached) {
+                status.state = AgvTaskState::Failed;
+                status.message =
+                    "SEER Robokit reported the tracked free-navigation task "
+                    "Completed, but the requested target was not reached: "
+                        + task_status.detail + ", " + pose_detail;
+            } else {
+                status.message += ", target_verified: " + pose_detail;
+            }
+        }
+        if (controller_state_terminal) {
+            clearPoseTask_(pose_context.navigation_generation);
+        }
+        return status;
+    }
+
+    PoseTaskContext changed_context;
+    if (currentPoseTask_(changed_context)) {
+        status.state = AgvTaskState::Waiting;
+        status.type = AgvTaskType::NavigateToPose;
+        status.message =
+            "SEER Robokit free-navigation task changed while its status was being "
+            "queried; query navigation status again";
+        return status;
+    }
+
+    Json::Value payload(Json::objectValue);
+    jsonMember(payload, "simple") = false;
+
+    Json::Value response;
+    const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response);
+    if (!result.ok()) {
+        status.state = AgvTaskState::Failed;
+        status.message = missing_pose_task_detail.empty()
+            ? result.message
+            : missing_pose_task_detail + "; 1020 status query failed: "
+                + result.message;
+        return status;
+    }
+    const auto controller_result = resultFromResponse_(response);
+    if (!controller_result.ok()) {
+        status.state = AgvTaskState::Failed;
+        status.message = missing_pose_task_detail.empty()
+            ? controller_result.message
+            : missing_pose_task_detail + "; 1020 status query failed: "
+                + controller_result.message;
+        return status;
+    }
+
+    status.state = toTaskState(jsonGet(response, "task_status", 0).asInt());
+    status.type = toTaskType(jsonGet(response, "task_type", 0).asInt());
+    status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString();
+    if (!missing_pose_task_detail.empty()) {
+        status.message = missing_pose_task_detail
+            + "; fallback_1020_status=" + std::to_string(
+                jsonGet(response, "task_status", 0).asInt())
+            + ", fallback_1020_type=" + std::to_string(
+                jsonGet(response, "task_type", 0).asInt())
+            + (status.message.empty() ? std::string{} : ", " + status.message);
+    }
+    if (const auto* task_status_package = jsonFind(response, "task_status_package")) {
+        status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble();
+    }
+    return status;
+}
+
+AgvResult SeerRobokitAgv::configurePush_()
+{
+    if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) {
+        return AgvResult::failure(
+            AgvErrorCode::InvalidArgument,
+            "SEER Robokit push included_fields and excluded_fields cannot both be set");
+    }
+
+    Json::Value payload(Json::objectValue);
+    if (config_.state_push_interval_ms() > 0) {
+        jsonMember(payload, "interval") = config_.state_push_interval_ms();
+    }
+    appendStringArray(payload, "included_fields", config_.state_push_included_fields());
+    appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields());
+
+    if (payload.empty()) {
+        return AgvResult::success();
+    }
+
+    const std::string payload_text = toJsonString_(payload);
+    const auto frame = buildFrame_(kRobotPushConfigReq, payload_text);
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (sock_push_ < 0) {
+        return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit push socket not connected");
+    }
+    if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast<ssize_t>(frame.size())) {
+        return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit send push config failed: " + systemError());
+    }
+
+    while (true) {
+        std::uint16_t command = 0;
+        std::string response_payload;
+        const auto result = receiveFrame_(sock_push_, command, response_payload);
+        if (!result.ok()) {
+            return result;
+        }
+
+        Json::Value response;
+        std::string error;
+        if (!response_payload.empty() && !parseJson_(response_payload, response, error)) {
+            return AgvResult::failure(AgvErrorCode::CommandFailed, error);
+        }
+
+        if (command == kRobotPushConfigRes) {
+            return resultFromResponse_(response);
+        }
+        if (command == kRobotPush && response.isObject()) {
+            updateCachedRuntimeState_(response);
+        }
+    }
+}
+
+void SeerRobokitAgv::startPushThread_()
+{
+    if (!state_push_enabled_) {
+        return;
+    }
+    if (push_running_.exchange(true)) {
+        return;
+    }
+    if (sock_push_ < 0) {
+        push_running_ = false;
+        return;
+    }
+    push_thread_ = std::thread(&SeerRobokitAgv::pushLoop_, this);
+}
+
+void SeerRobokitAgv::stopPushThread_()
+{
+    const bool was_running = push_running_.exchange(false);
+    if (was_running) {
+        int sock = -1;
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            sock = sock_push_;
+        }
+        if (sock >= 0) {
+            ::shutdown(sock, SHUT_RDWR);
+        }
+    }
+    if (push_thread_.joinable()) {
+        push_thread_.join();
+    }
+    invalidateControllerFaultState_();
+}
+
+void SeerRobokitAgv::pushLoop_()
+{
+    while (push_running_) {
+        int sock = -1;
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            sock = sock_push_;
+        }
+        if (sock < 0) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            continue;
+        }
+
+        std::uint16_t command = 0;
+        std::string payload;
+        const auto result = receiveFrame_(sock, command, payload);
+        if (!push_running_) {
+            break;
+        }
+        if (!result.ok()) {
+            if (result.code != AgvErrorCode::Timeout) {
+                invalidateControllerFaultState_();
+                std::lock_guard<std::mutex> lock(mutex_);
+                last_error_ = result.message;
+                closeSocket_(sock_push_);
+            }
+            continue;
+        }
+        if (command != kRobotPush || payload.empty()) {
+            continue;
+        }
+
+        Json::Value parsed;
+        std::string error;
+        if (!parseJson_(payload, parsed, error)) {
+            invalidateControllerFaultState_();
+            std::lock_guard<std::mutex> lock(mutex_);
+            last_error_ = error;
+            continue;
+        }
+        updateCachedRuntimeState_(parsed);
+    }
+}
+
+void SeerRobokitAgv::invalidateControllerFaultState_()
+{
+    std::lock_guard<std::mutex> lock(runtime_state_mutex_);
+    controller_fault_channel_epoch_.fetch_add(
+        1,
+        std::memory_order_relaxed);
+    controller_fault_state_observed_ = false;
+    controller_fault_state_observed_at_ = {};
+    active_controller_fault_detail_.clear();
+    runtime_state_cv_.notify_all();
+}
+
+void SeerRobokitAgv::updateCachedRuntimeState_(const Json::Value& payload)
+{
+    std::lock_guard<std::mutex> lock(runtime_state_mutex_);
+    auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{};
+    state.timestamp = nowSeconds();
+    state.connected = true;
+
+    if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble();
+    if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble();
+    if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble();
+    if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble();
+    if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble();
+    if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble();
+    if (jsonHas(payload, "battery_level")) {
+        state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble();
+    }
+    if (jsonHas(payload, "battery_temp")) {
+        state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble();
+    }
+    if (jsonHas(payload, "charging")) {
+        state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool();
+    }
+    if (jsonHas(payload, "voltage")) {
+        state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble();
+    }
+    if (jsonHas(payload, "current")) {
+        state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble();
+    }
+    if (jsonHas(payload, "current_map")) {
+        state.current_map = jsonGet(payload, "current_map", state.current_map).asString();
+    }
+    if (jsonHas(payload, "current_station")) {
+        state.current_station = jsonGet(payload, "current_station", state.current_station).asString();
+    }
+    if (jsonHas(payload, "confidence")) {
+        state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0;
+    }
+    if (jsonHas(payload, "emergency")) {
+        state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool();
+    }
+
+    state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4;
+    const bool has_fatals = jsonHas(payload, "fatals");
+    const bool has_errors = jsonHas(payload, "errors");
+    const bool has_fault_fields = has_fatals || has_errors;
+    if (has_fault_fields) {
+        const auto* fatals = jsonFind(payload, "fatals");
+        const auto* errors = jsonFind(payload, "errors");
+        const bool valid_fatals = !has_fatals
+            || (fatals && fatals->isArray());
+        const bool valid_errors = !has_errors
+            || (errors && errors->isArray());
+        const bool complete_fault_state =
+            has_fatals && has_errors && valid_fatals && valid_errors;
+        if (complete_fault_state) {
+            controller_fault_state_observed_ = true;
+            controller_fault_state_observed_at_ =
+                std::chrono::steady_clock::now();
+        } else {
+            controller_fault_state_observed_ = false;
+            controller_fault_state_observed_at_ = {};
+        }
+
+        const bool reported_fault =
+            hasFaultArray(payload, "fatals")
+            || hasFaultArray(payload, "errors");
+        const bool invalid_or_incomplete_fault_state =
+            !complete_fault_state && !reported_fault;
+        state.fault = reported_fault
+            || invalid_or_incomplete_fault_state;
+        if (state.fault) {
+            std::ostringstream detail;
+            detail << (reported_fault
+                    ? "SEER Robokit controller fault"
+                    : "SEER Robokit controller fault state is incomplete or malformed");
+            if (fatals
+                && (!fatals->isArray()
+                    || !fatals->empty()
+                    || !complete_fault_state)) {
+                detail << ": fatals="
+                       << (fatals->isNull()
+                               ? std::string("null")
+                               : jsonValueToString(*fatals));
+            }
+            if (errors
+                && (!errors->isArray()
+                    || !errors->empty()
+                    || !complete_fault_state)) {
+                detail << ": errors="
+                       << (errors->isNull()
+                               ? std::string("null")
+                               : jsonValueToString(*errors));
+            }
+            state.last_error = detail.str();
+            if (state.last_error != active_controller_fault_detail_) {
+                active_controller_fault_detail_ = state.last_error;
+                ++controller_fault_sequence_;
+                last_controller_fault_timestamp_ = state.timestamp;
+                last_controller_fault_detail_ = state.last_error;
+                last_controller_fault_control_attempt_ =
+                    control_attempt_sequence_.load(
+                        std::memory_order_acquire);
+            }
+        } else {
+            state.last_error.clear();
+            active_controller_fault_detail_.clear();
+        }
+    }
+    if (state.emergency_stopped) {
+        state.mode = AgvMode::EmergencyStop;
+    } else if (state.fault) {
+        state.mode = AgvMode::Fault;
+    } else if (state.battery.charging) {
+        state.mode = AgvMode::Charging;
+    } else if (state.moving) {
+        state.mode = AgvMode::Auto;
+    } else {
+        state.mode = AgvMode::Idle;
+    }
+
+    cached_runtime_state_ = state;
+    cached_runtime_state_valid_ = true;
+    runtime_state_cv_.notify_all();
+}
+
+} // namespace cmvr::device

cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp

@@ -0,0 +1,362 @@
+#include "seer_robokit_agv.h"
+#include "seer_robokit_protocol.h"
+#include "seer_robokit_utils.h"
+
+#include <algorithm>
+#include <arpa/inet.h>
+#include <cstddef>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+#include <utility>
+#include <vector>
+
+namespace cmvr::device {
+
+using namespace seer_robokit::protocol;
+using namespace seer_robokit::detail;
+
+AgvResult SeerRobokitAgv::connectSocket_(int& sock, const int port)
+{
+    sock = ::socket(AF_INET, SOCK_STREAM, 0);
+    if (sock < 0) {
+        last_error_ = "create socket failed: " + systemError();
+        return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
+    }
+
+    sockaddr_in address{};
+    address.sin_family = AF_INET;
+    address.sin_port = htons(static_cast<std::uint16_t>(port));
+    if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) {
+        closeSocket_(sock);
+        last_error_ = "invalid SEER Robokit ip: " + ip_;
+        return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_);
+    }
+
+    if (::connect(sock, reinterpret_cast<sockaddr*>(&address), sizeof(address)) < 0) {
+        closeSocket_(sock);
+        last_error_ = "connect SEER Robokit port " + std::to_string(port) + " failed: " + systemError();
+        return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
+    }
+
+    timeval timeout{};
+    timeout.tv_sec = recv_timeout_ms_ / 1000;
+    timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000;
+    ::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
+    return AgvResult::success();
+}
+
+AgvResult SeerRobokitAgv::ensureOtherSocket_()
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (sock_other_ >= 0) {
+        return AgvResult::success();
+    }
+    return connectSocket_(sock_other_, ports_.other);
+}
+
+void SeerRobokitAgv::closeSocket_(int& sock) const
+{
+    if (sock >= 0) {
+        ::close(sock);
+        sock = -1;
+    }
+}
+
+bool SeerRobokitAgv::connected_() const
+{
+    return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0;
+}
+
+AgvResult SeerRobokitAgv::sendCommand_(
+    const int sock,
+    const std::uint16_t command,
+    const Json::Value& payload,
+    Json::Value* response,
+    CommandTransmissionState* transmission_state) const
+{
+    std::string response_payload;
+    auto result = sendCommandRaw_(
+        sock,
+        command,
+        payload,
+        &response_payload,
+        transmission_state);
+    if (!result.ok()) {
+        return result;
+    }
+    if (!response) {
+        return AgvResult::success();
+    }
+
+    Json::Value parsed;
+    std::string error;
+    if (!parseJson_(response_payload, parsed, error)) {
+        const std::string json_text = extractJson_(response_payload);
+        if (json_text.empty() || !parseJson_(json_text, parsed, error)) {
+            return AgvResult::failure(AgvErrorCode::CommandFailed, error);
+        }
+    }
+
+    *response = std::move(parsed);
+    return AgvResult::success();
+}
+
+AgvResult SeerRobokitAgv::sendCommandRaw_(
+    const int sock,
+    const std::uint16_t command,
+    const Json::Value& payload,
+    std::string* response_payload,
+    CommandTransmissionState* transmission_state) const
+{
+    if (transmission_state) {
+        *transmission_state = CommandTransmissionState::NotSent;
+    }
+    const auto exchange = [&]() {
+        const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload);
+        const auto frame = buildFrame_(command, payload_text);
+        const auto sent = ::send(
+            sock,
+            frame.data(),
+            frame.size(),
+            MSG_NOSIGNAL);
+        if (sent > 0 && transmission_state) {
+            *transmission_state = CommandTransmissionState::PossiblySent;
+        }
+        if (sent != static_cast<ssize_t>(frame.size())) {
+            return AgvResult::failure(
+                AgvErrorCode::CommandFailed,
+                "SEER Robokit send command failed: " + systemError());
+        }
+
+        std::uint16_t response_command = 0;
+        std::string payload_text_response;
+        const auto result = receiveFrame_(sock, response_command, payload_text_response);
+        if (!result.ok()) {
+            return result;
+        }
+        const auto expected_response_command = static_cast<std::uint16_t>(
+            command + 10000U);
+        if (response_command != expected_response_command) {
+            return AgvResult::failure(
+                AgvErrorCode::CommandFailed,
+                "SEER Robokit response command mismatch: expected="
+                    + std::to_string(expected_response_command)
+                    + ", actual=" + std::to_string(response_command));
+        }
+        if (response_payload) {
+            *response_payload = std::move(payload_text_response);
+        }
+        return AgvResult::success();
+    };
+    const auto close_matching_socket_locked = [this, sock]() {
+        if (sock == sock_status_) {
+            closeSocket_(sock_status_);
+        } else if (sock == sock_control_) {
+            closeSocket_(sock_control_);
+        } else if (sock == sock_navigation_) {
+            closeSocket_(sock_navigation_);
+        } else if (sock == sock_config_) {
+            closeSocket_(sock_config_);
+        } else if (sock == sock_other_) {
+            closeSocket_(sock_other_);
+        }
+    };
+    const auto mark_channel_desynchronized = [](AgvResult result) {
+        std::string detail = result.message.empty()
+            ? "unknown transport or frame error"
+            : result.message;
+        detail +=
+            "; SEER Robokit channel closed because the response stream may be "
+            "desynchronized; reconnect before sending another command";
+        return AgvResult::failure(result.code, detail);
+    };
+
+    bool is_status_socket = false;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (sock < 0) {
+            return AgvResult::failure(
+                AgvErrorCode::NotConnected,
+                "SEER Robokit socket not connected");
+        }
+        is_status_socket = sock == sock_status_;
+    }
+
+    if (is_status_socket) {
+        // A slow 1110 status response must never hold the lifecycle/global I/O
+        // mutex needed by cancelNavigation() or emergencyStop(). The dedicated
+        // status lock still serializes requests on port 19204. connect_() and
+        // disconnect_() take this lock before changing the descriptor.
+        std::lock_guard<std::mutex> status_lock(status_io_mutex_);
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            if (sock < 0 || sock != sock_status_) {
+                return AgvResult::failure(
+                    AgvErrorCode::NotConnected,
+                    "SEER Robokit status socket is no longer connected");
+            }
+        }
+        auto result = exchange();
+        if (!result.ok()) {
+            std::lock_guard<std::mutex> lock(mutex_);
+            close_matching_socket_locked();
+            return mark_channel_desynchronized(std::move(result));
+        }
+        return result;
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (sock < 0
+        || (sock != sock_control_
+            && sock != sock_navigation_
+            && sock != sock_config_
+            && sock != sock_other_)) {
+        return AgvResult::failure(
+            AgvErrorCode::NotConnected,
+            "SEER Robokit socket is no longer connected");
+    }
+    auto result = exchange();
+    if (!result.ok()) {
+        close_matching_socket_locked();
+        return mark_channel_desynchronized(std::move(result));
+    }
+    return result;
+}
+
+AgvResult SeerRobokitAgv::sendCommandNoResponse_(
+    const int sock,
+    const std::uint16_t command,
+    const Json::Value& payload) const
+{
+    return sendCommand_(sock, command, payload, nullptr);
+}
+
+std::vector<std::uint8_t> SeerRobokitAgv::buildFrame_(
+    const std::uint16_t command,
+    const std::string& payload)
+{
+    std::vector<std::uint8_t> frame(16 + payload.size(), 0);
+    frame[0] = 0x5A;
+    frame[1] = 0x01;
+    frame[2] = 0x00;
+    frame[3] = 0x01;
+    const auto length = static_cast<std::uint32_t>(payload.size());
+    frame[4] = static_cast<std::uint8_t>((length >> 24U) & 0xFFU);
+    frame[5] = static_cast<std::uint8_t>((length >> 16U) & 0xFFU);
+    frame[6] = static_cast<std::uint8_t>((length >> 8U) & 0xFFU);
+    frame[7] = static_cast<std::uint8_t>(length & 0xFFU);
+    frame[8] = static_cast<std::uint8_t>((command >> 8U) & 0xFFU);
+    frame[9] = static_cast<std::uint8_t>(command & 0xFFU);
+    std::copy(payload.begin(), payload.end(), frame.begin() + 16);
+    return frame;
+}
+
+std::string SeerRobokitAgv::toJsonString_(const Json::Value& value)
+{
+    Json::StreamWriterBuilder builder;
+    builder["indentation"] = "";
+    return Json::writeString(builder, value);
+}
+
+bool SeerRobokitAgv::parseJson_(const std::string& input, Json::Value& output, std::string& error)
+{
+    Json::CharReaderBuilder builder;
+    std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
+    return reader->parse(input.data(), input.data() + input.size(), &output, &error);
+}
+
+std::string SeerRobokitAgv::extractJson_(const std::string& raw)
+{
+    const auto begin = raw.find('{');
+    const auto end = raw.rfind('}');
+    if (begin == std::string::npos || end == std::string::npos || end < begin) {
+        return {};
+    }
+    return raw.substr(begin, end - begin + 1);
+}
+
+AgvResult SeerRobokitAgv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload)
+{
+    const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult {
+        std::size_t offset = 0;
+        while (offset < size) {
+            const ssize_t count = ::recv(fd, data + offset, size - offset, 0);
+            if (count > 0) {
+                offset += static_cast<std::size_t>(count);
+                continue;
+            }
+            if (count == 0) {
+                return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit socket closed");
+            }
+            if (errno == EINTR) {
+                continue;
+            }
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit receive timeout");
+            }
+            return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit receive failed: " + systemError());
+        }
+        return AgvResult::success();
+    };
+
+    std::uint8_t header[16]{};
+    auto result = recv_exact(sock, header, sizeof(header));
+    if (!result.ok()) {
+        return result;
+    }
+    if (header[0] != 0x5A) {
+        return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame header is invalid");
+    }
+
+    const auto length = (static_cast<std::uint32_t>(header[4]) << 24U)
+        | (static_cast<std::uint32_t>(header[5]) << 16U)
+        | (static_cast<std::uint32_t>(header[6]) << 8U)
+        | static_cast<std::uint32_t>(header[7]);
+    command = static_cast<std::uint16_t>((static_cast<std::uint16_t>(header[8]) << 8U) | header[9]);
+    payload.clear();
+    if (length == 0) {
+        return AgvResult::success();
+    }
+    if (length > kMaxFramePayloadBytes) {
+        return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame payload is too large");
+    }
+
+    std::vector<std::uint8_t> buffer(length);
+    result = recv_exact(sock, buffer.data(), buffer.size());
+    if (!result.ok()) {
+        return result;
+    }
+    payload.assign(reinterpret_cast<const char*>(buffer.data()), buffer.size());
+    return AgvResult::success();
+}
+
+
+AgvResult SeerRobokitAgv::resultFromResponse_(const Json::Value& response)
+{
+    if (!hasNumericControllerRetCode(response)) {
+        return AgvResult::failure(
+            AgvErrorCode::CommandFailed,
+            "SEER Robokit controller response is missing a numeric ret_code");
+    }
+    const auto* ret_code_value = jsonFind(response, "ret_code");
+    const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64()
+        ? ret_code_value->asUInt64() == 0
+        : ret_code_value->asInt64() == 0;
+    const std::string ret_code = jsonValueToString(*ret_code_value);
+    const std::string message = jsonGet(response, "err_msg", "").asString();
+    if (success) {
+        return AgvResult::success();
+    }
+    std::string detail = "SEER Robokit command failed: ret_code=" + ret_code;
+    if (!message.empty()) {
+        detail += ", err_msg=" + message;
+    }
+    return AgvResult::failure(AgvErrorCode::CommandFailed, detail);
+}
+
+} // namespace cmvr::device

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

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

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

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

cmvr-es/devices/arm/CMakeLists.txt

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

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

@@ -2,6 +2,8 @@ add_library(aubo_arm SHARED
         aubo_arm.cpp
 )
 
+find_package(Threads REQUIRED)
+
 target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
@@ -71,7 +73,94 @@ target_link_libraries(aubo_arm
         cmvr_es::proto
         PRIVATE
         glog
+        jsoncpp
+        Threads::Threads
 )
 
 add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm)
 install(TARGETS aubo_arm LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    enable_testing()
+    add_executable(aubo_arm_motion_result_test
+            tests/aubo_arm_motion_result_test.cpp
+    )
+    target_include_directories(aubo_arm_motion_result_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    add_test(
+            NAME aubo_arm_motion_result_test
+            COMMAND aubo_arm_motion_result_test
+    )
+    set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10)
+
+    add_executable(aubo_motion_state_test
+            tests/aubo_motion_state_test.cpp
+    )
+    target_include_directories(aubo_motion_state_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    add_test(
+            NAME aubo_motion_state_test
+            COMMAND aubo_motion_state_test
+    )
+    set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10)
+
+    add_executable(aubo_safety_state_test
+            tests/aubo_safety_state_test.cpp
+    )
+    target_include_directories(aubo_safety_state_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    add_test(
+            NAME aubo_safety_state_test
+            COMMAND aubo_safety_state_test
+    )
+    set_tests_properties(aubo_safety_state_test PROPERTIES TIMEOUT 10)
+
+    add_executable(aubo_arm_json_command_test
+            tests/aubo_arm_json_command_test.cpp
+    )
+    target_link_libraries(aubo_arm_json_command_test
+            PRIVATE
+            cmvr_es::device::aubo_arm
+            cmvr_es::proto
+    )
+    add_test(
+            NAME aubo_arm_json_command_test
+            COMMAND aubo_arm_json_command_test
+    )
+    set_tests_properties(aubo_arm_json_command_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(UNIX AND NOT APPLE)
+        # The imported AUBO target still contributes its vendor directory to
+        # direct consumers' build-tree RUNPATH. Put the system runtime first
+        # for this test; installed artifacts exclude the vendor libstdc++.
+        execute_process(
+                COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6
+                OUTPUT_VARIABLE AUBO_TEST_SYSTEM_LIBSTDCXX
+                OUTPUT_STRIP_TRAILING_WHITESPACE
+        )
+        if(EXISTS "${AUBO_TEST_SYSTEM_LIBSTDCXX}")
+            get_filename_component(
+                    AUBO_TEST_SYSTEM_LIBSTDCXX_REAL
+                    "${AUBO_TEST_SYSTEM_LIBSTDCXX}"
+                    REALPATH
+            )
+            get_filename_component(
+                    AUBO_TEST_SYSTEM_LIBSTDCXX_DIR
+                    "${AUBO_TEST_SYSTEM_LIBSTDCXX_REAL}"
+                    DIRECTORY
+            )
+            set_property(
+                    TARGET aubo_arm_json_command_test
+                    PROPERTY BUILD_RPATH "${AUBO_TEST_SYSTEM_LIBSTDCXX_DIR}"
+            )
+        endif()
+    endif()
+endif()

cmvr-es/devices/arm/aubo_arm/README.md

@@ -0,0 +1,142 @@
+# AUBO RobotArm 与控制柜 IO
+
+`AuboArm` 是 AUBO SDK v0.27.1 的 `RobotArm` 后端。控制柜 Standard 数字 IO
+通过设备通用的 `executeJsonCommand` 接口访问，远程调用复用
+`cmvr.api.ArmService/ExecuteJsonCommand`，不经过 `SystemService` 或
+`MotorService`。该 RPC 只路由到 `RobotArm`，不会把 JSON 命令转发给其他设备类型。
+旧的 `cmvr.api.SystemService/ExecuteJsonCommand` 不再注册，调用方必须更新服务路径；
+请求和响应消息结构保持不变。
+
+返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。
+
+## 代码与配置
+
+- 实现：[`aubo_arm.h`](aubo_arm.h)、[`aubo_arm.cpp`](aubo_arm.cpp)
+- 测试：[`tests/aubo_arm_json_command_test.cpp`](tests/aubo_arm_json_command_test.cpp)
+- 设备配置：[`../../../config/devices/arm/aubo_arm.pb.txt`](../../../config/devices/arm/aubo_arm.pb.txt)
+- DeviceManager 配置：
+  [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
+- ArmService 实现：
+  [`../../../service/grpc/src/grpc_arm_service.cpp`](../../../service/grpc/src/grpc_arm_service.cpp)
+- Proto：[`../../../../protos/cmvr/api/arm_service.proto`](../../../../protos/cmvr/api/arm_service.proto)
+
+仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据，
+不要把生产密码提交到默认配置。
+
+## 控制柜 Standard 数字 IO
+
+当前支持：
+
+| `operation` | 说明 | 必填字段 |
+| --- | --- | --- |
+| `get_di` | 读取控制柜数字输入 | `index` |
+| `get_do` | 读取控制柜数字输出及其 runstate | `index` |
+| `set_do` | 设置控制柜数字输出 | `index`、`value` |
+
+JSON 命令：
+
+```json
+{"command":"cabinet_io","operation":"get_di","index":0}
+{"command":"cabinet_io","operation":"get_do","index":0}
+{"command":"cabinet_io","operation":"set_do","index":0,"value":true}
+```
+
+`index` 从 `0` 开始，运行时根据控制器返回的 IO 数量检查范围。
+`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。
+`set_do` 成功响应中的 `requested_value` 只表示 SDK 已接受请求；确认实际输出时
+必须再调用 `get_do`。
+
+读取成功响应示例：
+
+```json
+{
+  "success": true,
+  "command": "cabinet_io",
+  "operation": "get_di",
+  "index": 0,
+  "count": 16,
+  "value": false
+}
+```
+
+## 通过 gRPC 调用
+
+默认 gRPC 端口为 `50052`。读取 DI0：
+
+```shell
+grpcurl -plaintext \
+  -d '{
+    "header":{"deviceId":"aubo_arm"},
+    "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}"
+  }' \
+  127.0.0.1:50052 \
+  cmvr.api.ArmService/ExecuteJsonCommand
+```
+
+设置 DO0 为高电平：
+
+```shell
+grpcurl -plaintext \
+  -d '{
+    "header":{"deviceId":"aubo_arm"},
+    "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}"
+  }' \
+  127.0.0.1:50052 \
+  cmvr.api.ArmService/ExecuteJsonCommand
+```
+
+使用源码默认配置时：
+
+1. 在 `cmvr-es/config/devices/arm/aubo_arm.pb.txt` 填写正确地址和登录信息；
+2. 在 `cmvr-es/config/manager/device_manager.pb.txt` 将 `aubo_arm.enable`
+   改为 `true`；
+3. 重新安装配置并启动安装产物。
+
+```shell
+cmake --install build
+./output/bin/cmvr_es
+```
+
+`output/bin/cmvr_es` 默认读取 `output/bin/config/`。使用 `--config` 时，应修改
+对应外部配置根。设备未启用或初始化失败时，gRPC 返回
+`Device not found: aubo_arm`。
+
+## 安全与语义边界
+
+- 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、
+  `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时，
+  所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝；
+- 硬件急停、防护停机、Safety Fault/Violation 会锁存安全事件，并使当前运动
+  generation 失效。控制器重新报告 `Normal`/`ReducedMode` 不会自动解除锁存；
+- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。只有确认
+  `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止且机械臂稳定后，
+  显式 `torqueOn`/`clearFault`/`unlockProtectiveStop` 才可能恢复运动权限；
+- 恢复流程不会调用 `resume`、`arbitraryResume`、`startMove`，也不会重新提交
+  急停前的目标、速度、servo 指令或程序；
+- AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放
+  急停开关后的控制器恢复时序。因此本实现保持 fail-closed 并在释放后再次清队列，
+  但“释放开关后零位移”的最终保证仍需真机验证及控制器侧安全配置配合；
+- 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO；
+- `set_do` 不修改输出 runstate；
+- 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回
+  `output_managed_by_runstate`；
+- 普通访问不会调用会重置全部输出配置的
+  `setDigitalOutputRunstateDefault()`；
+- 模拟量 IO 涉及 domain、单位和量程，当前 JSON 接口不开放；
+- gRPC/JSON 返回成功不代表目标 IO 具备功能安全等级；
+- 真实写测试前应确认通道用途、负载、电气隔离、默认电平和控制器程序所有权。
+
+## 测试
+
+```bash
+cmake --build build --target \
+  aubo_safety_state_test \
+  aubo_motion_state_test \
+  aubo_arm_json_command_test -j4
+ctest --test-dir build \
+  -R 'aubo_(safety_state|motion_state|arm_json_command)_test' \
+  --output-on-failure
+```
+
+该测试覆盖 JSON 校验和无硬件错误路径，不代表已在真实 AUBO 控制柜完成 DI/DO
+读取、写入或 runstate 拒绝验证。

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

@@ -2,6 +2,7 @@
 #define CMVR_ES_AUBO_ARM_H
 
 #include <atomic>
+#include <cstdint>
 #include <memory>
 #include <mutex>
 #include <optional>
@@ -21,6 +22,8 @@ public:
     std::string typeName() const override { return "AuboARM"; }
     bool init() override;
     bool stop() override;
+    bool executeJsonCommand(const std::string& request_json,
+                            std::string& response_json) override;
 
     RobotModel getRobotModel() const override { return model_; }
     std::size_t getDof() const override { return model_.dof; }
@@ -28,19 +31,28 @@ public:
     JointGroupState getJointState() const override;
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
     RobotMode getRobotMode() const override;
-    SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
+    SafetyMode getSafetyMode() const override;
-    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
+    ControlMode getControlMode() const override;
+    bool supportsActionQueueMotion() const noexcept override { return true; }
 
     Result torqueOn() override;
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override { return emergencyStop(); }
+    Result protectiveStop() override;
+    Result recoverProtectiveStop(
+        const JointTrajectory&,
+        const MotionOptions&) override
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "protective recovery is not implemented for AuboArm");
+    }
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
-    bool isProtectiveStopped() const override { return false; }
+    bool isProtectiveStopped() const override;
-    bool isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isEmergencyStopped() const override;
-    bool isFault() const override { return false; }
+    bool isFault() const override;
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
     Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override;
@@ -64,8 +76,8 @@ public:
     Result powerOff() override { return torqueOff(); }
     Result brakeRelease() override { return torqueOn(); }
     Result shutdown() override;
-    Result clearFault() override { return Result::success(); }
+    Result clearFault() override;
-    Result unlockProtectiveStop() override { return Result::success(); }
+    Result unlockProtectiveStop() override;
     Result loadProgram(const std::string& program_name) override;
     Result playProgram() override;
     Result pauseProgram() override;
@@ -78,16 +90,27 @@ public:
     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 busy_.load(); }
+    bool busy() const override;
 
 private:
+    enum class MotionStopKind {
+        Automatic,
+        Joint,
+        Linear,
+    };
+
     Result unsupported_(const std::string& name) const;
     bool validDof_(std::size_t size, std::string& error) const;
     Result ensureConnected_(const std::string& context) const;
+    Result ensureMotionReady_(const std::string& context,
+                              std::uint64_t& safety_epoch) const;
+    Result completeSafetyRecovery_(const std::string& context,
+                                   std::uint64_t expected_safety_epoch);
+    Result unlockProtectiveStop_(
+        std::optional<std::uint64_t> expected_safety_epoch);
+    Result stopMotion_(MotionStopKind kind, double acceleration);
 
-#if defined(CMVR_HAS_AUBO_SDK)
     struct SdkState;
-#endif
 
 private:
     config::RobotArmConfig cfg_;
@@ -102,12 +125,10 @@ private:
     std::atomic<bool> connected_{false};
     std::atomic<bool> busy_{false};
     std::atomic<bool> servo_mode_{false};
-    bool emergency_stopped_{false};
+    std::atomic<bool> emergency_stopped_{false};
     mutable std::mutex mutex_;
 
-#if defined(CMVR_HAS_AUBO_SDK)
     std::unique_ptr<SdkState> sdk_;
-#endif
 };
 
 } // namespace cmvr::device

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

@@ -0,0 +1,45 @@
+#ifndef CMVR_ES_AUBO_MOTION_RESULT_H
+#define CMVR_ES_AUBO_MOTION_RESULT_H
+
+namespace cmvr::device::aubo_internal {
+
+enum class MotionCommandOutcome {
+    CompletedWithoutMotion,
+    CompletedAfterMotion,
+    Cancelled,
+    SubmitFailed,
+    CompletionFailed,
+};
+
+enum class MotionWaitResult {
+    Completed,
+    Cancelled,
+    Failed,
+};
+
+template <typename WaitForCompletion>
+MotionCommandOutcome resolveMotionCommand(
+    const int return_code,
+    const int success_code,
+    const int request_ignore_code,
+    WaitForCompletion&& wait_for_completion)
+{
+    if (return_code == request_ignore_code) {
+        return MotionCommandOutcome::CompletedWithoutMotion;
+    }
+    if (return_code != success_code) {
+        return MotionCommandOutcome::SubmitFailed;
+    }
+    const auto wait_result = wait_for_completion();
+    if (wait_result == MotionWaitResult::Cancelled) {
+        return MotionCommandOutcome::Cancelled;
+    }
+    if (wait_result != MotionWaitResult::Completed) {
+        return MotionCommandOutcome::CompletionFailed;
+    }
+    return MotionCommandOutcome::CompletedAfterMotion;
+}
+
+} // namespace cmvr::device::aubo_internal
+
+#endif // CMVR_ES_AUBO_MOTION_RESULT_H

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

@@ -0,0 +1,296 @@
+#ifndef CMVR_ES_AUBO_MOTION_STATE_H
+#define CMVR_ES_AUBO_MOTION_STATE_H
+
+#include <algorithm>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <mutex>
+
+namespace cmvr::device::aubo_internal {
+
+enum class MotionKind {
+    None,
+    Joint,
+    Linear,
+};
+
+struct MotionToken {
+    std::uint64_t generation{0};
+    MotionKind kind{MotionKind::None};
+
+    bool valid() const noexcept
+    {
+        return generation != 0 && kind != MotionKind::None;
+    }
+};
+
+enum class MotionStartStatus {
+    Started,
+    Invalid,
+    Busy,
+    Stopping,
+    Blocked,
+};
+
+struct MotionStartResult {
+    MotionStartStatus status{MotionStartStatus::Busy};
+    MotionToken token;
+
+    bool started() const noexcept
+    {
+        return status == MotionStartStatus::Started;
+    }
+};
+
+enum class MotionFinishMode {
+    RestorePrevious,
+    Clear,
+    Retain,
+};
+
+enum class StopStartStatus {
+    Started,
+    AlreadyStopping,
+};
+
+struct StopRequest {
+    StopStartStatus status{StopStartStatus::AlreadyStopping};
+    MotionKind kind{MotionKind::None};
+    MotionToken active_token;
+    bool tracked_motion{false};
+
+    bool started() const noexcept
+    {
+        return status == StopStartStatus::Started;
+    }
+};
+
+struct SafetyCancelResult {
+    MotionKind kind{MotionKind::None};
+    MotionToken active_token;
+    bool tracked_motion{false};
+};
+
+// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized
+// through the vendor call. Speed calls release the outer mutex before their
+// potentially blocking SDK call, so the generation cancellation below also
+// closes the stop-vs-speed-submission race.
+class MotionState final {
+public:
+    MotionStartResult begin(
+        const MotionKind kind,
+        const bool replace_retained_same_kind = false)
+    {
+        std::lock_guard lock(mutex_);
+        if (kind == MotionKind::None) {
+            return {MotionStartStatus::Invalid, {}};
+        }
+        if (stop_in_progress_) {
+            return {MotionStartStatus::Stopping, {}};
+        }
+        if (blocked_) {
+            return {MotionStartStatus::Blocked, {}};
+        }
+        if (owner_active_) {
+            return {MotionStartStatus::Busy, {}};
+        }
+        if (last_kind_ != MotionKind::None &&
+            (!replace_retained_same_kind || last_kind_ != kind)) {
+            return {MotionStartStatus::Busy, {}};
+        }
+
+        MotionToken token{++next_generation_, kind};
+        owner_active_ = true;
+        active_token_ = token;
+        previous_kind_ = last_kind_;
+        return {MotionStartStatus::Started, token};
+    }
+
+    void finish(
+        const MotionToken& token,
+        const MotionFinishMode mode = MotionFinishMode::RestorePrevious)
+    {
+        std::lock_guard lock(mutex_);
+        if (!owner_active_ ||
+            active_token_.generation != token.generation) {
+            return;
+        }
+
+        owner_active_ = false;
+        active_token_ = {};
+        if (!stop_in_progress_ && !blocked_) {
+            if (mode == MotionFinishMode::Retain) {
+                last_kind_ = token.kind;
+            } else if (mode == MotionFinishMode::Clear) {
+                last_kind_ = MotionKind::None;
+            } else {
+                last_kind_ = previous_kind_;
+            }
+        }
+        previous_kind_ = MotionKind::None;
+        owner_finished_cv_.notify_all();
+    }
+
+    void failMotion(const MotionToken& token)
+    {
+        std::lock_guard lock(mutex_);
+        if (!owner_active_ ||
+            active_token_.generation != token.generation) {
+            return;
+        }
+        owner_active_ = false;
+        active_token_ = {};
+        last_kind_ = token.kind;
+        previous_kind_ = MotionKind::None;
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+    }
+
+    StopRequest beginStop(
+        const MotionKind requested_kind = MotionKind::None)
+    {
+        std::lock_guard lock(mutex_);
+        if (stop_in_progress_) {
+            return {};
+        }
+
+        stop_in_progress_ = true;
+        const MotionToken active = owner_active_
+            ? active_token_
+            : MotionToken{};
+        // A successful speedJoint/speedLine call may keep the controller in
+        // velocity mode after the SDK function returns, even when the target
+        // velocity is zero and the robot currently reports steady. Retain that
+        // motion kind until a typed stop has been acknowledged.
+        const bool tracked_motion =
+            active.valid() || last_kind_ != MotionKind::None;
+        if (active.valid()) {
+            cancelled_generation_ = std::max(
+                cancelled_generation_, active.generation);
+        }
+        MotionKind kind = MotionKind::None;
+        if (active.valid()) {
+            kind = active.kind;
+        } else if (last_kind_ != MotionKind::None) {
+            kind = last_kind_;
+        } else if (requested_kind != MotionKind::None) {
+            kind = requested_kind;
+        } else {
+            kind = last_kind_;
+        }
+        if (kind != MotionKind::None) {
+            last_kind_ = kind;
+        }
+        return {
+            StopStartStatus::Started,
+            kind,
+            active,
+            tracked_motion};
+    }
+
+    SafetyCancelResult cancelActiveForSafety()
+    {
+        std::lock_guard lock(mutex_);
+        const MotionToken active = owner_active_
+            ? active_token_
+            : MotionToken{};
+        if (active.valid()) {
+            cancelled_generation_ = std::max(
+                cancelled_generation_, active.generation);
+        }
+
+        const MotionKind kind = active.valid()
+            ? active.kind
+            : last_kind_;
+        if (kind != MotionKind::None) {
+            last_kind_ = kind;
+        }
+        // This block is intentionally independent of stop_in_progress_. The
+        // monitor may observe the safety event while a software Stop owns the
+        // stop transaction; either way no new motion may enter.
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+        return {kind, active, active.valid() || kind != MotionKind::None};
+    }
+
+    bool cancelled(const MotionToken& token) const
+    {
+        std::lock_guard lock(mutex_);
+        return token.valid() &&
+               token.generation <= cancelled_generation_;
+    }
+
+    bool waitForOwnerExit(
+        const MotionToken& token,
+        const std::chrono::milliseconds timeout)
+    {
+        if (!token.valid()) {
+            return true;
+        }
+        std::unique_lock lock(mutex_);
+        return owner_finished_cv_.wait_for(
+            lock,
+            timeout,
+            [this, &token]() {
+                return !owner_active_ ||
+                       active_token_.generation != token.generation;
+            });
+    }
+
+    bool ownerActive(const MotionToken& token) const
+    {
+        if (!token.valid()) {
+            return false;
+        }
+        std::lock_guard lock(mutex_);
+        return owner_active_ &&
+               active_token_.generation == token.generation;
+    }
+
+    bool completeStop()
+    {
+        std::lock_guard lock(mutex_);
+        if (owner_active_) {
+            return false;
+        }
+        stop_in_progress_ = false;
+        blocked_ = false;
+        active_token_ = {};
+        last_kind_ = MotionKind::None;
+        previous_kind_ = MotionKind::None;
+        owner_finished_cv_.notify_all();
+        return true;
+    }
+
+    void failStop()
+    {
+        std::lock_guard lock(mutex_);
+        stop_in_progress_ = false;
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+    }
+
+    bool busy() const
+    {
+        std::lock_guard lock(mutex_);
+        return owner_active_ || stop_in_progress_ || blocked_ ||
+               last_kind_ != MotionKind::None;
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::condition_variable owner_finished_cv_;
+    std::uint64_t next_generation_{0};
+    std::uint64_t cancelled_generation_{0};
+    MotionToken active_token_;
+    MotionKind last_kind_{MotionKind::None};
+    MotionKind previous_kind_{MotionKind::None};
+    bool owner_active_{false};
+    bool stop_in_progress_{false};
+    bool blocked_{false};
+};
+
+} // namespace cmvr::device::aubo_internal
+
+#endif // CMVR_ES_AUBO_MOTION_STATE_H

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

@@ -0,0 +1,184 @@
+#ifndef CMVR_ES_AUBO_SAFETY_STATE_H
+#define CMVR_ES_AUBO_SAFETY_STATE_H
+
+#include <cstdint>
+#include <mutex>
+#include <optional>
+
+namespace cmvr::device::aubo_internal {
+
+// This is deliberately richer than RobotArm::SafetyMode. Recovery and
+// Violation have no lossless public mapping, but both must remain fail-closed.
+enum class SafetyCondition {
+    Unknown,
+    Normal,
+    Reduced,
+    Recovery,
+    Violation,
+    ProtectiveStop,
+    SafeguardStop,
+    SystemEmergencyStop,
+    RobotEmergencyStop,
+    Fault,
+};
+
+inline bool isMotionSafe(const SafetyCondition condition) noexcept
+{
+    return condition == SafetyCondition::Normal ||
+           condition == SafetyCondition::Reduced;
+}
+
+inline SafetyCondition effectiveSafetyCondition(
+    const SafetyCondition reported_condition,
+    const int robot_emergency_stop_source) noexcept
+{
+    if (robot_emergency_stop_source < 0) {
+        return SafetyCondition::Unknown;
+    }
+    if (robot_emergency_stop_source != 0) {
+        return SafetyCondition::RobotEmergencyStop;
+    }
+    return reported_condition;
+}
+
+inline bool needsProtectiveUnlock(
+    const SafetyCondition condition) noexcept
+{
+    return condition == SafetyCondition::ProtectiveStop ||
+           condition == SafetyCondition::Violation;
+}
+
+inline bool needsInterfaceBoardRestart(
+    const SafetyCondition condition) noexcept
+{
+    return condition == SafetyCondition::SystemEmergencyStop ||
+           condition == SafetyCondition::RobotEmergencyStop ||
+           condition == SafetyCondition::Fault;
+}
+
+struct SafetyPermit {
+    std::uint64_t epoch{0};
+
+    bool valid() const noexcept { return epoch != 0; }
+};
+
+struct RecoveryToken {
+    std::uint64_t epoch{0};
+
+    bool valid() const noexcept { return epoch != 0; }
+};
+
+struct SafetySnapshot {
+    SafetyCondition observed{SafetyCondition::Unknown};
+    SafetyCondition latched_reason{SafetyCondition::Unknown};
+    std::uint64_t epoch{0};
+    bool latched{false};
+    bool recovery_in_progress{false};
+};
+
+// Hardware safety is an event, not a level. Once an unsafe state has been
+// observed, returning to Normal only changes the observed level. A separate,
+// explicit recovery must prove that the old controller operation has been
+// cancelled before new motion permits can be issued.
+class SafetyState final {
+public:
+    SafetyState() = default;
+
+    void observe(const SafetyCondition condition)
+    {
+        std::lock_guard lock(mutex_);
+        const bool changed = observed_ != condition;
+        observed_ = condition;
+        if (isMotionSafe(condition)) {
+            return;
+        }
+
+        if (!latched_ || recovery_in_progress_ || changed) {
+            ++epoch_;
+        }
+        latched_ = true;
+        recovery_in_progress_ = false;
+        latched_reason_ = condition;
+    }
+
+    std::optional<SafetyPermit> tryPermit() const
+    {
+        std::lock_guard lock(mutex_);
+        if (latched_ || !isMotionSafe(observed_)) {
+            return std::nullopt;
+        }
+        return SafetyPermit{epoch_};
+    }
+
+    bool validate(const SafetyPermit permit) const
+    {
+        std::lock_guard lock(mutex_);
+        return permit.valid() && permit.epoch == epoch_ && !latched_ &&
+               isMotionSafe(observed_);
+    }
+
+    std::optional<RecoveryToken> beginRecovery(
+        const std::uint64_t expected_epoch)
+    {
+        std::lock_guard lock(mutex_);
+        if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ ||
+            recovery_in_progress_ ||
+            !isMotionSafe(observed_)) {
+            return std::nullopt;
+        }
+        recovery_in_progress_ = true;
+        return RecoveryToken{epoch_};
+    }
+
+    bool completeRecovery(
+        const RecoveryToken token,
+        const bool robot_running,
+        const bool controller_idle,
+        const bool cancellation_confirmed)
+    {
+        std::lock_guard lock(mutex_);
+        if (!token.valid() || token.epoch != epoch_ || !latched_ ||
+            !recovery_in_progress_ || !isMotionSafe(observed_) ||
+            !robot_running || !controller_idle ||
+            !cancellation_confirmed) {
+            return false;
+        }
+
+        latched_ = false;
+        recovery_in_progress_ = false;
+        latched_reason_ = SafetyCondition::Unknown;
+        ++epoch_;
+        return true;
+    }
+
+    void failRecovery(const RecoveryToken token)
+    {
+        std::lock_guard lock(mutex_);
+        if (token.valid() && token.epoch == epoch_) {
+            recovery_in_progress_ = false;
+        }
+    }
+
+    SafetySnapshot snapshot() const
+    {
+        std::lock_guard lock(mutex_);
+        return {
+            observed_,
+            latched_reason_,
+            epoch_,
+            latched_,
+            recovery_in_progress_};
+    }
+
+private:
+    mutable std::mutex mutex_;
+    SafetyCondition observed_{SafetyCondition::Unknown};
+    SafetyCondition latched_reason_{SafetyCondition::Unknown};
+    std::uint64_t epoch_{1};
+    bool latched_{false};
+    bool recovery_in_progress_{false};
+};
+
+} // namespace cmvr::device::aubo_internal
+
+#endif // CMVR_ES_AUBO_SAFETY_STATE_H

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

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

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

@@ -0,0 +1,83 @@
+#include "devices/arm/aubo_arm/aubo_motion_result.h"
+
+#include <iostream>
+
+namespace {
+
+#define CHECK_TRUE(condition)                                                        \
+    do {                                                                             \
+        if (!(condition)) {                                                          \
+            std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": "           \
+                      << #condition << std::endl;                                     \
+            return 1;                                                                \
+        }                                                                            \
+    } while (false)
+
+} // namespace
+
+int main()
+{
+    using cmvr::device::aubo_internal::MotionCommandOutcome;
+    using cmvr::device::aubo_internal::MotionWaitResult;
+    using cmvr::device::aubo_internal::resolveMotionCommand;
+
+    constexpr int success_code = 0;
+    constexpr int request_ignore_code = 13;
+
+    int wait_calls = 0;
+    const auto wait_succeeded = [&wait_calls]() {
+        ++wait_calls;
+        return MotionWaitResult::Completed;
+    };
+    CHECK_TRUE(resolveMotionCommand(
+                   success_code,
+                   success_code,
+                   request_ignore_code,
+                   wait_succeeded) == MotionCommandOutcome::CompletedAfterMotion);
+    CHECK_TRUE(wait_calls == 1);
+
+    wait_calls = 0;
+    CHECK_TRUE(resolveMotionCommand(
+                   request_ignore_code,
+                   success_code,
+                   request_ignore_code,
+                   wait_succeeded) == MotionCommandOutcome::CompletedWithoutMotion);
+    CHECK_TRUE(wait_calls == 0);
+
+    const int submit_failures[] = {1, 2, 3, -request_ignore_code};
+    for (const int return_code : submit_failures) {
+        wait_calls = 0;
+        CHECK_TRUE(resolveMotionCommand(
+                       return_code,
+                       success_code,
+                       request_ignore_code,
+                       wait_succeeded) == MotionCommandOutcome::SubmitFailed);
+        CHECK_TRUE(wait_calls == 0);
+    }
+
+    wait_calls = 0;
+    const auto wait_failed = [&wait_calls]() {
+        ++wait_calls;
+        return MotionWaitResult::Failed;
+    };
+    CHECK_TRUE(resolveMotionCommand(
+                   success_code,
+                   success_code,
+                   request_ignore_code,
+                   wait_failed) == MotionCommandOutcome::CompletionFailed);
+    CHECK_TRUE(wait_calls == 1);
+
+    wait_calls = 0;
+    const auto wait_cancelled = [&wait_calls]() {
+        ++wait_calls;
+        return MotionWaitResult::Cancelled;
+    };
+    CHECK_TRUE(resolveMotionCommand(
+                   success_code,
+                   success_code,
+                   request_ignore_code,
+                   wait_cancelled) == MotionCommandOutcome::Cancelled);
+    CHECK_TRUE(wait_calls == 1);
+
+    return 0;
+}

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

@@ -0,0 +1,156 @@
+#include "devices/arm/aubo_arm/aubo_motion_state.h"
+
+#include <chrono>
+#include <iostream>
+
+namespace {
+
+#define CHECK_TRUE(condition)                                                        \
+    do {                                                                             \
+        if (!(condition)) {                                                          \
+            std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": "           \
+                      << #condition << std::endl;                                     \
+            return 1;                                                                \
+        }                                                                            \
+    } while (false)
+
+} // namespace
+
+int main()
+{
+    using namespace cmvr::device::aubo_internal;
+
+    MotionState state;
+    CHECK_TRUE(state.begin(MotionKind::None).status ==
+               MotionStartStatus::Invalid);
+    const auto joint = state.begin(MotionKind::Joint);
+    CHECK_TRUE(joint.started());
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Busy);
+
+    const auto stop_joint = state.beginStop();
+    CHECK_TRUE(stop_joint.started());
+    CHECK_TRUE(stop_joint.kind == MotionKind::Joint);
+    CHECK_TRUE(stop_joint.active_token.generation ==
+               joint.token.generation);
+    CHECK_TRUE(stop_joint.tracked_motion);
+    CHECK_TRUE(state.cancelled(joint.token));
+    CHECK_TRUE(state.beginStop().status ==
+               StopStartStatus::AlreadyStopping);
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Stopping);
+    CHECK_TRUE(!state.waitForOwnerExit(
+        joint.token, std::chrono::milliseconds(1)));
+    CHECK_TRUE(!state.completeStop());
+
+    state.finish(joint.token);
+    CHECK_TRUE(state.waitForOwnerExit(
+        joint.token, std::chrono::milliseconds(1)));
+    CHECK_TRUE(state.completeStop());
+    CHECK_TRUE(!state.busy());
+
+    const auto linear = state.begin(MotionKind::Linear);
+    CHECK_TRUE(linear.started());
+    CHECK_TRUE(!state.cancelled(linear.token));
+    // A delayed guard from the cancelled command must not release a newer one.
+    state.finish(joint.token);
+    CHECK_TRUE(state.busy());
+    state.finish(linear.token, MotionFinishMode::Clear);
+    CHECK_TRUE(!state.busy());
+
+    const auto speed_joint = state.begin(MotionKind::Joint);
+    CHECK_TRUE(speed_joint.started());
+    state.finish(speed_joint.token, MotionFinishMode::Retain);
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Busy);
+    const auto rejected_speed_update =
+        state.begin(MotionKind::Joint, true);
+    CHECK_TRUE(rejected_speed_update.started());
+    state.finish(
+        rejected_speed_update.token,
+        MotionFinishMode::RestorePrevious);
+    const auto stop_speed = state.beginStop();
+    CHECK_TRUE(stop_speed.started());
+    CHECK_TRUE(stop_speed.kind == MotionKind::Joint);
+    CHECK_TRUE(stop_speed.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+    CHECK_TRUE(!state.busy());
+
+    const auto idle_stop = state.beginStop();
+    CHECK_TRUE(idle_stop.kind == MotionKind::None);
+    CHECK_TRUE(!idle_stop.tracked_motion);
+    state.failStop();
+    const auto retry_idle_stop = state.beginStop();
+    CHECK_TRUE(retry_idle_stop.kind == MotionKind::None);
+    CHECK_TRUE(!retry_idle_stop.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+
+    const auto mismatched_stop_motion = state.begin(MotionKind::Linear);
+    CHECK_TRUE(mismatched_stop_motion.started());
+    const auto mismatched_stop = state.beginStop(MotionKind::Joint);
+    CHECK_TRUE(mismatched_stop.kind == MotionKind::Linear);
+    state.finish(mismatched_stop_motion.token);
+    CHECK_TRUE(state.completeStop());
+
+    const auto uncertain_motion = state.begin(MotionKind::Joint);
+    CHECK_TRUE(uncertain_motion.started());
+    state.failMotion(uncertain_motion.token);
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Blocked);
+    const auto stop_uncertain = state.beginStop();
+    CHECK_TRUE(stop_uncertain.kind == MotionKind::Joint);
+    CHECK_TRUE(stop_uncertain.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+
+    const auto failed_stop_motion = state.begin(MotionKind::Linear);
+    CHECK_TRUE(failed_stop_motion.started());
+    const auto failed_stop = state.beginStop();
+    CHECK_TRUE(failed_stop.kind == MotionKind::Linear);
+    state.failStop();
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Blocked);
+    state.finish(failed_stop_motion.token);
+
+    const auto retry = state.beginStop();
+    CHECK_TRUE(retry.started());
+    CHECK_TRUE(retry.kind == MotionKind::Linear);
+    CHECK_TRUE(state.completeStop());
+    const auto recovered = state.begin(MotionKind::Joint);
+    CHECK_TRUE(recovered.started());
+    state.finish(recovered.token, MotionFinishMode::Clear);
+
+    const auto safety_motion = state.begin(MotionKind::Linear);
+    CHECK_TRUE(safety_motion.started());
+    const auto safety_cancel = state.cancelActiveForSafety();
+    CHECK_TRUE(safety_cancel.kind == MotionKind::Linear);
+    CHECK_TRUE(safety_cancel.tracked_motion);
+    CHECK_TRUE(state.cancelled(safety_motion.token));
+    CHECK_TRUE(state.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Blocked);
+    state.finish(safety_motion.token);
+    const auto safety_stop = state.beginStop();
+    CHECK_TRUE(safety_stop.started());
+    CHECK_TRUE(safety_stop.kind == MotionKind::Linear);
+    CHECK_TRUE(state.completeStop());
+
+    const auto retained_speed = state.begin(MotionKind::Joint);
+    CHECK_TRUE(retained_speed.started());
+    state.finish(retained_speed.token, MotionFinishMode::Retain);
+    const auto retained_cancel = state.cancelActiveForSafety();
+    CHECK_TRUE(retained_cancel.kind == MotionKind::Joint);
+    CHECK_TRUE(retained_cancel.tracked_motion);
+    CHECK_TRUE(!retained_cancel.active_token.valid());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Blocked);
+    const auto retained_stop = state.beginStop();
+    CHECK_TRUE(retained_stop.started());
+    CHECK_TRUE(retained_stop.kind == MotionKind::Joint);
+    CHECK_TRUE(retained_stop.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+
+    return 0;
+}

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

@@ -0,0 +1,88 @@
+#include "devices/arm/aubo_arm/aubo_safety_state.h"
+
+#include <iostream>
+
+namespace {
+
+#define CHECK_TRUE(condition)                                                        \
+    do {                                                                             \
+        if (!(condition)) {                                                          \
+            std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": "           \
+                      << #condition << std::endl;                                     \
+            return 1;                                                                \
+        }                                                                            \
+    } while (false)
+
+} // namespace
+
+int main()
+{
+    using namespace cmvr::device::aubo_internal;
+
+    SafetyState state;
+    CHECK_TRUE(!state.tryPermit().has_value());
+
+    state.observe(SafetyCondition::Normal);
+    const auto initial_permit = state.tryPermit();
+    CHECK_TRUE(initial_permit.has_value());
+    CHECK_TRUE(state.validate(*initial_permit));
+
+    CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, 1) ==
+               SafetyCondition::RobotEmergencyStop);
+    CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, -1) ==
+               SafetyCondition::Unknown);
+    CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Reduced, 0) ==
+               SafetyCondition::Reduced);
+    CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::ProtectiveStop));
+    CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::Violation));
+    CHECK_TRUE(!needsProtectiveUnlock(SafetyCondition::SafeguardStop));
+    CHECK_TRUE(needsInterfaceBoardRestart(
+        SafetyCondition::RobotEmergencyStop));
+    CHECK_TRUE(needsInterfaceBoardRestart(
+        SafetyCondition::SystemEmergencyStop));
+    CHECK_TRUE(needsInterfaceBoardRestart(SafetyCondition::Fault));
+    CHECK_TRUE(!needsInterfaceBoardRestart(SafetyCondition::Recovery));
+
+    state.observe(SafetyCondition::RobotEmergencyStop);
+    CHECK_TRUE(!state.validate(*initial_permit));
+    CHECK_TRUE(state.snapshot().latched);
+    CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value());
+
+    // Releasing the hardware switch must not unlock motion by itself.
+    state.observe(SafetyCondition::Normal);
+    CHECK_TRUE(state.snapshot().latched);
+    CHECK_TRUE(!state.tryPermit().has_value());
+
+    const auto recovery = state.beginRecovery(state.snapshot().epoch);
+    CHECK_TRUE(recovery.has_value());
+    CHECK_TRUE(!state.completeRecovery(*recovery, true, true, false));
+    state.failRecovery(*recovery);
+
+    const auto retry = state.beginRecovery(state.snapshot().epoch);
+    CHECK_TRUE(retry.has_value());
+    CHECK_TRUE(state.completeRecovery(*retry, true, true, true));
+    const auto recovered_permit = state.tryPermit();
+    CHECK_TRUE(recovered_permit.has_value());
+    CHECK_TRUE(state.validate(*recovered_permit));
+
+    // A new safety event invalidates an in-flight recovery token.
+    state.observe(SafetyCondition::ProtectiveStop);
+    state.observe(SafetyCondition::Reduced);
+    const auto stale_recovery = state.beginRecovery(
+        state.snapshot().epoch);
+    CHECK_TRUE(stale_recovery.has_value());
+    state.observe(SafetyCondition::SafeguardStop);
+    state.observe(SafetyCondition::Normal);
+    CHECK_TRUE(!state.completeRecovery(
+        *stale_recovery, true, true, true));
+    CHECK_TRUE(state.snapshot().latched);
+
+    // An old API call must not begin recovery for a newer safety event.
+    const auto stale_epoch = state.snapshot().epoch;
+    state.observe(SafetyCondition::RobotEmergencyStop);
+    state.observe(SafetyCondition::Normal);
+    CHECK_TRUE(!state.beginRecovery(stale_epoch).has_value());
+    CHECK_TRUE(!state.snapshot().recovery_in_progress);
+
+    return 0;
+}

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

@@ -1,5 +1,7 @@
 add_library(huayan_arm SHARED huayan_arm.cpp)
 
+find_package(Threads REQUIRED)
+
 set(HUAYAN_ARM_SDK_DIR ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/huayan_arm/v1.0)
 
 target_include_directories(huayan_arm
@@ -20,9 +22,56 @@ target_link_libraries(huayan_arm
         PRIVATE
         HR_Pro
         glog
+        Threads::Threads
 )
 
 add_library(cmvr_es::device::huayan_arm ALIAS huayan_arm)
 
 install(TARGETS huayan_arm LIBRARY DESTINATION lib)
 install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(huayan_lifecycle_state_test
+            tests/huayan_lifecycle_state_test.cpp
+    )
+    target_include_directories(huayan_lifecycle_state_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(huayan_lifecycle_state_test
+            PRIVATE
+            Threads::Threads
+    )
+    add_test(
+            NAME huayan_lifecycle_state_test
+            COMMAND huayan_lifecycle_state_test
+    )
+    set_tests_properties(huayan_lifecycle_state_test PROPERTIES TIMEOUT 10)
+
+    if(UNIX AND NOT APPLE)
+        add_executable(huayan_arm_sdk_test
+                tests/huayan_arm_sdk_test.cpp
+        )
+        target_include_directories(huayan_arm_sdk_test
+                PRIVATE
+                ${CMAKE_SOURCE_DIR}/cmvr-es
+                ${HUAYAN_ARM_SDK_DIR}/include
+        )
+        target_link_libraries(huayan_arm_sdk_test
+                PRIVATE
+                cmvr_es::device::huayan_arm
+                Threads::Threads
+        )
+        # Export the fake HRIF_* definitions so libhuayan_arm resolves its SDK
+        # calls to the deterministic test controller instead of real hardware.
+        target_link_options(huayan_arm_sdk_test PRIVATE -Wl,--export-dynamic)
+        add_test(
+                NAME huayan_arm_sdk_test
+                COMMAND huayan_arm_sdk_test
+        )
+        set_tests_properties(huayan_arm_sdk_test PROPERTIES
+                TIMEOUT 20
+                ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+        )
+    endif()
+endif()

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

@@ -9,12 +9,18 @@
 #define CMVR_ES_HUAYAN_ROBOT_H
 
 #include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <functional>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include "cmvr/config/arm_config/arm_config.pb.h"
+#include "devices/arm/huayan_arm/huayan_lifecycle_state.h"
 #include "devices/arm/robot_arm.h"
 
 namespace cmvr::device {
@@ -35,15 +41,24 @@ public:
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override;
-    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
+    ControlMode getControlMode() const override;
+    bool supportsActionQueueMotion() const noexcept override { return true; }
 
     Result torqueOn() override;
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override { return emergencyStop(); }
+    Result protectiveStop() override;
+    Result recoverProtectiveStop(
+        const JointTrajectory&,
+        const MotionOptions&) override
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "protective recovery is not implemented for HuayanRobot");
+    }
     Result setSpeedScaling(double scaling) override;
-    double getSpeedScaling() const override { return speed_scaling_; }
+    double getSpeedScaling() const override { return speed_scaling_.load(); }
     bool isProtectiveStopped() const override;
     bool isEmergencyStopped() const override;
     bool isFault() const override;
@@ -71,7 +86,7 @@ public:
     Result brakeRelease() override { return torqueOn(); }
     Result shutdown() override;
     Result clearFault() override;
-    Result unlockProtectiveStop() override { return clearFault(); }
+    Result unlockProtectiveStop() override;
     Result loadProgram(const std::string& program_name) override;
     Result playProgram() override;
     Result pauseProgram() override;
@@ -84,7 +99,7 @@ public:
     CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
     CartesianPose fk(bool is_tcp = true) override;
     CartesianVelocity getSpeedLCommandTwistBase() const override;
-    bool busy() const override { return busy_.load(); }
+    bool busy() const override;
 
 private:
     struct HrState {
@@ -101,21 +116,69 @@ private:
         int connected_to_box{0};
         int blending_done{0};
         int in_pos{0};
+        int emergency_signal_fault{0};
+        int emergency_input{0};
+        int safeguard_signal_fault{0};
+        int safeguard_input{0};
         bool valid{false};
     };
 
+    struct RuntimeState;
+
     Result ensureConnected_(const std::string& context) const;
+    Result ensureMotionReady_(
+        const std::string& context,
+        const std::shared_ptr<RuntimeState>& runtime,
+        huayan_internal::SafetyPermit& permit) const;
     Result unsupported_(const std::string& name) const;
     Result hrResult_(int code, const std::string& context) const;
+    Result motionStartFailure_(
+        const std::string& context,
+        huayan_internal::MotionStartStatus status) const;
     bool validDof_(std::size_t size, std::string& error) const;
     HrState readHrState_() const;
+    HrState sampleHrState_(
+        const std::shared_ptr<RuntimeState>& runtime) const;
+    void publishHrState_(
+        const std::shared_ptr<RuntimeState>& runtime,
+        const HrState& state) const;
     std::vector<double> readJointPositionRad_() const;
     std::vector<double> readJointVelocityRad_() const;
     CartesianPose readTcpPose_() const;
     CartesianVelocity readTcpVelocity_() const;
+    bool readJointPositionSample_(std::vector<double>& values) const;
+    bool readJointVelocitySample_(std::vector<double>& values) const;
+    bool readTcpPoseSample_(CartesianPose& pose) const;
     std::vector<double> currentJointPositionDeg_() const;
     std::string nextCommandId_() const;
-    Result waitMotionDone_(const std::string& context, int timeout_ms) const;
+    Result waitMotionDone_(
+        const std::string& context,
+        const std::shared_ptr<RuntimeState>& runtime,
+        huayan_internal::MotionToken motion_token,
+        huayan_internal::SafetyPermit safety_permit,
+        const std::string& command_id,
+        const std::vector<double>* joint_target,
+        const CartesianPose* tcp_target,
+        int timeout_ms,
+        const std::function<bool()>& cancellation_requested = {}) const;
+    bool targetReached_(
+        const std::vector<double>* joint_target,
+        const CartesianPose* tcp_target) const;
+    bool controllerIdleStable_(
+        const std::shared_ptr<RuntimeState>& runtime,
+        std::chrono::milliseconds timeout) const;
+    bool terminateController_(
+        const std::shared_ptr<RuntimeState>& runtime,
+        std::chrono::milliseconds timeout,
+        bool stop_program) const;
+    Result completeSafetyRecovery_(
+        const std::string& context,
+        const std::shared_ptr<RuntimeState>& runtime,
+        std::uint64_t expected_epoch,
+        bool enable_robot,
+        bool release_software_guard);
+    std::shared_ptr<RuntimeState> runtimeSnapshot_() const;
+    void safetyMonitorLoop_(const std::shared_ptr<RuntimeState>& runtime);
 
 private:
     config::RobotArmConfig cfg_;
@@ -127,12 +190,16 @@ private:
     unsigned int robot_id_{0};
     std::string tcp_name_{"TCP"};
     std::string ucs_name_{"Base"};
-    double speed_scaling_{1.0};
+    std::atomic<double> speed_scaling_{1.0};
     std::atomic<bool> connected_{false};
-    std::atomic<bool> busy_{false};
+    mutable std::atomic<bool> servo_mode_{false};
-    std::atomic<bool> servo_mode_{false};
+    std::atomic<bool> software_emergency_stopped_{false};
+    std::atomic<bool> software_protective_stopped_{false};
     mutable std::mutex mutex_;
+    mutable std::recursive_mutex sdk_mutex_;
     mutable std::atomic<unsigned long long> command_seq_{0};
+    std::shared_ptr<RuntimeState> runtime_;
+    std::thread safety_monitor_thread_;
 };
 
 } // namespace cmvr::device

cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h

@@ -0,0 +1,504 @@
+#ifndef CMVR_ES_HUAYAN_LIFECYCLE_STATE_H
+#define CMVR_ES_HUAYAN_LIFECYCLE_STATE_H
+
+#include <algorithm>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <mutex>
+#include <optional>
+
+namespace cmvr::device::huayan_internal {
+
+enum class MotionKind {
+    None,
+    Joint,
+    Linear,
+    SpeedJoint,
+    SpeedLinear,
+    Servo,
+    Program,
+};
+
+struct MotionToken {
+    std::uint64_t generation{0};
+    MotionKind kind{MotionKind::None};
+
+    bool valid() const noexcept
+    {
+        return generation != 0 && kind != MotionKind::None;
+    }
+};
+
+enum class MotionStartStatus {
+    Started,
+    Invalid,
+    Busy,
+    Stopping,
+    Blocked,
+};
+
+struct MotionStartResult {
+    MotionStartStatus status{MotionStartStatus::Busy};
+    MotionToken token;
+
+    bool started() const noexcept
+    {
+        return status == MotionStartStatus::Started;
+    }
+};
+
+enum class MotionFinishMode {
+    RestorePrevious,
+    Clear,
+    Retain,
+};
+
+enum class StopStartStatus {
+    Started,
+    AlreadyStopping,
+};
+
+struct StopRequest {
+    StopStartStatus status{StopStartStatus::AlreadyStopping};
+    MotionKind kind{MotionKind::None};
+    MotionToken active_token;
+    bool tracked_motion{false};
+
+    bool started() const noexcept
+    {
+        return status == StopStartStatus::Started;
+    }
+};
+
+struct SafetyCancelResult {
+    MotionKind kind{MotionKind::None};
+    MotionToken active_token;
+    bool tracked_motion{false};
+};
+
+struct MotionSnapshot {
+    MotionKind active_kind{MotionKind::None};
+    MotionKind retained_kind{MotionKind::None};
+    std::uint64_t active_generation{0};
+    bool owner_active{false};
+    bool stop_in_progress{false};
+    bool blocked{false};
+};
+
+// Tracks a single Huayan controller operation owner. Generation tokens make
+// completion from an older RPC harmless after Stop or a safety event has
+// cancelled it. Servo and program operations may retain their kind after the
+// submitting RPC returns; begin(kind, true) supports same-kind updates while
+// that retained controller mode remains active.
+class MotionState final {
+public:
+    MotionStartResult begin(
+        const MotionKind kind,
+        const bool replace_retained_same_kind = false)
+    {
+        std::lock_guard lock(mutex_);
+        if (kind == MotionKind::None) {
+            return {MotionStartStatus::Invalid, {}};
+        }
+        if (stop_in_progress_) {
+            return {MotionStartStatus::Stopping, {}};
+        }
+        if (blocked_) {
+            return {MotionStartStatus::Blocked, {}};
+        }
+        if (owner_active_) {
+            return {MotionStartStatus::Busy, {}};
+        }
+        if (retained_kind_ != MotionKind::None &&
+            (!replace_retained_same_kind || retained_kind_ != kind)) {
+            return {MotionStartStatus::Busy, {}};
+        }
+
+        const MotionToken token{++next_generation_, kind};
+        owner_active_ = true;
+        active_token_ = token;
+        previous_kind_ = retained_kind_;
+        return {MotionStartStatus::Started, token};
+    }
+
+    void finish(
+        const MotionToken token,
+        const MotionFinishMode mode = MotionFinishMode::RestorePrevious)
+    {
+        std::lock_guard lock(mutex_);
+        if (!owner_active_ ||
+            active_token_.generation != token.generation) {
+            return;
+        }
+
+        owner_active_ = false;
+        active_token_ = {};
+        if (!stop_in_progress_ && !blocked_) {
+            switch (mode) {
+            case MotionFinishMode::RestorePrevious:
+                retained_kind_ = previous_kind_;
+                break;
+            case MotionFinishMode::Clear:
+                retained_kind_ = MotionKind::None;
+                break;
+            case MotionFinishMode::Retain:
+                retained_kind_ = token.kind;
+                break;
+            }
+        }
+        previous_kind_ = MotionKind::None;
+        owner_finished_cv_.notify_all();
+    }
+
+    void failMotion(const MotionToken token)
+    {
+        std::lock_guard lock(mutex_);
+        if (!owner_active_ ||
+            active_token_.generation != token.generation) {
+            return;
+        }
+
+        owner_active_ = false;
+        active_token_ = {};
+        retained_kind_ = token.kind;
+        previous_kind_ = MotionKind::None;
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+    }
+
+    StopRequest beginStop(
+        const MotionKind requested_kind = MotionKind::None)
+    {
+        std::lock_guard lock(mutex_);
+        if (stop_in_progress_) {
+            return {};
+        }
+
+        stop_in_progress_ = true;
+        const MotionToken active = owner_active_
+            ? active_token_
+            : MotionToken{};
+        if (active.valid()) {
+            cancelled_generation_ = std::max(
+                cancelled_generation_, active.generation);
+        }
+
+        MotionKind kind = MotionKind::None;
+        if (active.valid()) {
+            kind = active.kind;
+        } else if (retained_kind_ != MotionKind::None) {
+            kind = retained_kind_;
+        } else {
+            kind = requested_kind;
+        }
+        if (kind != MotionKind::None) {
+            retained_kind_ = kind;
+        }
+
+        return {
+            StopStartStatus::Started,
+            kind,
+            active,
+            active.valid() || kind != MotionKind::None};
+    }
+
+    SafetyCancelResult cancelActiveForSafety()
+    {
+        std::lock_guard lock(mutex_);
+        const MotionToken active = owner_active_
+            ? active_token_
+            : MotionToken{};
+        if (active.valid()) {
+            cancelled_generation_ = std::max(
+                cancelled_generation_, active.generation);
+        }
+
+        const MotionKind kind = active.valid()
+            ? active.kind
+            : retained_kind_;
+        if (kind != MotionKind::None) {
+            retained_kind_ = kind;
+        }
+        // A hardware safety transition is independent of a concurrent
+        // software Stop. New controller operations remain rejected until
+        // termination is positively confirmed.
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+        return {kind, active, active.valid() || kind != MotionKind::None};
+    }
+
+    bool cancelled(const MotionToken token) const
+    {
+        std::lock_guard lock(mutex_);
+        return token.valid() &&
+               token.generation <= cancelled_generation_;
+    }
+
+    bool waitForOwnerExit(
+        const MotionToken token,
+        const std::chrono::milliseconds timeout)
+    {
+        if (!token.valid()) {
+            return true;
+        }
+        std::unique_lock lock(mutex_);
+        return owner_finished_cv_.wait_for(
+            lock,
+            timeout,
+            [this, token]() {
+                return !owner_active_ ||
+                       active_token_.generation != token.generation;
+            });
+    }
+
+    bool ownerActive(const MotionToken token) const
+    {
+        if (!token.valid()) {
+            return false;
+        }
+        std::lock_guard lock(mutex_);
+        return owner_active_ &&
+               active_token_.generation == token.generation;
+    }
+
+    bool completeStop()
+    {
+        std::lock_guard lock(mutex_);
+        if (owner_active_) {
+            return false;
+        }
+        stop_in_progress_ = false;
+        blocked_ = false;
+        active_token_ = {};
+        retained_kind_ = MotionKind::None;
+        previous_kind_ = MotionKind::None;
+        owner_finished_cv_.notify_all();
+        return true;
+    }
+
+    void failStop()
+    {
+        std::lock_guard lock(mutex_);
+        stop_in_progress_ = false;
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+    }
+
+    MotionSnapshot snapshot() const
+    {
+        std::lock_guard lock(mutex_);
+        return {
+            owner_active_ ? active_token_.kind : MotionKind::None,
+            retained_kind_,
+            owner_active_ ? active_token_.generation : 0,
+            owner_active_,
+            stop_in_progress_,
+            blocked_};
+    }
+
+    bool busy() const
+    {
+        const auto state = snapshot();
+        return state.owner_active || state.stop_in_progress ||
+               state.blocked ||
+               state.retained_kind != MotionKind::None;
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::condition_variable owner_finished_cv_;
+    std::uint64_t next_generation_{0};
+    std::uint64_t cancelled_generation_{0};
+    MotionToken active_token_;
+    MotionKind retained_kind_{MotionKind::None};
+    MotionKind previous_kind_{MotionKind::None};
+    bool owner_active_{false};
+    bool stop_in_progress_{false};
+    bool blocked_{false};
+};
+
+enum class SafetyCondition {
+    Unknown,
+    Normal,
+    EmergencyStop,
+    SafeguardStop,
+    RobotFault,
+    EmergencySignalFault,
+    SafeguardSignalFault,
+    SoftwareEmergencyStop,
+    SoftwareProtectiveStop,
+};
+
+struct RawSafetyState {
+    bool valid{false};
+    int emergency_signal_fault{0};
+    int emergency_stop{0};
+    int safeguard_signal_fault{0};
+    int safeguard_stop{0};
+    int robot_fault{0};
+    bool software_emergency_stop{false};
+    bool software_protective_stop{false};
+};
+
+inline SafetyCondition classifySafetyCondition(
+    const RawSafetyState& state) noexcept
+{
+    if (!state.valid) {
+        return SafetyCondition::Unknown;
+    }
+    if (state.emergency_signal_fault != 0) {
+        return SafetyCondition::EmergencySignalFault;
+    }
+    if (state.safeguard_signal_fault != 0) {
+        return SafetyCondition::SafeguardSignalFault;
+    }
+    if (state.emergency_stop != 0) {
+        return SafetyCondition::EmergencyStop;
+    }
+    if (state.safeguard_stop != 0) {
+        return SafetyCondition::SafeguardStop;
+    }
+    if (state.robot_fault != 0) {
+        return SafetyCondition::RobotFault;
+    }
+    if (state.software_emergency_stop) {
+        return SafetyCondition::SoftwareEmergencyStop;
+    }
+    if (state.software_protective_stop) {
+        return SafetyCondition::SoftwareProtectiveStop;
+    }
+    return SafetyCondition::Normal;
+}
+
+inline bool isMotionSafe(const SafetyCondition condition) noexcept
+{
+    return condition == SafetyCondition::Normal;
+}
+
+struct SafetyPermit {
+    std::uint64_t epoch{0};
+
+    bool valid() const noexcept { return epoch != 0; }
+};
+
+struct RecoveryToken {
+    std::uint64_t epoch{0};
+
+    bool valid() const noexcept { return epoch != 0; }
+};
+
+struct SafetySnapshot {
+    SafetyCondition observed{SafetyCondition::Unknown};
+    SafetyCondition latched_reason{SafetyCondition::Unknown};
+    std::uint64_t epoch{0};
+    bool latched{false};
+    bool recovery_in_progress{false};
+};
+
+// Safety inputs are events, not merely levels. Returning to Normal never
+// clears a prior unsafe event. Explicit recovery is tied atomically to the
+// event epoch, so a second event invalidates an older in-flight recovery.
+class SafetyState final {
+public:
+    void observe(const RawSafetyState& raw_state)
+    {
+        observe(classifySafetyCondition(raw_state));
+    }
+
+    void observe(const SafetyCondition condition)
+    {
+        std::lock_guard lock(mutex_);
+        const bool changed = observed_ != condition;
+        observed_ = condition;
+        if (isMotionSafe(condition)) {
+            return;
+        }
+
+        if (!latched_ || recovery_in_progress_ || changed) {
+            ++epoch_;
+        }
+        latched_ = true;
+        recovery_in_progress_ = false;
+        latched_reason_ = condition;
+    }
+
+    std::optional<SafetyPermit> tryPermit() const
+    {
+        std::lock_guard lock(mutex_);
+        if (latched_ || !isMotionSafe(observed_)) {
+            return std::nullopt;
+        }
+        return SafetyPermit{epoch_};
+    }
+
+    bool validate(const SafetyPermit permit) const
+    {
+        std::lock_guard lock(mutex_);
+        return permit.valid() && permit.epoch == epoch_ && !latched_ &&
+               isMotionSafe(observed_);
+    }
+
+    std::optional<RecoveryToken> beginRecovery(
+        const std::uint64_t expected_epoch)
+    {
+        std::lock_guard lock(mutex_);
+        if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ ||
+            recovery_in_progress_ || !isMotionSafe(observed_)) {
+            return std::nullopt;
+        }
+        recovery_in_progress_ = true;
+        return RecoveryToken{epoch_};
+    }
+
+    bool completeRecovery(
+        const RecoveryToken token,
+        const bool robot_ready,
+        const bool controller_idle,
+        const bool cancellation_confirmed)
+    {
+        std::lock_guard lock(mutex_);
+        if (!token.valid() || token.epoch != epoch_ || !latched_ ||
+            !recovery_in_progress_ || !isMotionSafe(observed_) ||
+            !robot_ready || !controller_idle || !cancellation_confirmed) {
+            return false;
+        }
+
+        latched_ = false;
+        recovery_in_progress_ = false;
+        latched_reason_ = SafetyCondition::Unknown;
+        ++epoch_;
+        return true;
+    }
+
+    void failRecovery(const RecoveryToken token)
+    {
+        std::lock_guard lock(mutex_);
+        if (token.valid() && token.epoch == epoch_) {
+            recovery_in_progress_ = false;
+        }
+    }
+
+    SafetySnapshot snapshot() const
+    {
+        std::lock_guard lock(mutex_);
+        return {
+            observed_,
+            latched_reason_,
+            epoch_,
+            latched_,
+            recovery_in_progress_};
+    }
+
+private:
+    mutable std::mutex mutex_;
+    SafetyCondition observed_{SafetyCondition::Unknown};
+    SafetyCondition latched_reason_{SafetyCondition::Unknown};
+    std::uint64_t epoch_{1};
+    bool latched_{false};
+    bool recovery_in_progress_{false};
+};
+
+} // namespace cmvr::device::huayan_internal
+
+#endif // CMVR_ES_HUAYAN_LIFECYCLE_STATE_H

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

@@ -0,0 +1,874 @@
+#include "devices/arm/huayan_arm/huayan_arm.h"
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <future>
+#include <iostream>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "HR_Pro.h"
+
+namespace {
+
+using Clock = std::chrono::steady_clock;
+using namespace std::chrono_literals;
+
+constexpr double kPi = 3.14159265358979323846;
+
+double radToDeg(const double value)
+{
+    return value * 180.0 / kPi;
+}
+
+struct FakeSdkState final {
+    std::mutex mutex;
+    bool connected{false};
+    bool enabled{true};
+    bool electrified{true};
+    bool robot_error{false};
+    bool paused{false};
+    bool emergency_input{false};
+    bool emergency_signal_fault{false};
+    bool safeguard_input{false};
+    bool safeguard_signal_fault{false};
+    bool software_safeguard{false};
+
+    bool motion_active{false};
+    bool motion_is_joint{true};
+    bool stop_pending{false};
+    bool hold_next_motion{false};
+    bool stale_done_once{false};
+    Clock::time_point completion_at{};
+    Clock::time_point stop_complete_at{};
+    std::array<double, 6> joint_position_deg{};
+    std::array<double, 6> joint_target_deg{};
+    std::array<double, 6> tcp_position_hr{};
+    std::array<double, 6> tcp_target_hr{};
+    std::string waypoint_id;
+
+    bool servo_started{false};
+    bool program_running{false};
+    std::string selected_program;
+
+    int move_j_calls{0};
+    int move_l_calls{0};
+    int speed_j_calls{0};
+    int speed_l_calls{0};
+    int group_stop_calls{0};
+    int group_reset_calls{0};
+    int start_servo_calls{0};
+    int stop_script_calls{0};
+    int idle_velocity_reads_after_stop{0};
+    bool count_idle_reads{false};
+
+    void refreshLocked()
+    {
+        const auto now = Clock::now();
+        const bool safety_active = emergency_input || safeguard_input ||
+            software_safeguard;
+
+        if (stop_pending && now >= stop_complete_at) {
+            stop_pending = false;
+            motion_active = false;
+            stale_done_once = false;
+            count_idle_reads = true;
+        }
+
+        if (motion_active && !stop_pending && !safety_active &&
+            completion_at != Clock::time_point{} && now >= completion_at) {
+            motion_active = false;
+            stale_done_once = false;
+            if (motion_is_joint) {
+                joint_position_deg = joint_target_deg;
+            } else {
+                tcp_position_hr = tcp_target_hr;
+            }
+        }
+    }
+
+    bool movingLocked()
+    {
+        refreshLocked();
+        return motion_active && !emergency_input && !safeguard_input &&
+            !software_safeguard;
+    }
+
+    bool doneLocked()
+    {
+        refreshLocked();
+        return !motion_active;
+    }
+
+    void startMotionLocked(const bool joint)
+    {
+        motion_active = true;
+        motion_is_joint = joint;
+        stop_pending = false;
+        count_idle_reads = false;
+        stale_done_once = true;
+        if (hold_next_motion) {
+            // The fallback deadline keeps a failed test from leaving a worker
+            // blocked for the production 60 second timeout.
+            completion_at = Clock::now() + 3s;
+            hold_next_motion = false;
+        } else {
+            completion_at = Clock::now() + 120ms;
+        }
+    }
+};
+
+FakeSdkState g_sdk;
+
+void resetFakeSdk()
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.connected = false;
+    g_sdk.enabled = true;
+    g_sdk.electrified = true;
+    g_sdk.robot_error = false;
+    g_sdk.paused = false;
+    g_sdk.emergency_input = false;
+    g_sdk.emergency_signal_fault = false;
+    g_sdk.safeguard_input = false;
+    g_sdk.safeguard_signal_fault = false;
+    g_sdk.software_safeguard = false;
+    g_sdk.motion_active = false;
+    g_sdk.motion_is_joint = true;
+    g_sdk.stop_pending = false;
+    g_sdk.hold_next_motion = false;
+    g_sdk.stale_done_once = false;
+    g_sdk.completion_at = {};
+    g_sdk.stop_complete_at = {};
+    g_sdk.joint_position_deg = {};
+    g_sdk.joint_target_deg = {};
+    g_sdk.tcp_position_hr = {};
+    g_sdk.tcp_target_hr = {};
+    g_sdk.waypoint_id.clear();
+    g_sdk.servo_started = false;
+    g_sdk.program_running = false;
+    g_sdk.selected_program.clear();
+    g_sdk.move_j_calls = 0;
+    g_sdk.move_l_calls = 0;
+    g_sdk.speed_j_calls = 0;
+    g_sdk.speed_l_calls = 0;
+    g_sdk.group_stop_calls = 0;
+    g_sdk.group_reset_calls = 0;
+    g_sdk.start_servo_calls = 0;
+    g_sdk.stop_script_calls = 0;
+    g_sdk.idle_velocity_reads_after_stop = 0;
+    g_sdk.count_idle_reads = false;
+}
+
+void holdNextMotion()
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.hold_next_motion = true;
+}
+
+void setHardwareEmergencyStop(const bool active)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.emergency_input = active;
+    // If the wrapper never sends a real group Stop, releasing the switch makes
+    // the pending fake waypoint move again. This models the field failure.
+}
+
+void setEmergencySignalFault(const bool active)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.emergency_signal_fault = active;
+}
+
+void dropFakeTransport()
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.connected = false;
+}
+
+template <typename Predicate>
+bool waitUntil(Predicate&& predicate,
+               const std::chrono::milliseconds timeout = 2s)
+{
+    const auto deadline = Clock::now() + timeout;
+    while (Clock::now() < deadline) {
+        if (predicate()) {
+            return true;
+        }
+        std::this_thread::sleep_for(10ms);
+    }
+    return predicate();
+}
+
+cmvr::config::RobotArmConfig makeConfig()
+{
+    cmvr::config::RobotArmConfig cfg;
+    cfg.set_id("huayan_fake_sdk");
+    auto* vendor = cfg.mutable_vendor();
+    vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM);
+    vendor->set_ip("127.0.0.1");
+    vendor->set_port(10003);
+    vendor->set_model("HuayanFake");
+    vendor->set_dof(6);
+    vendor->set_base_frame("Base");
+    vendor->set_tool_frame("TCP");
+    for (int i = 1; i <= 6; ++i) {
+        vendor->add_joint_names("joint_" + std::to_string(i));
+    }
+    return cfg;
+}
+
+int failures = 0;
+
+#define CHECK_TRUE(condition)                                                        \
+    do {                                                                             \
+        if (!(condition)) {                                                          \
+            std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": "           \
+                      << #condition << std::endl;                                     \
+            ++failures;                                                              \
+        }                                                                            \
+    } while (false)
+
+} // namespace
+
+// The test executable exports these strong symbols. On ELF platforms they
+// interpose the real SDK definitions used by libhuayan_arm, giving the test a
+// deterministic controller without opening a network connection.
+extern "C" {
+
+int HRIF_Connect(unsigned int, const char*, unsigned short)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.connected = true;
+    return 0;
+}
+
+int HRIF_DisConnect(unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.connected = false;
+    g_sdk.motion_active = false;
+    return 0;
+}
+
+bool HRIF_IsConnected(unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    return g_sdk.connected;
+}
+
+int HRIF_GetErrorCodeStr(unsigned int, int error_code, std::string& message)
+{
+    message = "fake SDK error " + std::to_string(error_code);
+    return 0;
+}
+
+int HRIF_GrpEnable(unsigned int, unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (g_sdk.emergency_input || g_sdk.safeguard_input ||
+        g_sdk.software_safeguard) {
+        return 101;
+    }
+    g_sdk.enabled = true;
+    g_sdk.electrified = true;
+    return 0;
+}
+
+int HRIF_GrpDisable(unsigned int, unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.enabled = false;
+    g_sdk.electrified = false;
+    return 0;
+}
+
+int HRIF_GrpReset(unsigned int, unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.group_reset_calls;
+    if (g_sdk.emergency_input || g_sdk.safeguard_input ||
+        g_sdk.software_safeguard) {
+        return 102;
+    }
+    g_sdk.robot_error = false;
+    return 0;
+}
+
+int HRIF_GrpStop(unsigned int, unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.group_stop_calls;
+    g_sdk.idle_velocity_reads_after_stop = 0;
+    g_sdk.count_idle_reads = false;
+    if (g_sdk.motion_active) {
+        g_sdk.stop_pending = true;
+        g_sdk.stop_complete_at = Clock::now() + 120ms;
+        g_sdk.completion_at = {};
+    } else {
+        g_sdk.stop_pending = false;
+        g_sdk.count_idle_reads = true;
+    }
+    g_sdk.servo_started = false;
+    return 0;
+}
+
+int HRIF_SetOverride(unsigned int, unsigned int, double)
+{
+    return 0;
+}
+
+int HRIF_ReadRobotState(unsigned int, unsigned int,
+                        int& moving, int& enabled, int& error,
+                        int& error_code, int& error_axis, int& brake,
+                        int& paused, int& emergency_stop, int& safeguard,
+                        int& electrified, int& connected_to_box,
+                        int& blending_done, int& in_position)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (!g_sdk.connected) {
+        return 201;
+    }
+    moving = g_sdk.movingLocked() ? 1 : 0;
+    enabled = g_sdk.enabled ? 1 : 0;
+    error = g_sdk.robot_error ? 1 : 0;
+    error_code = g_sdk.robot_error ? 9001 : 0;
+    error_axis = 0;
+    brake = g_sdk.enabled ? 1 : 0;
+    paused = g_sdk.paused ? 1 : 0;
+    emergency_stop = g_sdk.emergency_input ? 1 : 0;
+    safeguard = (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0;
+    electrified = g_sdk.electrified ? 1 : 0;
+    connected_to_box = 1;
+    blending_done = moving == 0 ? 1 : 0;
+    in_position = g_sdk.doneLocked() ? 1 : 0;
+    return 0;
+}
+
+int HRIF_ReadEmergencyInfo(unsigned int, unsigned int,
+                           int& emergency_signal_fault,
+                           int& emergency_input,
+                           int& safeguard_signal_fault,
+                           int& safeguard_input)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (!g_sdk.connected) {
+        return 202;
+    }
+    emergency_signal_fault = g_sdk.emergency_signal_fault ? 1 : 0;
+    emergency_input = g_sdk.emergency_input ? 1 : 0;
+    safeguard_signal_fault = g_sdk.safeguard_signal_fault ? 1 : 0;
+    safeguard_input =
+        (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0;
+    return 0;
+}
+
+int HRIF_ReadCurWaypointID(unsigned int, unsigned int, std::string& waypoint)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    waypoint = g_sdk.waypoint_id;
+    return 0;
+}
+
+int HRIF_IsMotionDone(unsigned int, unsigned int, bool& done)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (g_sdk.stale_done_once) {
+        g_sdk.stale_done_once = false;
+        done = true;
+    } else {
+        done = g_sdk.doneLocked();
+    }
+    return 0;
+}
+
+int HRIF_ReadActJointPos(unsigned int, unsigned int,
+                         double& j1, double& j2, double& j3,
+                         double& j4, double& j5, double& j6)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.refreshLocked();
+    j1 = g_sdk.joint_position_deg[0];
+    j2 = g_sdk.joint_position_deg[1];
+    j3 = g_sdk.joint_position_deg[2];
+    j4 = g_sdk.joint_position_deg[3];
+    j5 = g_sdk.joint_position_deg[4];
+    j6 = g_sdk.joint_position_deg[5];
+    return 0;
+}
+
+int HRIF_ReadActJointVel(unsigned int, unsigned int,
+                         double& j1, double& j2, double& j3,
+                         double& j4, double& j5, double& j6)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0;
+    j1 = j2 = j3 = j4 = j5 = j6 = velocity;
+    if (velocity == 0.0 && g_sdk.count_idle_reads) {
+        ++g_sdk.idle_velocity_reads_after_stop;
+    }
+    return 0;
+}
+
+int HRIF_ReadActTcpPos(unsigned int, unsigned int,
+                       double& x, double& y, double& z,
+                       double& rx, double& ry, double& rz)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.refreshLocked();
+    x = g_sdk.tcp_position_hr[0];
+    y = g_sdk.tcp_position_hr[1];
+    z = g_sdk.tcp_position_hr[2];
+    rx = g_sdk.tcp_position_hr[3];
+    ry = g_sdk.tcp_position_hr[4];
+    rz = g_sdk.tcp_position_hr[5];
+    return 0;
+}
+
+int HRIF_ReadActTcpVel(unsigned int, unsigned int,
+                       double& x, double& y, double& z,
+                       double& rx, double& ry, double& rz)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0;
+    x = y = z = rx = ry = rz = velocity;
+    return 0;
+}
+
+int HRIF_MoveJ(unsigned int, unsigned int,
+               double, double, double, double, double, double,
+               double j1, double j2, double j3,
+               double j4, double j5, double j6,
+               std::string, std::string, double, double, double,
+               int, int, int, int, std::string command_id)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.move_j_calls;
+    g_sdk.joint_target_deg = {j1, j2, j3, j4, j5, j6};
+    g_sdk.waypoint_id = std::move(command_id);
+    g_sdk.startMotionLocked(true);
+    return 0;
+}
+
+int HRIF_MoveL(unsigned int, unsigned int,
+               double x, double y, double z,
+               double rx, double ry, double rz,
+               double, double, double, double, double, double,
+               std::string, std::string, double, double, double,
+               int, int, int, std::string command_id)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.move_l_calls;
+    g_sdk.tcp_target_hr = {x, y, z, rx, ry, rz};
+    g_sdk.waypoint_id = std::move(command_id);
+    g_sdk.startMotionLocked(false);
+    return 0;
+}
+
+int HRIF_SpeedJ(unsigned int, unsigned int,
+                double, double, double, double, double, double,
+                double, double)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.speed_j_calls;
+    g_sdk.startMotionLocked(true);
+    return 0;
+}
+
+int HRIF_SpeedL(unsigned int, unsigned int,
+                double, double, double, double, double, double,
+                double, double, double)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.speed_l_calls;
+    g_sdk.startMotionLocked(false);
+    return 0;
+}
+
+int HRIF_StartServo(unsigned int, unsigned int, double, double)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.start_servo_calls;
+    g_sdk.servo_started = true;
+    return 0;
+}
+
+int HRIF_PushServoJ(unsigned int, unsigned int,
+                    double j1, double j2, double j3,
+                    double j4, double j5, double j6)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (!g_sdk.servo_started) {
+        return 301;
+    }
+    g_sdk.joint_position_deg = {j1, j2, j3, j4, j5, j6};
+    return 0;
+}
+
+int HRIF_PushServoP(unsigned int, unsigned int,
+                    std::vector<double>& coord,
+                    std::vector<double>&,
+                    std::vector<double>&)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (!g_sdk.servo_started || coord.size() < 6) {
+        return 302;
+    }
+    std::copy_n(coord.begin(), 6, g_sdk.tcp_position_hr.begin());
+    return 0;
+}
+
+int HRIF_SwitchScript(unsigned int, unsigned int, std::string script_name)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (script_name.empty()) {
+        return 401;
+    }
+    g_sdk.selected_program = std::move(script_name);
+    return 0;
+}
+
+int HRIF_StartScript(unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (g_sdk.selected_program.empty()) {
+        return 402;
+    }
+    g_sdk.program_running = true;
+    g_sdk.paused = false;
+    return 0;
+}
+
+int HRIF_PauseScript(unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    if (!g_sdk.program_running) {
+        return 403;
+    }
+    g_sdk.paused = true;
+    return 0;
+}
+
+int HRIF_StopScript(unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    ++g_sdk.stop_script_calls;
+    g_sdk.program_running = false;
+    g_sdk.paused = false;
+    return 0;
+}
+
+int HRIF_EnterSafetyGuard(unsigned int, unsigned int, int flag)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.software_safeguard = flag != 0;
+    return 0;
+}
+
+int HRIF_ShutdownRobot(unsigned int)
+{
+    std::lock_guard lock(g_sdk.mutex);
+    g_sdk.connected = false;
+    g_sdk.enabled = false;
+    g_sdk.electrified = false;
+    return 0;
+}
+
+} // extern "C"
+
+int main()
+{
+    using namespace cmvr::device;
+
+    resetFakeSdk();
+    HuayanRobot arm(makeConfig());
+    CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok());
+
+    MotionOptions options;
+    options.velocity = 0.4;
+    options.acceleration = 0.8;
+    CHECK_TRUE(arm.supportsActionQueueMotion());
+
+    JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}};
+    MotionOptions cancelled_options = options;
+    cancelled_options.cancellation_requested = []() { return true; };
+    CHECK_TRUE(!arm.moveJ(joint_a, cancelled_options).ok());
+    CartesianPose cancelled_pose;
+    cancelled_pose.x = 0.20;
+    cancelled_pose.z = 0.30;
+    CHECK_TRUE(!arm.moveL(cancelled_pose, cancelled_options).ok());
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        CHECK_TRUE(g_sdk.move_j_calls == 0);
+        CHECK_TRUE(g_sdk.move_l_calls == 0);
+    }
+
+    // Once a command has been accepted, caller cancellation returns promptly
+    // but retains the typed motion barrier until Stop confirms controller idle.
+    std::atomic<bool> cancel_during_wait{false};
+    MotionOptions cancellable_options = options;
+    cancellable_options.cancellation_requested = [&cancel_during_wait]() {
+        return cancel_during_wait.load();
+    };
+    JointPositionCommand cancelled_in_wait{
+        {0.05, -0.02, 0.04, 0.01, 0.0, -0.01}};
+    holdNextMotion();
+    auto cancelled_motion = std::async(std::launch::async, [&]() {
+        return arm.moveJ(cancelled_in_wait, cancellable_options);
+    });
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.move_j_calls > 0;
+    }));
+    cancel_during_wait.store(true);
+    CHECK_TRUE(cancelled_motion.wait_for(1s) == std::future_status::ready);
+    if (cancelled_motion.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!cancelled_motion.get().ok());
+    }
+    CHECK_TRUE(arm.busy());
+    CHECK_TRUE(arm.stopMotion().ok());
+    CHECK_TRUE(!arm.busy());
+
+    // The first IsMotionDone read intentionally reports the preceding idle
+    // state. Completion must be correlated with the command/target. Once the
+    // target is reached, an identical command is an idempotent no-op.
+    CHECK_TRUE(arm.moveJ(joint_a, options).ok());
+    int move_j_after_first = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        move_j_after_first = g_sdk.move_j_calls;
+    }
+    CHECK_TRUE(arm.moveJ(joint_a, options).ok());
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        CHECK_TRUE(g_sdk.move_j_calls == move_j_after_first);
+    }
+
+    CartesianPose pose_a;
+    pose_a.x = 0.31;
+    pose_a.y = -0.12;
+    pose_a.z = 0.42;
+    pose_a.rx = 0.08;
+    pose_a.ry = -0.04;
+    pose_a.rz = 0.12;
+    CHECK_TRUE(arm.moveL(pose_a, options).ok());
+    int move_l_after_first = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        move_l_after_first = g_sdk.move_l_calls;
+    }
+    CHECK_TRUE(arm.moveL(pose_a, options).ok());
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        CHECK_TRUE(g_sdk.move_l_calls == move_l_after_first);
+    }
+
+    // Stop must cancel the old owner and wait until the controller reports
+    // stable idle; clearing the owner immediately after GrpStop would fail the
+    // elapsed-time and consecutive-idle checks below.
+    JointPositionCommand joint_b{{0.22, -0.08, 0.14, 0.03, 0.04, -0.01}};
+    holdNextMotion();
+    const int before_held_move = move_j_after_first;
+    auto held_move = std::async(std::launch::async, [&]() {
+        return arm.moveJ(joint_b, options);
+    });
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.move_j_calls > before_held_move;
+    }));
+    const auto stop_started = Clock::now();
+    CHECK_TRUE(arm.stopMotion().ok());
+    const auto stop_elapsed = Clock::now() - stop_started;
+    CHECK_TRUE(stop_elapsed >= 100ms);
+    CHECK_TRUE(held_move.wait_for(1s) == std::future_status::ready);
+    if (held_move.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!held_move.get().ok());
+    }
+    CHECK_TRUE(!arm.busy());
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        CHECK_TRUE(g_sdk.idle_velocity_reads_after_stop >= 3);
+    }
+    CHECK_TRUE(arm.moveJ(joint_b, options).ok());
+
+    // A hardware E-stop cancels and terminates the active waypoint. Releasing
+    // the switch does not clear the software latch or grant a new permit.
+    JointPositionCommand joint_c{{0.34, -0.02, 0.09, 0.05, -0.02, 0.07}};
+    holdNextMotion();
+    int before_estop_move = 0;
+    int before_estop_stop = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        before_estop_move = g_sdk.move_j_calls;
+        before_estop_stop = g_sdk.group_stop_calls;
+    }
+    auto estop_move = std::async(std::launch::async, [&]() {
+        return arm.moveJ(joint_c, options);
+    });
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.move_j_calls > before_estop_move;
+    }));
+    setHardwareEmergencyStop(true);
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.group_stop_calls > before_estop_stop;
+    }));
+    CHECK_TRUE(estop_move.wait_for(2s) == std::future_status::ready);
+    if (estop_move.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!estop_move.get().ok());
+    }
+    setHardwareEmergencyStop(false);
+    std::this_thread::sleep_for(150ms);
+
+    int move_count_while_latched = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        move_count_while_latched = g_sdk.move_j_calls;
+    }
+    const auto rejected_while_latched = arm.moveJ(joint_a, options);
+    CHECK_TRUE(!rejected_while_latched.ok());
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        CHECK_TRUE(g_sdk.move_j_calls == move_count_while_latched);
+    }
+    CHECK_TRUE(arm.clearFault().ok());
+    CHECK_TRUE(arm.torqueOn().ok());
+    CHECK_TRUE(arm.moveJ(joint_a, options).ok());
+
+    // Speed commands own the controller while waiting. A different motion is
+    // rejected, and Stop releases ownership only after termination.
+    holdNextMotion();
+    JointVelocityCommand speed{{0.1, 0.0, 0.0, 0.0, 0.0, 0.0}};
+    int speed_calls_before = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        speed_calls_before = g_sdk.speed_j_calls;
+    }
+    auto speed_motion = std::async(std::launch::async, [&]() {
+        return arm.speedJ(speed, 0.5, 2.0);
+    });
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.speed_j_calls > speed_calls_before;
+    }));
+    CHECK_TRUE(!arm.moveL(pose_a, options).ok());
+    CHECK_TRUE(arm.stopMotion().ok());
+    CHECK_TRUE(speed_motion.wait_for(1s) == std::future_status::ready);
+    if (speed_motion.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!speed_motion.get().ok());
+    }
+    CHECK_TRUE(!arm.busy());
+
+    // SpeedL used to hold the SDK mutex while waiting, which deadlocked its
+    // own timeout/Stop path. A concurrent Stop must cancel it, settle the
+    // controller, and allow a genuinely new Move command afterwards.
+    holdNextMotion();
+    CartesianVelocity line_speed;
+    line_speed.vx = 0.05;
+    int speed_l_calls_before = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        speed_l_calls_before = g_sdk.speed_l_calls;
+    }
+    auto line_speed_motion = std::async(std::launch::async, [&]() {
+        return arm.speedL(line_speed, 0.5, 2.0, FrameType::Base);
+    });
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.speed_l_calls > speed_l_calls_before;
+    }));
+    CHECK_TRUE(arm.stopMotion().ok());
+    CHECK_TRUE(line_speed_motion.wait_for(1s) == std::future_status::ready);
+    if (line_speed_motion.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!line_speed_motion.get().ok());
+    }
+    CHECK_TRUE(!arm.busy());
+    CHECK_TRUE(arm.moveJ(joint_b, options).ok());
+
+    // A dual-channel emergency input mismatch is a typed emergency latch. It
+    // remains blocked after the wiring level is healthy and is recovered only
+    // through the emergency recovery path.
+    int stops_before_signal_fault = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        stops_before_signal_fault = g_sdk.group_stop_calls;
+    }
+    setEmergencySignalFault(true);
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.group_stop_calls > stops_before_signal_fault;
+    }));
+    setEmergencySignalFault(false);
+    std::this_thread::sleep_for(100ms);
+    CHECK_TRUE(!arm.moveJ(joint_c, options).ok());
+    CHECK_TRUE(arm.torqueOn().ok());
+    CHECK_TRUE(arm.moveJ(joint_c, options).ok());
+
+    // Power-off holds a terminal barrier through GrpDisable. Motion remains
+    // denied until an explicit enable confirms the powered state again.
+    CHECK_TRUE(arm.torqueOff().ok());
+    CHECK_TRUE(!arm.moveJ(joint_a, options).ok());
+    CHECK_TRUE(arm.torqueOn().ok());
+    CHECK_TRUE(arm.moveJ(joint_a, options).ok());
+
+    // Servo and program modes retain ownership beyond the start call. Stop of
+    // a retained program must use StopScript as well as the group stop path.
+    ServoOptions servo_options;
+    CHECK_TRUE(arm.startServoMode(servo_options).ok());
+    CHECK_TRUE(!arm.moveJ(joint_b, options).ok());
+    CHECK_TRUE(arm.servoJ(joint_b).ok());
+    CHECK_TRUE(arm.stopServoMode().ok());
+    CHECK_TRUE(!arm.busy());
+
+    CHECK_TRUE(arm.loadProgram("fake_program.script").ok());
+    CHECK_TRUE(arm.playProgram().ok());
+    CHECK_TRUE(!arm.moveJ(joint_c, options).ok());
+    int stop_script_calls_before = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        stop_script_calls_before = g_sdk.stop_script_calls;
+    }
+    CHECK_TRUE(arm.stopMotion().ok());
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        CHECK_TRUE(g_sdk.stop_script_calls > stop_script_calls_before);
+    }
+    CHECK_TRUE(!arm.busy());
+
+    // Retire an in-flight stale generation after transport loss before
+    // reconnecting; no old waiter may issue SDK reads into the new session.
+    holdNextMotion();
+    int moves_before_transport_loss = 0;
+    {
+        std::lock_guard lock(g_sdk.mutex);
+        moves_before_transport_loss = g_sdk.move_j_calls;
+    }
+    auto transport_lost_move = std::async(std::launch::async, [&]() {
+        return arm.moveJ(joint_c, options);
+    });
+    CHECK_TRUE(waitUntil([&]() {
+        std::lock_guard lock(g_sdk.mutex);
+        return g_sdk.move_j_calls > moves_before_transport_loss;
+    }));
+    dropFakeTransport();
+    CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok());
+    CHECK_TRUE(transport_lost_move.wait_for(1s) == std::future_status::ready);
+    if (transport_lost_move.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!transport_lost_move.get().ok());
+    }
+    CHECK_TRUE(arm.moveJ(joint_b, options).ok());
+    CHECK_TRUE(arm.disconnect().ok());
+
+    resetFakeSdk();
+    HuayanRobot shutdown_arm(makeConfig());
+    CHECK_TRUE(shutdown_arm.connect("127.0.0.1", 10003).ok());
+    CHECK_TRUE(shutdown_arm.shutdown().ok());
+    CHECK_TRUE(!shutdown_arm.isConnected());
+    return failures == 0 ? 0 : 1;
+}

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

@@ -0,0 +1,232 @@
+#include "devices/arm/huayan_arm/huayan_lifecycle_state.h"
+
+#include <chrono>
+#include <iostream>
+
+namespace {
+
+#define CHECK_TRUE(condition)                                                        \
+    do {                                                                             \
+        if (!(condition)) {                                                          \
+            std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": "           \
+                      << #condition << std::endl;                                     \
+            return 1;                                                                \
+        }                                                                            \
+    } while (false)
+
+} // namespace
+
+int main()
+{
+    using namespace cmvr::device::huayan_internal;
+
+    MotionState motion;
+    CHECK_TRUE(motion.begin(MotionKind::None).status ==
+               MotionStartStatus::Invalid);
+
+    // A completed target does not poison an identical subsequent command,
+    // while an actually concurrent command is rejected.
+    const auto first_joint = motion.begin(MotionKind::Joint);
+    CHECK_TRUE(first_joint.started());
+    CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Busy);
+    CHECK_TRUE(motion.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Busy);
+    motion.finish(first_joint.token);
+    const auto repeated_joint = motion.begin(MotionKind::Joint);
+    CHECK_TRUE(repeated_joint.started());
+    CHECK_TRUE(repeated_joint.token.generation >
+               first_joint.token.generation);
+    motion.finish(first_joint.token);
+    CHECK_TRUE(motion.ownerActive(repeated_joint.token));
+    motion.finish(repeated_joint.token);
+    CHECK_TRUE(!motion.busy());
+
+    // Stop cancels the current generation and cannot complete before its
+    // owner exits.
+    const auto linear = motion.begin(MotionKind::Linear);
+    CHECK_TRUE(linear.started());
+    const auto stop_linear = motion.beginStop();
+    CHECK_TRUE(stop_linear.started());
+    CHECK_TRUE(stop_linear.kind == MotionKind::Linear);
+    CHECK_TRUE(stop_linear.active_token.generation ==
+               linear.token.generation);
+    CHECK_TRUE(stop_linear.tracked_motion);
+    CHECK_TRUE(motion.cancelled(linear.token));
+    CHECK_TRUE(motion.beginStop().status ==
+               StopStartStatus::AlreadyStopping);
+    CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Stopping);
+    CHECK_TRUE(!motion.waitForOwnerExit(
+        linear.token, std::chrono::milliseconds(1)));
+    CHECK_TRUE(!motion.completeStop());
+    motion.finish(linear.token);
+    CHECK_TRUE(motion.waitForOwnerExit(
+        linear.token, std::chrono::milliseconds(1)));
+    CHECK_TRUE(motion.completeStop());
+    CHECK_TRUE(!motion.busy());
+
+    // An uncertain submission/completion remains fail-closed until a
+    // positively acknowledged Stop clears it.
+    const auto failed_speed = motion.begin(MotionKind::SpeedLinear);
+    CHECK_TRUE(failed_speed.started());
+    motion.failMotion(failed_speed.token);
+    CHECK_TRUE(motion.snapshot().blocked);
+    CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Blocked);
+    const auto stop_failed_speed = motion.beginStop();
+    CHECK_TRUE(stop_failed_speed.kind == MotionKind::SpeedLinear);
+    CHECK_TRUE(stop_failed_speed.tracked_motion);
+    CHECK_TRUE(motion.completeStop());
+
+    const auto failed_stop_motion = motion.begin(MotionKind::Joint);
+    CHECK_TRUE(failed_stop_motion.started());
+    const auto failed_stop = motion.beginStop();
+    CHECK_TRUE(failed_stop.kind == MotionKind::Joint);
+    motion.failStop();
+    CHECK_TRUE(motion.snapshot().blocked);
+    CHECK_TRUE(motion.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Blocked);
+    motion.finish(failed_stop_motion.token);
+    const auto retry_failed_stop = motion.beginStop();
+    CHECK_TRUE(retry_failed_stop.kind == MotionKind::Joint);
+    CHECK_TRUE(motion.completeStop());
+
+    // Servo and program modes remain owned after their start RPC returns.
+    const auto servo = motion.begin(MotionKind::Servo);
+    CHECK_TRUE(servo.started());
+    motion.finish(servo.token, MotionFinishMode::Retain);
+    CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo);
+    CHECK_TRUE(motion.begin(MotionKind::Program).status ==
+               MotionStartStatus::Busy);
+    const auto servo_update = motion.begin(MotionKind::Servo, true);
+    CHECK_TRUE(servo_update.started());
+    motion.finish(servo_update.token);
+    CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo);
+    const auto stop_servo = motion.beginStop();
+    CHECK_TRUE(stop_servo.kind == MotionKind::Servo);
+    CHECK_TRUE(stop_servo.tracked_motion);
+    CHECK_TRUE(motion.completeStop());
+
+    const auto program = motion.begin(MotionKind::Program);
+    CHECK_TRUE(program.started());
+    motion.finish(program.token, MotionFinishMode::Retain);
+    const auto cancelled_program = motion.cancelActiveForSafety();
+    CHECK_TRUE(cancelled_program.kind == MotionKind::Program);
+    CHECK_TRUE(cancelled_program.tracked_motion);
+    CHECK_TRUE(!cancelled_program.active_token.valid());
+    CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Blocked);
+    const auto stop_program = motion.beginStop();
+    CHECK_TRUE(stop_program.kind == MotionKind::Program);
+    CHECK_TRUE(motion.completeStop());
+
+    const auto safety_move = motion.begin(MotionKind::SpeedJoint);
+    CHECK_TRUE(safety_move.started());
+    const auto cancelled_move = motion.cancelActiveForSafety();
+    CHECK_TRUE(cancelled_move.kind == MotionKind::SpeedJoint);
+    CHECK_TRUE(cancelled_move.active_token.generation ==
+               safety_move.token.generation);
+    CHECK_TRUE(motion.cancelled(safety_move.token));
+    motion.finish(safety_move.token);
+    const auto stop_safety_move = motion.beginStop();
+    CHECK_TRUE(stop_safety_move.kind == MotionKind::SpeedJoint);
+    CHECK_TRUE(motion.completeStop());
+
+    RawSafetyState raw;
+    CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Unknown);
+    raw.valid = true;
+    CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Normal);
+    raw.software_protective_stop = true;
+    CHECK_TRUE(classifySafetyCondition(raw) ==
+               SafetyCondition::SoftwareProtectiveStop);
+    raw.software_emergency_stop = true;
+    CHECK_TRUE(classifySafetyCondition(raw) ==
+               SafetyCondition::SoftwareEmergencyStop);
+    raw.robot_fault = 1;
+    CHECK_TRUE(classifySafetyCondition(raw) ==
+               SafetyCondition::RobotFault);
+    raw.safeguard_stop = 1;
+    CHECK_TRUE(classifySafetyCondition(raw) ==
+               SafetyCondition::SafeguardStop);
+    raw.emergency_stop = 1;
+    CHECK_TRUE(classifySafetyCondition(raw) ==
+               SafetyCondition::EmergencyStop);
+    raw.safeguard_signal_fault = 1;
+    CHECK_TRUE(classifySafetyCondition(raw) ==
+               SafetyCondition::SafeguardSignalFault);
+    raw.emergency_signal_fault = 1;
+    CHECK_TRUE(classifySafetyCondition(raw) ==
+               SafetyCondition::EmergencySignalFault);
+
+    SafetyState safety;
+    CHECK_TRUE(!safety.tryPermit().has_value());
+    safety.observe(SafetyCondition::Normal);
+    const auto initial_permit = safety.tryPermit();
+    CHECK_TRUE(initial_permit.has_value());
+    CHECK_TRUE(safety.validate(*initial_permit));
+
+    safety.observe(SafetyCondition::EmergencyStop);
+    CHECK_TRUE(safety.snapshot().latched);
+    CHECK_TRUE(!safety.validate(*initial_permit));
+    CHECK_TRUE(!safety.beginRecovery(safety.snapshot().epoch).has_value());
+    const auto first_emergency_epoch = safety.snapshot().epoch;
+    safety.observe(SafetyCondition::EmergencyStop);
+    safety.observe(SafetyCondition::EmergencyStop);
+    CHECK_TRUE(safety.snapshot().epoch == first_emergency_epoch);
+
+    // Releasing the hardware switch only changes the observed level; it does
+    // not clear the event latch or issue a new motion permit.
+    safety.observe(SafetyCondition::Normal);
+    CHECK_TRUE(safety.snapshot().latched);
+    CHECK_TRUE(!safety.tryPermit().has_value());
+
+    const auto not_ready = safety.beginRecovery(safety.snapshot().epoch);
+    CHECK_TRUE(not_ready.has_value());
+    CHECK_TRUE(!safety.completeRecovery(*not_ready, false, true, true));
+    CHECK_TRUE(safety.snapshot().latched);
+    safety.failRecovery(*not_ready);
+
+    const auto not_idle = safety.beginRecovery(safety.snapshot().epoch);
+    CHECK_TRUE(not_idle.has_value());
+    CHECK_TRUE(!safety.completeRecovery(*not_idle, true, false, true));
+    CHECK_TRUE(safety.snapshot().latched);
+    safety.failRecovery(*not_idle);
+
+    const auto not_cancelled =
+        safety.beginRecovery(safety.snapshot().epoch);
+    CHECK_TRUE(not_cancelled.has_value());
+    CHECK_TRUE(!safety.completeRecovery(*not_cancelled, true, true, false));
+    CHECK_TRUE(safety.snapshot().latched);
+    safety.failRecovery(*not_cancelled);
+
+    const auto recovery_retry =
+        safety.beginRecovery(safety.snapshot().epoch);
+    CHECK_TRUE(recovery_retry.has_value());
+    CHECK_TRUE(safety.completeRecovery(
+        *recovery_retry, true, true, true));
+    const auto recovered_permit = safety.tryPermit();
+    CHECK_TRUE(recovered_permit.has_value());
+    CHECK_TRUE(safety.validate(*recovered_permit));
+
+    // A second safety event, including the same physical E-stop being pressed
+    // again, invalidates an older recovery token atomically.
+    safety.observe(SafetyCondition::EmergencyStop);
+    safety.observe(SafetyCondition::Normal);
+    const auto stale_recovery =
+        safety.beginRecovery(safety.snapshot().epoch);
+    CHECK_TRUE(stale_recovery.has_value());
+    safety.observe(SafetyCondition::EmergencyStop);
+    safety.observe(SafetyCondition::Normal);
+    CHECK_TRUE(!safety.completeRecovery(
+        *stale_recovery, true, true, true));
+    CHECK_TRUE(safety.snapshot().latched);
+    CHECK_TRUE(!safety.snapshot().recovery_in_progress);
+
+    const auto stale_epoch = safety.snapshot().epoch;
+    safety.observe(SafetyCondition::SafeguardStop);
+    safety.observe(SafetyCondition::Normal);
+    CHECK_TRUE(!safety.beginRecovery(stale_epoch).has_value());
+
+    return 0;
+}

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

@@ -34,3 +34,21 @@ target_link_libraries(motor_robot_arm_mujoco_test
         gtest_main
         pthread
 )
+
+add_executable(motor_robot_arm_gen2_mujoco_test
+        src/motor_robot_arm_gen2_mujoco_test.cpp
+)
+
+target_link_libraries(motor_robot_arm_gen2_mujoco_test
+        PRIVATE
+        cmvr_es::device::motor_robot_arm
+        cmvr_es::device::motor_manager
+        cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device_manager
+        cmvr_es::mujoco_viewer
+        cmvr_es::proto
+        cmvr_es::task
+        gtest
+        gtest_main
+        pthread
+)

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

@@ -36,16 +36,26 @@ 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;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override { return emergencyStop(); }
+    Result protectiveStop() override;
+    Result recoverProtectiveStop(
+        const JointTrajectory& path,
+        const MotionOptions& options) override;
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
-    bool isProtectiveStopped() const override { return false; }
+    bool isProtectiveStopped() const override { return protective_stopped_.load(); }
-    bool isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isEmergencyStopped() const override { return emergency_stopped_.load(); }
     bool isFault() const override { return false; }
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
@@ -76,7 +86,7 @@ public:
     Result brakeRelease() override;
     Result shutdown() override;
     Result clearFault() override { return Result::success(); }
-    Result unlockProtectiveStop() override { return Result::success(); }
+    Result unlockProtectiveStop() override;
     Result loadProgram(const std::string& program_name) override;
     Result playProgram() override;
     Result pauseProgram() override;
@@ -94,6 +104,10 @@ public:
 
 private:
     bool containsJoint_(const std::string& joint_name) const;
+    bool safetyStopRequested_() const;
+    std::optional<Result> safetyStopResult_(const std::string& command,
+                                            bool interrupted = false) const;
+    Result quickStopMotors_();
     bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const;
     bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const;
     std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const;
@@ -125,8 +139,13 @@ 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};
+    std::atomic<bool> protective_stopped_{false};
+    std::atomic<bool> emergency_stopped_{false};
+    std::atomic<bool> protective_recovery_active_{false};
+    std::atomic<bool> protective_recovery_cancel_requested_{false};
     ServoOptions servo_options_;
 };
 

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

@@ -2,6 +2,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <cmath>
 #include <Eigen/Dense>
 #include <stdexcept>
 #include <thread>
@@ -29,6 +30,28 @@ struct BusyGuard {
     ~BusyGuard() { busy.store(false); }
 };
 
+struct AtomicFlagGuard {
+    std::atomic<bool>& flag;
+    ~AtomicFlagGuard() { flag.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)
@@ -67,6 +90,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()
@@ -133,12 +189,15 @@ bool MotorRobotArm::stop()
 
 ArmState MotorRobotArm::getRobotState() const
 {
+    const bool protective_stopped = protective_stopped_.load();
+    const bool emergency_stopped = emergency_stopped_.load();
     ArmState state;
     state.connected = motor_manager_ != nullptr;
-    state.powered_on = true;
+    state.powered_on = powered_on_.load(std::memory_order_acquire);
-    state.brake_released = !emergency_stopped_;
+    state.brake_released = state.powered_on && !emergency_stopped;
     state.moving = busy();
-    state.emergency_stopped = emergency_stopped_;
+    state.protective_stopped = protective_stopped;
+    state.emergency_stopped = emergency_stopped;
     state.speed_scaling = speed_scaling_;
     state.robot_mode = RobotMode::Idle;
     state.safety_mode = getSafetyMode();
@@ -155,15 +214,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;
 }
 
@@ -187,7 +266,13 @@ CartesianPose MotorRobotArm::getTcpPose(const FrameType frame) const
 
 SafetyMode MotorRobotArm::getSafetyMode() const
 {
-    return emergency_stopped_ ? SafetyMode::EmergencyStop : SafetyMode::Normal;
+    if (emergency_stopped_.load()) {
+        return SafetyMode::EmergencyStop;
+    }
+    if (protective_stopped_.load()) {
+        return SafetyMode::ProtectiveStop;
+    }
+    return SafetyMode::Normal;
 }
 
 Result MotorRobotArm::torqueOn()
@@ -202,12 +287,16 @@ Result MotorRobotArm::torqueOn()
                                    "failed to torque on motor for joint: " + joint_name);
         }
     }
-    emergency_stopped_ = false;
+    emergency_stopped_.store(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) {
@@ -256,6 +345,194 @@ Result MotorRobotArm::emergencyStop()
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
+    protective_recovery_cancel_requested_.store(true);
+    protective_stopped_.store(false);
+    emergency_stopped_.store(true);
+    return quickStopMotors_();
+}
+
+Result MotorRobotArm::protectiveStop()
+{
+    if (emergency_stopped_.load()) {
+        return Result::success();
+    }
+    if (cartesian_velocity_controller_) {
+        cartesian_velocity_controller_->shutdown();
+    }
+    protective_recovery_cancel_requested_.store(true);
+    protective_stopped_.store(true);
+    return quickStopMotors_();
+}
+
+Result MotorRobotArm::recoverProtectiveStop(
+    const JointTrajectory& path,
+    const MotionOptions& options)
+{
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            "protective recovery rejected: arm is in emergency stop");
+    }
+    if (!protective_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery rejected: arm is not protective stopped");
+    }
+    if (path.size() < 2 ||
+        !std::isfinite(options.velocity) || options.velocity <= 0.0 ||
+        !std::isfinite(options.acceleration) || options.acceleration <= 0.0) {
+        return Result::failure(
+            ArmErrorCode::InvalidArgument,
+            "protective recovery path or options are invalid");
+    }
+
+    for (std::size_t i = 0; i < path.size(); ++i) {
+        const auto& sample = path[i];
+        if (!std::isfinite(sample.time_s) ||
+            sample.position.size() != joint_names_.size() ||
+            sample.velocity.size() != joint_names_.size() ||
+            (i > 0 && sample.time_s <= path[i - 1].time_s)) {
+            return Result::failure(
+                ArmErrorCode::InvalidArgument,
+                "protective recovery sample shape or time is invalid");
+        }
+        for (std::size_t joint = 0; joint < sample.position.size(); ++joint) {
+            if (!std::isfinite(sample.position[joint]) ||
+                !std::isfinite(sample.velocity[joint])) {
+                return Result::failure(
+                    ArmErrorCode::InvalidArgument,
+                    "protective recovery sample contains a non-finite value");
+            }
+        }
+    }
+
+    if (busy_.exchange(true)) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery rejected: arm is busy");
+    }
+    BusyGuard busy_guard{busy_};
+
+    bool expected = false;
+    if (!protective_recovery_active_.compare_exchange_strong(expected, true)) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery is already active");
+    }
+    AtomicFlagGuard recovery_guard{protective_recovery_active_};
+    protective_recovery_cancel_requested_.store(false);
+
+    if (!joint_planner_) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "protective recovery planner is not initialized");
+    }
+
+    JointTrajectory recovery_trajectory;
+    const auto planning_start = std::chrono::steady_clock::now();
+    if (!joint_planner_->planReplay(
+            readJointPosition_(), path, options, recovery_trajectory)) {
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            "failed to plan protective recovery replay trajectory");
+    }
+    const double planning_ms = std::chrono::duration<double, std::milli>(
+        std::chrono::steady_clock::now() - planning_start).count();
+    CMVR_LOG(INFO) << "[MotorRobotArm] protective recovery planned"
+                   << ", input_samples=" << path.size()
+                   << ", command_samples=" << recovery_trajectory.size()
+                   << ", planning_ms=" << planning_ms
+                   << ", trajectory_duration_s="
+                   << recovery_trajectory.back().time_s;
+
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            "protective recovery interrupted by emergency stop during planning");
+    }
+    if (protective_recovery_cancel_requested_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInProtectiveStop,
+            "protective recovery aborted by collision monitor during planning");
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(joint_names_.size());
+    for (const auto& joint_name : joint_names_) {
+        auto motor = getMotor_(joint_name);
+        if (!motor) {
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "motor not found for joint: " + joint_name);
+        }
+        if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION &&
+            !motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "failed to set recovery position mode for joint: " + joint_name);
+        }
+        motors.push_back(std::move(motor));
+    }
+
+    const auto trajectory_start = std::chrono::steady_clock::now();
+    for (std::size_t i = 0; i < recovery_trajectory.size(); ++i) {
+        if (emergency_stopped_.load()) {
+            return Result::failure(
+                ArmErrorCode::RobotInEmergencyStop,
+                "protective recovery interrupted by emergency stop");
+        }
+        if (protective_recovery_cancel_requested_.load()) {
+            return Result::failure(
+                ArmErrorCode::RobotInProtectiveStop,
+                "protective recovery aborted by collision monitor");
+        }
+
+        const auto& sample = recovery_trajectory[i];
+        if (!motor_manager_->commandCyclicPositionsAtomic(
+                motors, sample.position, sample.velocity)) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "failed to submit protective recovery sample");
+        }
+
+        if (i + 1 < recovery_trajectory.size()) {
+            std::this_thread::sleep_until(
+                trajectory_start +
+                std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+                    std::chrono::duration<double>(
+                        recovery_trajectory[i + 1].time_s)));
+        }
+    }
+
+    const std::vector<double> zero_velocity(joint_names_.size(), 0.0);
+    if (!motor_manager_->commandCyclicPositionsAtomic(
+            motors, path.front().position, zero_velocity)) {
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            "failed to hold final protective recovery position");
+    }
+    return Result::success();
+}
+
+Result MotorRobotArm::unlockProtectiveStop()
+{
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            "cannot unlock protective stop while arm is emergency stopped");
+    }
+    if (protective_recovery_active_.load()) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "cannot unlock protective stop while recovery is active");
+    }
+    protective_stopped_.store(false);
+    return Result::success();
+}
+
+Result MotorRobotArm::quickStopMotors_()
+{
     for (const auto& joint_name : joint_names_) {
         auto motor = getMotor_(joint_name);
         if (!motor) {
@@ -266,10 +543,32 @@ Result MotorRobotArm::emergencyStop()
                                    "failed to quick stop motor for joint: " + joint_name);
         }
     }
-    emergency_stopped_ = true;
     return Result::success();
 }
 
+bool MotorRobotArm::safetyStopRequested_() const
+{
+    return emergency_stopped_.load() || protective_stopped_.load();
+}
+
+std::optional<Result> MotorRobotArm::safetyStopResult_(
+    const std::string& command,
+    const bool interrupted) const
+{
+    const char* action = interrupted ? " interrupted by " : " rejected: arm is in ";
+    if (emergency_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInEmergencyStop,
+            command + action + "emergency stop");
+    }
+    if (protective_stopped_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotInProtectiveStop,
+            command + action + "protective stop");
+    }
+    return std::nullopt;
+}
+
 Result MotorRobotArm::setSpeedScaling(const double scaling)
 {
     if (scaling < 0.0 || scaling > 1.0) {
@@ -281,6 +580,9 @@ Result MotorRobotArm::setSpeedScaling(const double scaling)
 
 Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
 {
+    if (const auto stopped = safetyStopResult_("moveJ")) {
+        return *stopped;
+    }
     std::string error;
     if (!validatePositionCommand_(target, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
@@ -294,7 +596,7 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
     BusyGuard busy_guard{busy_};
     std::lock_guard<std::mutex> lock(mutex_);
 
-    std::vector<JointTrajectorySample> samples;
+    JointTrajectory samples;
     if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) {
         return Result::failure(ArmErrorCode::CommandFailed, "[MotorRobotArm] moveJ planner failed: " + id_);
     }
@@ -323,6 +625,9 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
     constexpr double fallback_dt = 0.001;
     std::vector<double> command_velocity(motors.size(), 0.0);
     for (std::size_t k = 1; k < samples.size(); ++k) {
+        if (const auto stopped = safetyStopResult_("moveJ", true)) {
+            return *stopped;
+        }
         const auto& sample = samples[k];
         if (sample.position.size() != motors.size()) {
             return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
@@ -337,7 +642,8 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
                                    "failed to submit atomic cyclic position command");
         }
         if (k + 1 < samples.size()) {
-            const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t
+            const double next_t = samples[k + 1].time_s > 0.0
+                                      ? samples[k + 1].time_s
                                                          : static_cast<double>(k + 1) * fallback_dt;
             std::this_thread::sleep_until(t0 + std::chrono::duration_cast<std::chrono::steady_clock::duration>(
                                                    std::chrono::duration<double>(next_t)));
@@ -351,6 +657,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
                              const double duration)
 {
     (void)acceleration;
+    if (const auto stopped = safetyStopResult_("speedJ")) {
+        return *stopped;
+    }
     std::string error;
     if (!validateVelocityCommand_(velocity, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
@@ -388,6 +697,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
 
 Result MotorRobotArm::stopJ(const double acceleration)
 {
+    if (safetyStopRequested_()) {
+        return Result::success();
+    }
     JointVelocityCommand zero;
     zero.velocity.assign(joint_names_.size(), 0.0);
     return speedJ(zero, acceleration, 0.0);
@@ -397,6 +709,9 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                             const MotionOptions& options,
                             const FrameType frame)
 {
+    if (const auto stopped = safetyStopResult_("moveL")) {
+        return *stopped;
+    }
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
@@ -438,8 +753,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                           << ", executable_path_m=" << trajectory.executable_path_length;
     }
 
-    return executeMoveLTrajectory_(trajectory) ? Result::success()
+    if (executeMoveLTrajectory_(trajectory)) {
-                                               : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
+        return Result::success();
+    }
+    if (const auto stopped = safetyStopResult_("moveL", true)) {
+        return *stopped;
+    }
+    return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
 }
 
 Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@@ -447,6 +767,9 @@ Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
                              const double duration,
                              const FrameType frame)
 {
+    if (const auto stopped = safetyStopResult_("speedL")) {
+        return *stopped;
+    }
     if (busy_.load()) {
         return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
     }
@@ -486,6 +809,9 @@ Result MotorRobotArm::startServoMode(const ServoOptions& options)
 
 Result MotorRobotArm::servoJ(const JointPositionCommand& target)
 {
+    if (const auto stopped = safetyStopResult_("servoJ")) {
+        return *stopped;
+    }
     std::string error;
     if (!validatePositionCommand_(target, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
@@ -790,6 +1116,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
 
     auto next_deadline = std::chrono::steady_clock::now();
     for (std::size_t i = 1; i < trajectory.position.size(); ++i) {
+        if (safetyStopRequested_()) {
+            return false;
+        }
         const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]);
         const auto& position = trajectory.position[i];
         const auto& velocity = trajectory.velocity[i];

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

@@ -0,0 +1,881 @@
+#include "arm/motor_robot_arm/include/motor_robot_arm.h"
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <filesystem>
+#include <functional>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Geometry>
+#include <gtest/gtest.h>
+
+#include "common/io/proto_file_io.h"
+#include "common/math/transform_math.h"
+#include "manager/device_manager/include/device_manager.h"
+#include "devices/motor/manager/include/motor_manager.h"
+#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+#include "task/self_collision_task/include/self_collision_task.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr std::size_t kDof = 7;
+constexpr std::array<const char*, kDof> kJointNames = {
+    "right_arm_J1", "right_arm_J2", "right_arm_J3", "right_arm_J4",
+    "right_arm_J5", "right_arm_J6", "right_arm_J7"
+};
+
+const std::vector<double> kSetupPose{
+    0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0
+};
+
+const std::vector<double> kTorsoCollisionPose{
+    1.57607137794121,
+    2.06613762981425,
+    -1.76915077905899,
+    0.959251437141443,
+    -0.725973209527894,
+    1.79390262120717,
+    0.2223354372144,
+};
+
+std::filesystem::path findProjectRoot()
+{
+    const std::filesystem::path marker = "model/gen2/gen2_fixed.xml";
+    const auto search = [&](std::filesystem::path current) {
+        while (!current.empty()) {
+            if (std::filesystem::exists(current / marker)) {
+                return current;
+            }
+            const auto parent = current.parent_path();
+            if (parent == current) {
+                break;
+            }
+            current = parent;
+        }
+        return std::filesystem::path{};
+    };
+
+    auto root = search(std::filesystem::current_path());
+    if (!root.empty()) {
+        return root;
+    }
+    return search(std::filesystem::path(__FILE__).parent_path());
+}
+
+double maxPositionError(const std::vector<double>& actual,
+                        const std::vector<double>& expected)
+{
+    if (actual.size() != expected.size()) {
+        return std::numeric_limits<double>::infinity();
+    }
+    double error = 0.0;
+    for (std::size_t i = 0; i < actual.size(); ++i) {
+        error = std::max(error, std::abs(actual[i] - expected[i]));
+    }
+    return error;
+}
+
+double translationError(const CartesianPose& lhs, const CartesianPose& rhs)
+{
+    return std::sqrt(std::pow(lhs.x - rhs.x, 2.0) +
+                     std::pow(lhs.y - rhs.y, 2.0) +
+                     std::pow(lhs.z - rhs.z, 2.0));
+}
+
+double rotationError(const CartesianPose& lhs, const CartesianPose& rhs)
+{
+    const Eigen::Matrix3d lhs_rotation =
+        common::math::poseToMatrix(lhs).block<3, 3>(0, 0);
+    const Eigen::Matrix3d rhs_rotation =
+        common::math::poseToMatrix(rhs).block<3, 3>(0, 0);
+    return std::abs(Eigen::AngleAxisd(lhs_rotation.transpose() * rhs_rotation).angle());
+}
+
+Eigen::Vector3d baseRotationDelta(const CartesianPose& start, const CartesianPose& end)
+{
+    const Eigen::Matrix3d start_rotation =
+        common::math::poseToMatrix(start).block<3, 3>(0, 0);
+    const Eigen::Matrix3d end_rotation =
+        common::math::poseToMatrix(end).block<3, 3>(0, 0);
+    const Eigen::AngleAxisd delta(end_rotation * start_rotation.transpose());
+    return delta.axis() * delta.angle();
+}
+
+template <class Predicate>
+void waitFor(Predicate predicate, const std::chrono::milliseconds timeout)
+{
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (!predicate() && std::chrono::steady_clock::now() < deadline) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+struct ScenarioOutcome {
+    Result move_j{Result::failure(ArmErrorCode::UnknownError, "not run")};
+    Result move_l{Result::failure(ArmErrorCode::UnknownError, "not run")};
+    double move_j_error{std::numeric_limits<double>::infinity()};
+    double move_l_error{std::numeric_limits<double>::infinity()};
+    double move_l_rotation_error{std::numeric_limits<double>::infinity()};
+    std::string worker_error;
+};
+
+class MotorRobotArmGen2MujocoTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        DeviceManager::destroyInstance();
+        project_root_ = findProjectRoot();
+        ASSERT_FALSE(project_root_.empty());
+
+        config::MujocoWorldRootConfig world_root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(),
+            &world_root_config));
+        ASSERT_GT(world_root_config.worlds_size(), 0);
+
+        auto world_config = world_root_config.worlds(0);
+        world_config.set_model_path(
+            (project_root_ / "model/gen2/gen2_fixed.xml").string());
+        world_device_ = std::make_shared<simulate::MujocoWorldDevice>(world_config);
+        ASSERT_TRUE(world_device_->init());
+        ASSERT_TRUE(world_device_->start());
+
+        config::MotorRootConfig motor_root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ /
+             "cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt").string(),
+            &motor_root_config));
+
+        std::unordered_set<std::string> right_arm_joints;
+        for (const auto* joint_name : kJointNames) {
+            right_arm_joints.insert(joint_name);
+        }
+        MotorManager::clearActiveJoints();
+        MotorManager::setActiveJoints(
+            "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}});
+
+        motor_system_ = std::make_shared<MotorManager>(
+            "mujoco_motors", motor_root_config.motor());
+        ASSERT_TRUE(motor_system_->init());
+        world_ = MotorManager::mujocoWorldFor("mujoco_motors");
+        ASSERT_TRUE(world_);
+        ASSERT_TRUE(world_->isLoaded());
+
+        config::ArmRootConfig root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ /
+             "cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt").string(),
+            &root_config));
+        ASSERT_GT(root_config.arm().robot_arms_size(), 0);
+
+        auto arm_config = root_config.arm().robot_arms(0);
+        arm_config.mutable_kinematics()
+            ->mutable_pinocchio_dls_ik_solver()
+            ->set_urdf_path((project_root_ / "model/gen2/robot.urdf").string());
+
+        arm_ = std::make_shared<MotorRobotArm>(arm_config);
+        ASSERT_TRUE(arm_->init());
+        const Result torque_result = arm_->torqueOn();
+        ASSERT_TRUE(torque_result.ok()) << torque_result.message;
+
+        config::DeviceManagerConfig device_manager_config;
+        device_manager_config.set_name("gen2_collision_mujoco_test");
+        DeviceManager::getInstance(device_manager_config).registerDevice(arm_);
+    }
+
+    void TearDown() override
+    {
+        if (arm_) {
+            arm_->stop();
+        }
+        if (motor_system_) {
+            motor_system_->stop();
+        }
+        if (world_device_) {
+            world_device_->stop();
+        }
+        DeviceManager::destroyInstance();
+        MotorManager::clearActiveJoints();
+    }
+
+    std::filesystem::path project_root_;
+    std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
+    std::shared_ptr<MotorManager> motor_system_;
+    std::shared_ptr<simulate::MujocoWorld> world_;
+    std::shared_ptr<MotorRobotArm> arm_;
+};
+
+TEST_F(MotorRobotArmGen2MujocoTest, HoldsInitialPosition)
+{
+    const auto start = arm_->getJointState().position;
+    ASSERT_EQ(start.size(), kDof);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    const auto end = arm_->getJointState().position;
+    const double drift = maxPositionError(end, start);
+
+    std::cout << "[MotorRobotArmGen2MujocoTest] hold max drift: "
+              << drift << std::endl;
+    EXPECT_LT(drift, 0.02);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, ProtectiveAndEmergencyStopAreDistinct)
+{
+    ASSERT_TRUE(arm_->protectiveStop().ok());
+    EXPECT_TRUE(arm_->isProtectiveStopped());
+    EXPECT_FALSE(arm_->isEmergencyStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::ProtectiveStop);
+    const auto protective_state = arm_->getRobotState();
+    EXPECT_TRUE(protective_state.protective_stopped);
+    EXPECT_FALSE(protective_state.emergency_stopped);
+
+    MotionOptions options;
+    options.velocity = 0.6;
+    options.acceleration = 2.0;
+    const Result protected_move = arm_->moveJ(
+        JointPositionCommand{kSetupPose}, options);
+    EXPECT_EQ(protected_move.code, ArmErrorCode::RobotInProtectiveStop);
+
+    ASSERT_TRUE(arm_->unlockProtectiveStop().ok());
+    EXPECT_FALSE(arm_->isProtectiveStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
+
+    ASSERT_TRUE(arm_->emergencyStop().ok());
+    EXPECT_FALSE(arm_->isProtectiveStopped());
+    EXPECT_TRUE(arm_->isEmergencyStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::EmergencyStop);
+    const auto emergency_state = arm_->getRobotState();
+    EXPECT_FALSE(emergency_state.protective_stopped);
+    EXPECT_TRUE(emergency_state.emergency_stopped);
+
+    const Result rejected_unlock = arm_->unlockProtectiveStop();
+    EXPECT_EQ(rejected_unlock.code, ArmErrorCode::RobotInEmergencyStop);
+    ASSERT_TRUE(arm_->torqueOn().ok());
+    EXPECT_FALSE(arm_->isEmergencyStopped());
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, MoveJ)
+{
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+    ScenarioOutcome outcome;
+
+    std::thread scenario([&] {
+        try {
+            if (!world_ || !world_->isRunning()) {
+                throw std::runtime_error("MuJoCo world is not running");
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            MotionOptions options;
+            options.velocity = 0.6;
+            options.acceleration = 2.0;
+
+            outcome.move_j = arm_->moveJ(JointPositionCommand{kSetupPose}, options);
+            waitFor([&] {
+                return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
+            }, std::chrono::seconds(3));
+            outcome.move_j_error = maxPositionError(
+                arm_->getJointState().position, kSetupPose);
+        } catch (const std::exception& error) {
+            outcome.worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    std::cout << "[MotorRobotArmGen2MujocoTest] moveJ max error: "
+              << outcome.move_j_error << std::endl;
+    EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
+    EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
+    EXPECT_LT(outcome.move_j_error, 0.08);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, MoveL)
+{
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+    ScenarioOutcome outcome;
+
+    std::thread scenario([&] {
+        try {
+            if (!world_ || !world_->isRunning()) {
+                throw std::runtime_error("MuJoCo world is not running");
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            MotionOptions joint_options;
+            joint_options.velocity = 1.6;
+            joint_options.acceleration = 12.0;
+            outcome.move_j = arm_->moveJ(
+                JointPositionCommand{kSetupPose}, joint_options);
+            waitFor([&] {
+                return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
+            }, std::chrono::seconds(3));
+            outcome.move_j_error = maxPositionError(
+                arm_->getJointState().position, kSetupPose);
+            if (!outcome.move_j.ok()) {
+                throw std::runtime_error(outcome.move_j.message);
+            }
+
+            MotionOptions cartesian_options;
+            cartesian_options.velocity = 0.08;
+            cartesian_options.acceleration = 0.4;
+            cartesian_options.jerk = 1.0;
+
+            MotionOptions rotation_options;
+            rotation_options.velocity = 0.15;
+            rotation_options.acceleration = 0.5;
+            rotation_options.jerk = 2.0;
+
+            const auto return_to_setup = [&](const char* step_name) {
+                outcome.move_j = arm_->moveJ(
+                    JointPositionCommand{kSetupPose}, joint_options);
+                if (!outcome.move_j.ok()) {
+                    throw std::runtime_error(
+                        std::string("moveJ before moveL ") + step_name +
+                        ": " + outcome.move_j.message);
+                }
+                waitFor([&] {
+                    return maxPositionError(
+                               arm_->getJointState().position, kSetupPose) < 0.04;
+                }, std::chrono::seconds(3));
+                std::this_thread::sleep_for(std::chrono::milliseconds(300));
+            };
+
+            struct CartesianStep {
+                const char* name;
+                double dx;
+                double dy;
+                double dz;
+            };
+            const std::array<CartesianStep, 3> translation_steps{{
+                {"+X", 0.15, 0.0, 0.0},
+                {"+Y", 0.0, 0.15, 0.0},
+                {"+Z", 0.0, 0.0, 0.15},
+            }};
+
+            struct RotationStep {
+                const char* name;
+                double drx;
+                double dry;
+                double drz;
+            };
+            constexpr double kRotationStep =
+                20.0 * 3.14159265358979323846 / 180.0;
+            const std::array<RotationStep, 3> rotation_steps{{
+                {"+RX", kRotationStep, 0.0, 0.0},
+                {"+RY", 0.0, kRotationStep, 0.0},
+                {"+RZ", 0.0, 0.0, kRotationStep},
+            }};
+
+            outcome.move_l_error = 0.0;
+            for (std::size_t i = 0; i < translation_steps.size(); ++i) {
+                const auto& step = translation_steps[i];
+                if (i > 0) {
+                    return_to_setup(step.name);
+                }
+                CartesianPose target = arm_->getTcpPose();
+                target.x += step.dx;
+                target.y += step.dy;
+                target.z += step.dz;
+
+                outcome.move_l = arm_->moveL(
+                    target, cartesian_options, FrameType::Base);
+                if (!outcome.move_l.ok()) {
+                    throw std::runtime_error(
+                        std::string("moveL ") + step.name + ": " +
+                        outcome.move_l.message);
+                }
+                waitFor([&] {
+                    return translationError(arm_->getTcpPose(), target) < 0.005;
+                }, std::chrono::seconds(5));
+                const double error = translationError(arm_->getTcpPose(), target);
+                outcome.move_l_error = std::max(outcome.move_l_error, error);
+                std::cout << "[MotorRobotArmGen2MujocoTest] moveL "
+                          << step.name << " translation error: " << error
+                          << std::endl;
+                std::this_thread::sleep_for(std::chrono::milliseconds(500));
+            }
+
+            outcome.move_l_rotation_error = 0.0;
+            for (const auto& step : rotation_steps) {
+                return_to_setup(step.name);
+                CartesianPose target = arm_->getTcpPose();
+                target.rx += step.drx;
+                target.ry += step.dry;
+                target.rz += step.drz;
+
+                outcome.move_l = arm_->moveL(
+                    target, rotation_options, FrameType::Base);
+                if (!outcome.move_l.ok()) {
+                    throw std::runtime_error(
+                        std::string("moveL ") + step.name + ": " +
+                        outcome.move_l.message);
+                }
+                waitFor([&] {
+                    return rotationError(arm_->getTcpPose(), target) < 0.01;
+                }, std::chrono::seconds(7));
+                const double error = rotationError(arm_->getTcpPose(), target);
+                outcome.move_l_rotation_error = std::max(
+                    outcome.move_l_rotation_error, error);
+                std::cout << "[MotorRobotArmGen2MujocoTest] moveL "
+                          << step.name << " rotation error: " << error
+                          << " rad" << std::endl;
+                std::this_thread::sleep_for(std::chrono::milliseconds(500));
+            }
+        } catch (const std::exception& error) {
+            outcome.worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(3));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    std::cout << "[MotorRobotArmGen2MujocoTest] moveJ setup max error: "
+              << outcome.move_j_error
+              << ", moveL translation error: " << outcome.move_l_error
+              << ", moveL rotation error: "
+              << outcome.move_l_rotation_error << " rad" << std::endl;
+    EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
+    EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
+    EXPECT_LT(outcome.move_j_error, 0.08);
+    EXPECT_TRUE(outcome.move_l.ok()) << outcome.move_l.message;
+    EXPECT_LT(outcome.move_l_error, 0.01);
+    EXPECT_LT(outcome.move_l_rotation_error, 0.02);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, SelfCollisionProtectiveStopMoveJMoveLSpeedL)
+{
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+
+    struct CollisionCaseOutcome {
+        std::string name;
+        Result setup_move{Result::failure(ArmErrorCode::UnknownError, "not run")};
+        Result collision_move{Result::failure(ArmErrorCode::UnknownError, "not run")};
+        Result recovery{Result::failure(ArmErrorCode::UnknownError, "not run")};
+        task::SelfCollisionTaskStatus initial_status;
+        task::SelfCollisionTaskStatus stop_status;
+        task::SelfCollisionTaskStatus recovered_status;
+        CartesianPose start_tcp;
+        CartesianPose final_tcp;
+        std::vector<double> final_position;
+        bool task_initialized{false};
+        bool task_started{false};
+        bool stop_seen{false};
+        bool recovery_succeeded{false};
+        bool monitor_ok{true};
+        bool protective_stopped{false};
+        bool emergency_stopped{false};
+        SafetyMode safety_mode{SafetyMode::Unknown};
+    };
+
+    CollisionCaseOutcome move_j_outcome;
+    CollisionCaseOutcome move_l_outcome;
+    CollisionCaseOutcome speed_l_outcome;
+    CartesianPose move_l_target;
+    CartesianPose collision_tcp_target;
+    std::string worker_error;
+
+    std::thread scenario([&] {
+        try {
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            config::SelfCollisionTaskRootConfig root_config;
+            const auto config_path = project_root_ /
+                "cmvr-es/config/tasks/self_collision_task/"
+                "self_collision_task_gen2.pb.txt";
+            if (!ProtoMessageIo::getProtoFromAsciiFile(
+                    config_path.string(), &root_config)) {
+                throw std::runtime_error(
+                    "failed to load self-collision config: " + config_path.string());
+            }
+            auto collision_config = root_config.self_collision_task();
+            collision_config.mutable_checker()->set_urdf_path(
+                (project_root_ /
+                 "model/gen2/collision/robot_collision.urdf").string());
+
+            Eigen::Matrix4d collision_tcp_transform = Eigen::Matrix4d::Identity();
+            const auto solver = arm_->kinematicsSolver();
+            if (!solver ||
+                !solver->fk(kTorsoCollisionPose, collision_tcp_transform, true)) {
+                throw std::runtime_error("failed to calculate collision TCP target");
+            }
+            collision_tcp_target =
+                common::math::matrixToPose(collision_tcp_transform);
+
+            const auto run_collision_case = [&](
+                const std::string& name,
+                const std::function<Result()>& start_motion,
+                const std::chrono::milliseconds stop_timeout) {
+                CollisionCaseOutcome outcome;
+                outcome.name = name;
+
+                const Result torque_result = arm_->torqueOn();
+                if (!torque_result.ok()) {
+                    throw std::runtime_error(
+                        name + " torqueOn: " + torque_result.message);
+                }
+
+                MotionOptions setup_options;
+                setup_options.velocity = 0.6;
+                setup_options.acceleration = 2.0;
+                outcome.setup_move = arm_->moveJ(
+                    JointPositionCommand{kSetupPose}, setup_options);
+                if (!outcome.setup_move.ok()) {
+                    throw std::runtime_error(
+                        name + " setup moveJ: " + outcome.setup_move.message);
+                }
+
+                task::SelfCollisionTask collision_task(collision_config);
+                outcome.task_initialized = collision_task.init();
+                if (!outcome.task_initialized) {
+                    throw std::runtime_error(
+                        name + " init: " + collision_task.detailStatusString());
+                }
+                outcome.task_started = collision_task.start();
+                if (!outcome.task_started || !collision_task.step(0.002)) {
+                    throw std::runtime_error(
+                        name + " start: " + collision_task.detailStatusString());
+                }
+                outcome.initial_status = collision_task.latestStatus();
+                if (outcome.initial_status.level != task::CollisionSafetyLevel::SAFE) {
+                    throw std::runtime_error(
+                        name + " setup pose is not SAFE: " +
+                        collision_task.detailStatusString());
+                }
+                outcome.start_tcp = arm_->getTcpPose();
+
+                std::atomic_bool monitor_running{true};
+                std::atomic_bool monitor_ok{true};
+                std::atomic_bool stop_seen{false};
+                std::thread monitor([&] {
+                    while (monitor_running.load()) {
+                        if (!collision_task.step(0.002)) {
+                            monitor_ok = false;
+                            break;
+                        }
+                        const auto status = collision_task.latestStatus();
+                        if (status.stop_latched && !stop_seen.load()) {
+                            outcome.stop_status = status;
+                            stop_seen.store(true);
+                        }
+                        std::this_thread::sleep_for(std::chrono::milliseconds(2));
+                    }
+                });
+
+                outcome.collision_move = start_motion();
+                waitFor([&] {
+                    return stop_seen.load() || !monitor_ok.load();
+                }, stop_timeout);
+                if (!stop_seen.load()) {
+                    arm_->stopMotion();
+                    monitor_running = false;
+                    monitor.join();
+                    collision_task.stop();
+                    throw std::runtime_error(name + " did not trigger protective stop");
+                }
+
+                outcome.recovery = collision_task.requestRecovery(
+                    outcome.stop_status.event_id);
+                outcome.recovered_status = collision_task.latestStatus();
+                outcome.recovery_succeeded = outcome.recovery.ok();
+
+                monitor_running = false;
+                monitor.join();
+                collision_task.stop();
+                outcome.stop_seen = stop_seen.load();
+                outcome.monitor_ok = monitor_ok.load();
+                outcome.final_position = arm_->getJointState().position;
+                outcome.final_tcp = arm_->getTcpPose();
+                outcome.protective_stopped = arm_->isProtectiveStopped();
+                outcome.emergency_stopped = arm_->isEmergencyStopped();
+                outcome.safety_mode = arm_->getSafetyMode();
+
+                std::cout << "[MotorRobotArmGen2MujocoTest] collision "
+                          << name
+                          << " stop_seen=" << outcome.stop_seen
+                          << ", distance_m="
+                          << outcome.stop_status.result.minimum_distance_m
+                          << ", pair=" << outcome.stop_status.result.first
+                          << "/" << outcome.stop_status.result.second
+                          << ", event_id=" << outcome.stop_status.event_id
+                          << ", recovery=" << outcome.recovery.message
+                          << ", recovered_distance_m="
+                          << outcome.recovered_status.result.minimum_distance_m
+                          << std::endl;
+                return outcome;
+            };
+
+            move_j_outcome = run_collision_case(
+                "MoveJ",
+                [&] {
+                    MotionOptions options;
+                    options.velocity = 0.45;
+                    options.acceleration = 1.0;
+                    return arm_->moveJ(
+                        JointPositionCommand{kTorsoCollisionPose}, options);
+                },
+                std::chrono::seconds(3));
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+
+            move_l_outcome = run_collision_case(
+                "MoveL",
+                [&] {
+                    move_l_target = collision_tcp_target;
+                    MotionOptions options;
+                    options.velocity = 0.12;
+                    options.acceleration = 0.5;
+                    options.jerk = 2.0;
+                    return arm_->moveL(
+                        move_l_target, options, FrameType::Base);
+                },
+                std::chrono::seconds(3));
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+
+            speed_l_outcome = run_collision_case(
+                "SpeedL",
+                [&] {
+                    const CartesianPose start = arm_->getTcpPose();
+                    Eigen::Vector3d linear_direction{
+                        collision_tcp_target.x - start.x,
+                        collision_tcp_target.y - start.y,
+                        collision_tcp_target.z - start.z,
+                    };
+                    Eigen::Vector3d angular_direction =
+                        baseRotationDelta(start, collision_tcp_target);
+                    const double command_duration_s = std::max(
+                        linear_direction.norm() / 0.05,
+                        angular_direction.norm() / 0.20);
+                    if (command_duration_s <= 0.0) {
+                        return Result::failure(
+                            ArmErrorCode::InvalidArgument,
+                            "SpeedL collision target has zero displacement");
+                    }
+                    linear_direction /= command_duration_s;
+                    angular_direction /= command_duration_s;
+                    std::cout
+                        << "[MotorRobotArmGen2MujocoTest] collision SpeedL target_time="
+                        << command_duration_s << " s" << std::endl;
+                    return arm_->speedL(
+                        CartesianVelocity{
+                            linear_direction.x(),
+                            linear_direction.y(),
+                            linear_direction.z(),
+                            angular_direction.x(),
+                            angular_direction.y(),
+                            angular_direction.z(),
+                        },
+                        0.5,
+                        0.0,
+                        FrameType::Base);
+                },
+                std::chrono::seconds(15));
+        } catch (const std::exception& error) {
+            worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(3));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    EXPECT_TRUE(worker_error.empty()) << worker_error;
+    const auto expect_protective_stop = [&](const CollisionCaseOutcome& outcome) {
+        EXPECT_TRUE(outcome.setup_move.ok())
+            << outcome.name << ": " << outcome.setup_move.message;
+        EXPECT_TRUE(outcome.task_initialized) << outcome.name;
+        EXPECT_TRUE(outcome.task_started) << outcome.name;
+        EXPECT_EQ(outcome.initial_status.level, task::CollisionSafetyLevel::SAFE)
+            << outcome.name;
+        EXPECT_TRUE(outcome.monitor_ok) << outcome.name;
+        EXPECT_TRUE(outcome.stop_seen) << outcome.name;
+        EXPECT_EQ(outcome.stop_status.level, task::CollisionSafetyLevel::STOP)
+            << outcome.name;
+        EXPECT_TRUE(outcome.stop_status.stop_latched) << outcome.name;
+        EXPECT_NE(outcome.stop_status.event_id, 0U) << outcome.name;
+        EXPECT_EQ(outcome.stop_status.recovery_state,
+                  task::ProtectiveRecoveryState::AVAILABLE)
+            << outcome.name;
+        EXPECT_GE(outcome.stop_status.recovery_sample_count, 2U)
+            << outcome.name;
+        EXPECT_LE(outcome.stop_status.result.minimum_distance_m, 0.005)
+            << outcome.name;
+        EXPECT_TRUE(outcome.stop_status.result.first == "body_link" ||
+                    outcome.stop_status.result.second == "body_link")
+            << outcome.name;
+        EXPECT_TRUE(outcome.recovery_succeeded)
+            << outcome.name << ": " << outcome.recovery.message;
+        EXPECT_FALSE(outcome.recovered_status.stop_latched) << outcome.name;
+        EXPECT_EQ(outcome.recovered_status.recovery_state,
+                  task::ProtectiveRecoveryState::SUCCEEDED)
+            << outcome.name;
+        EXPECT_GE(outcome.recovered_status.result.minimum_distance_m, 0.025)
+            << outcome.name;
+        EXPECT_FALSE(outcome.protective_stopped) << outcome.name;
+        EXPECT_FALSE(outcome.emergency_stopped) << outcome.name;
+        EXPECT_EQ(outcome.safety_mode, SafetyMode::Normal)
+            << outcome.name;
+    };
+    expect_protective_stop(move_j_outcome);
+    expect_protective_stop(move_l_outcome);
+    expect_protective_stop(speed_l_outcome);
+
+    EXPECT_EQ(move_j_outcome.collision_move.code,
+              ArmErrorCode::RobotInProtectiveStop);
+    EXPECT_GT(maxPositionError(move_j_outcome.final_position, kTorsoCollisionPose), 0.02);
+    EXPECT_EQ(move_l_outcome.collision_move.code,
+              ArmErrorCode::RobotInProtectiveStop);
+    EXPECT_GT(translationError(move_l_outcome.final_tcp, move_l_target), 0.01);
+    EXPECT_TRUE(speed_l_outcome.collision_move.ok())
+        << speed_l_outcome.collision_move.message;
+    EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
+}
+
+TEST_F(MotorRobotArmGen2MujocoTest, SpeedL)
+{
+    constexpr auto kCommandDuration = std::chrono::seconds(2);
+    MuJocoViewer viewer(world_);
+    viewer.setupCamera(2.5, -160.0, -20.0);
+    ScenarioOutcome outcome;
+    std::array<double, 6> measured_deltas{};
+
+    std::thread scenario([&] {
+        try {
+            if (!world_ || !world_->isRunning()) {
+                throw std::runtime_error("MuJoCo world is not running");
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            MotionOptions joint_options;
+            joint_options.velocity = 0.6;
+            joint_options.acceleration = 2.0;
+            outcome.move_j = arm_->moveJ(
+                JointPositionCommand{kSetupPose}, joint_options);
+            waitFor([&] {
+                return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
+            }, std::chrono::seconds(3));
+            outcome.move_j_error = maxPositionError(
+                arm_->getJointState().position, kSetupPose);
+            if (!outcome.move_j.ok()) {
+                throw std::runtime_error(outcome.move_j.message);
+            }
+
+            struct SpeedStep {
+                const char* name;
+                CartesianVelocity command;
+                bool angular;
+                std::size_t axis;
+            };
+            const std::array<SpeedStep, 6> steps{{
+                {"+X", CartesianVelocity{0.05, 0.0, 0.0, 0.0, 0.0, 0.0}, false, 0},
+                {"+Y", CartesianVelocity{0.0, 0.05, 0.0, 0.0, 0.0, 0.0}, false, 1},
+                {"+Z", CartesianVelocity{0.0, 0.0, 0.05, 0.0, 0.0, 0.0}, false, 2},
+                {"+RX", CartesianVelocity{0.0, 0.0, 0.0, 0.30, 0.0, 0.0}, true, 0},
+                {"+RY", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.30, 0.0}, true, 1},
+                {"+RZ", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.0, 0.30}, true, 2},
+            }};
+
+            for (std::size_t i = 0; i < steps.size(); ++i) {
+                const auto& step = steps[i];
+                if (i > 0) {
+                    outcome.move_j = arm_->moveJ(
+                        JointPositionCommand{kSetupPose}, joint_options);
+                    if (!outcome.move_j.ok()) {
+                        throw std::runtime_error(
+                            std::string("moveJ before speedL ") + step.name +
+                            ": " + outcome.move_j.message);
+                    }
+                    waitFor([&] {
+                        return maxPositionError(
+                                   arm_->getJointState().position, kSetupPose) < 0.04;
+                    }, std::chrono::seconds(3));
+                    std::this_thread::sleep_for(std::chrono::milliseconds(300));
+                }
+
+                const CartesianPose start = arm_->getTcpPose();
+                const Result speed_result = arm_->speedL(
+                    step.command, 0.5, 0.0, FrameType::Base);
+                if (!speed_result.ok()) {
+                    throw std::runtime_error(
+                        std::string("speedL ") + step.name + ": " +
+                        speed_result.message);
+                }
+
+                std::this_thread::sleep_for(kCommandDuration);
+                const Result stop_result = arm_->stopL(0.5);
+                if (!stop_result.ok()) {
+                    throw std::runtime_error(
+                        std::string("stopL ") + step.name + ": " +
+                        stop_result.message);
+                }
+                waitFor([&] { return !arm_->busy(); }, std::chrono::seconds(3));
+
+                const CartesianPose end = arm_->getTcpPose();
+                if (step.angular) {
+                    measured_deltas[i] = baseRotationDelta(start, end)[step.axis];
+                    std::cout << "[MotorRobotArmGen2MujocoTest] speedL "
+                              << step.name << " rotation delta: "
+                              << measured_deltas[i] << " rad" << std::endl;
+                } else {
+                    const Eigen::Vector3d translation_delta{
+                        end.x - start.x, end.y - start.y, end.z - start.z};
+                    measured_deltas[i] = translation_delta[step.axis];
+                    std::cout << "[MotorRobotArmGen2MujocoTest] speedL "
+                              << step.name << " translation delta: "
+                              << measured_deltas[i] << " m" << std::endl;
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(500));
+            }
+        } catch (const std::exception& error) {
+            outcome.worker_error = error.what();
+        }
+
+        std::this_thread::sleep_for(std::chrono::seconds(3));
+        viewer.requestStop();
+    });
+
+    viewer.setRunning(true);
+    viewer.run();
+    scenario.join();
+
+    EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
+    EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
+    EXPECT_LT(outcome.move_j_error, 0.08);
+    for (std::size_t i = 0; i < 3; ++i) {
+        EXPECT_GT(measured_deltas[i], 0.02);
+    }
+    for (std::size_t i = 3; i < measured_deltas.size(); ++i) {
+        EXPECT_GT(measured_deltas[i], 0.10);
+    }
+}
+
+} // namespace
+} // namespace cmvr::device

cmvr-es/devices/arm/robot_arm.h

@@ -28,12 +28,30 @@ public:
     virtual SafetyMode getSafetyMode() const = 0;
     virtual ControlMode getControlMode() const = 0;
 
+    // Queued actions require synchronous completion, cooperative cancellation
+    // at the final device-submission boundary, and a bounded typed Stop which
+    // returns success only after controller idle is confirmed. Backends must
+    // opt in only after all of these semantics have been validated.
+    virtual bool supportsActionQueueMotion() const noexcept { return false; }
+
+    // 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;
 
     virtual Result emergencyStop() = 0;
     virtual Result protectiveStop() = 0;
+    virtual Result recoverProtectiveStop(
+        const JointTrajectory& path,
+        const MotionOptions& options) = 0;
     virtual Result setSpeedScaling(double scaling) = 0;
     virtual double getSpeedScaling() const = 0;
     virtual bool isProtectiveStopped() const = 0;
@@ -73,6 +91,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,189 @@
+#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 recoverProtectiveStop(
+        const JointTrajectory&,
+        const MotionOptions&) override
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "protective recovery is not implemented for UmeRobotArm");
+    }
+    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