feat(motor): initialize motor groups by device entry

(cherry picked from commit 78fd7d7a04)

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/arm_control/include/cartesian_velocity_controller.h

@@ -53,6 +53,8 @@ public:
 private:
     void ensureWorkerStarted_();
     void workerLoop_();
+    void requestStop_(std::optional<double> acceleration = std::nullopt);
+    void abortCommand_();
     void sendZero_();
 
     static double velocityNorm_(const std::vector<double>& velocity);

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp

@@ -61,8 +61,13 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
     if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
         return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
     }
-    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+    if (!worker_ || !worker_->joinable()) {
+        if (busy_.exchange(true)) {
+            return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+        }
+    } else {
+        // speedL is a streaming command: an existing worker may receive a new target.
+        busy_.store(true);
     }
 
     ensureWorkerStarted_();
@@ -103,15 +108,7 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        target_twist_ = {};
-        target_frame_ = FrameType::Base;
-        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
-        command_active_ = true;
-        ++command_version_;
-    }
-    cv_.notify_all();
+    requestStop_(acceleration);
     return Result::success();
 }
 
@@ -192,27 +189,26 @@ void CartesianVelocityController::workerLoop_()
             }
 
             if (!planner_->updateSpeedLAcceleration(acceleration)) {
-                if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
-                    std::lock_guard<std::mutex> lock(mutex_);
-                    command_active_ = false;
-                    sendZero_();
-                    busy_.store(false);
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
                     break;
                 }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
-                sendZero_();
-                busy_.store(false);
-                return;
+                requestStop_();
+                continue;
             }
 
             std::vector<double> q_now;
             std::vector<double> qd_now;
             if (!read_state_(q_now, qd_now)) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
-                sendZero_();
-                busy_.store(false);
-                return;
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             std::vector<double> qd_cmd;
@@ -222,9 +218,12 @@ void CartesianVelocityController::workerLoop_()
                                 << target_twist.vz << ", " << target_twist.wx << ", "
                                 << target_twist.wy << ", " << target_twist.wz
                                 << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
-                sendZero_();
-                busy_.store(false);
-                return;
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             JointVelocityCommand velocity_command;
@@ -233,9 +232,12 @@ void CartesianVelocityController::workerLoop_()
             if (!send_result.ok()) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
                                 << send_result.message;
-                sendZero_();
-                busy_.store(false);
-                return;
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                    abortCommand_();
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             if (twistNorm_(target_twist) < config_.stop_twist_norm &&
@@ -260,6 +262,32 @@ void CartesianVelocityController::workerLoop_()
     busy_.store(false);
 }
 
+void CartesianVelocityController::requestStop_(const std::optional<double> acceleration)
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+        target_acceleration_ = acceleration.has_value() ? *acceleration
+                                                         : config_.stop_acceleration;
+        command_active_ = true;
+        ++command_version_;
+    }
+    cv_.notify_all();
+}
+
+void CartesianVelocityController::abortCommand_()
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        command_active_ = false;
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+    }
+    sendZero_();
+    busy_.store(false);
+}
+
 void CartesianVelocityController::sendZero_()
 {
     if (!send_velocity_) {

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/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp

@@ -932,7 +932,11 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         qdot = applyJointAccelerationLimits_(qdot, reference, dt);
     }
     const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
+    // During a stop, the limiter intentionally commands a near-zero residual
+    // twist while the measured arm can still be moving in a different direction.
+    // Direction and speed-ratio checks are not meaningful for that transient.
+    if (!is_stop_command &&
+        !validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
                                                      achieved_twist_base,
                                                      toEigenVector(q_measured),
                                                      qdot)) {

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

cmvr-es/common/CMakeLists.txt

@@ -29,6 +29,12 @@ target_link_libraries(common PUBLIC
 add_library(cmvr_es::common ALIAS common)
 install(TARGETS common LIBRARY DESTINATION lib)
 
+add_executable(support_functions_test
+        math/support_functions_test.cpp
+)
+target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es)
+target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog)
+
 #add_executable(image_display_test
 #        utils/visualization/image_display_test.cpp
 #)

cmvr-es/common/math/support_functions.h

@@ -3,6 +3,7 @@
 //
 
 #pragma once
+#include <cstdint>
 #include <cmath>
 #include <vector>
 #include <algorithm>
@@ -14,6 +15,25 @@ class SupportFunctions {
 private:
     static constexpr double EPS = 1e-9;
 public:
+    static constexpr std::int64_t absoluteDifference(const std::int32_t lhs,
+                                                     const std::int32_t rhs) noexcept {
+        return lhs >= rhs
+                   ? static_cast<std::int64_t>(lhs) - static_cast<std::int64_t>(rhs)
+                   : static_cast<std::int64_t>(rhs) - static_cast<std::int64_t>(lhs);
+    }
+
+    static constexpr std::int64_t cyclicAbsoluteDifference(
+        const std::int32_t lhs,
+        const std::int32_t rhs,
+        const std::int64_t period) noexcept {
+        const auto linear_distance = absoluteDifference(lhs, rhs);
+        if (period <= 0) {
+            return linear_distance;
+        }
+        const auto wrapped_distance = linear_distance % period;
+        return std::min(wrapped_distance, period - wrapped_distance);
+    }
+
     static  std::vector<double> eigen_to_vector(const Eigen::VectorXd &v) {
         return std::vector<double>(v.data(), v.data() + v.size());
     }

cmvr-es/common/math/support_functions_test.cpp

@@ -0,0 +1,34 @@
+#include <cstdint>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+#include "common/math/support_functions.h"
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceTreatsFullTurnsAsEquivalent)
+{
+    constexpr std::int64_t period = 65536LL * 101LL;
+
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(5254257, -1364879, period), 0);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(-10883488, -17502624, period), 0);
+}
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceUsesShortestWrappedDistance)
+{
+    constexpr std::int64_t period = 100;
+
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(3, 97, period), 6);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(97, 3, period), 6);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(10, 40, period), 30);
+}
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceHandlesInt32Range)
+{
+    constexpr std::int64_t period = 65536LL * 101LL;
+
+    const auto distance = SupportFunctions::cyclicAbsoluteDifference(
+        std::numeric_limits<std::int32_t>::min(),
+        std::numeric_limits<std::int32_t>::max(), period);
+    EXPECT_GE(distance, 0);
+    EXPECT_LE(distance, period / 2);
+}

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

@@ -112,6 +112,14 @@ struct JointGroupState {
     }
 };
 
+struct JointTrajectoryPoint {
+    double time_s{0.0};
+    std::vector<double> position;
+    std::vector<double> velocity;
+};
+
+using JointTrajectory = std::vector<JointTrajectoryPoint>;
+
 struct JointPositionCommand {
     std::vector<double> position;
 

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

@@ -3,8 +3,8 @@ arm {
     id: "right_arm"
 
     motor {
-      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can"
+      motor_system_id: "right_arm_can_motors"
+      motor_group_ids: "right_arm_can_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"

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: "right_arm_mujoco_motors"
+      motor_group_ids: "right_arm_mujoco_motors"
+      dof: 7
+      joint_names: "right_arm_J1"
+      joint_names: "right_arm_J2"
+      joint_names: "right_arm_J3"
+      joint_names: "right_arm_J4"
+      joint_names: "right_arm_J5"
+      joint_names: "right_arm_J6"
+      joint_names: "right_arm_J7"
+      upd_freq: 1000
+      buffer_size: 50
+      default_vel: 0.6
+      default_acc: 2.0
+    }
+
+    kinematics {
+      pinocchio_dls_ik_solver {
+        urdf_path: "model/gen2/robot.urdf"
+        base_frame_name: "body_link"
+        flange_frame_name: "arm_link_7_2"
+        max_iters: 200
+        pos_eps: 1e-6
+        rot_eps: 1e-6
+        damping: 1e-5
+        joint_limit_policy {
+          limits {
+            enable: true
+            source: JOINT_LIMIT_SOURCE_CUSTOM
+            joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+            joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.15
+            max_push: 0.25
+            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/arm_mujoco.pb.txt

@@ -3,8 +3,8 @@ arm {
     id: "mujoco_right_arm"
 
     motor {
-      motor_system_id: "mujoco_motors"
-      motor_group_ids: "mujoco_right_arm"
+      motor_system_id: "right_arm_mujoco_motors"
+      motor_group_ids: "right_arm_mujoco_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"

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

@@ -3,8 +3,8 @@ arm {
     id: "mujoco_right_arm"
 
     motor {
-      motor_system_id: "mujoco_motors"
-      motor_group_ids: "mujoco_right_arm"
+      motor_system_id: "right_arm_mujoco_motors"
+      motor_group_ids: "right_arm_mujoco_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"

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

@@ -3,8 +3,8 @@ arm {
     id: "right_arm"
 
     motor {
-      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can"
+      motor_system_id: "right_arm_can_motors"
+      motor_group_ids: "right_arm_can_motors"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"

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

@@ -83,8 +83,8 @@ camera {
         stream_mode: STREAM_MODE_RGBD
       }
       encoder {
-        width: 1280
-        height: 720
+        width: 480
+        height: 320
         fps: 30
         codec: "H264"
         enable_stream_timestamp: true

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

@@ -38,4 +38,9 @@ dexhand {
       auto_calibrate: false
     }
   }
+
+  dexhands {
+    id: "mujoco_zero_touch_dexhand"
+    zero_sim_touch {}
+  }
 }

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

@@ -2,7 +2,7 @@ motor {
   id: "ethercat_motors"
 
   motor_groups {
-    id: "right_arm_ethercat"
+    id: "right_arm_ethercat_motors"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@@ -14,13 +14,20 @@ motor {
       slave_state_poll_period_ms: 10
 
       cia402 {
-        profile_position_trigger_delay_ms: 2
         state_transition_timeout_ms: 1200
         velocity_stop_timeout_ms: 2000
         status_poll_period_ms: 10
         stopped_velocity_tolerance_rad_s: 0.001
       }
 
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 1000
+      }
+
       dc {
         enable: true
         reference_motor_id: 1

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

@@ -2,7 +2,7 @@ motor {
   id: "ethercat_motors"
 
   motor_groups {
-    id: "right_arm_ethercat"
+    id: "right_arm_ethercat_motors"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@@ -14,13 +14,20 @@ motor {
       slave_state_poll_period_ms: 10
 
       cia402 {
-        profile_position_trigger_delay_ms: 2
         state_transition_timeout_ms: 1200
         velocity_stop_timeout_ms: 2000
         status_poll_period_ms: 10
         stopped_velocity_tolerance_rad_s: 0.001
       }
 
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 1000
+      }
+
       dc {
         enable: false
         reference_motor_id: 1

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

@@ -2,7 +2,7 @@ motor {
   id: "mujoco_motors"
 
   motor_groups {
-    id: "mujoco_right_arm"
+    id: "right_arm_mujoco_motors"
     bus_type: MOTOR_BUS_MUJOCO
     vendor: MOTOR_VENDOR_MUJOCO
     protocol: MOTOR_PROTOCOL_MUJOCO

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

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

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

@@ -2,7 +2,7 @@ motor {
   id: "ti5_motors"
 
   motor_groups {
-    id: "left_arm_can"
+    id: "left_arm_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -26,7 +26,7 @@ motor {
   }
 
   motor_groups {
-    id: "right_arm_can"
+    id: "right_arm_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -56,7 +56,7 @@ motor {
   }
 
   motor_groups {
-    id: "head_can"
+    id: "head_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -78,7 +78,7 @@ motor {
   }
 
   motor_groups {
-    id: "waist_can"
+    id: "waist_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN

cmvr-es/config/logger/logger.pb.txt

@@ -30,7 +30,7 @@ logger {
   max_file_size_mb: 100
   flush_interval_seconds: 1
   format {
-    show_time: false
+    show_time: true
     show_level: true
     show_thread_id: false
     show_source_location: true

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

@@ -2,41 +2,39 @@ device_manager {
   name: "cmvr_es"
   version: "0.1"
   description: "cmvr edge system version 0.1"
-  init_all_motors_when_no_active_joints: true
-
   devices {
     id: "mujoco_world"
     type: DEVICE_TYPE_MUJOCO_WORLD
     config_file: "devices/mujoco/mujoco_world.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
-    id: "mujoco_motors"
+    id: "right_arm_mujoco_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/mujoco_motors.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm_mujoco_qp.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_viewer"
     type: DEVICE_TYPE_MUJOCO_VIEWER
     config_file: "devices/mujoco/mujoco_viewer.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
@@ -69,14 +67,42 @@ device_manager {
   }
 
   devices {
-    id: "ti5_motors"
+    id: "mujoco_zero_touch_dexhand"
+    type: DEVICE_TYPE_DEXHAND
+    config_file: "devices/dexhand/dexhand.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "left_arm_can_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
     enable: false
   }
 
   devices {
-    id: "ethercat_motors"
+    id: "right_arm_can_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "head_can_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "waist_can_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "right_arm_ethercat_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ethercat_motors.pb.txt"
     enable: false
@@ -100,7 +126,7 @@ device_manager {
     id: "huayan_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/huayan_arm.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {

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

@@ -4,8 +4,8 @@ task_manager {
     type: TASK_TYPE_TOUCH_SCREEN
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.001
-    config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
-    enable: false
+    config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"
+    enable: true
   }
   tasks {
     id: "grpc_server"
@@ -20,6 +20,6 @@ task_manager {
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.002
     config_file: "tasks/self_collision_task/self_collision_task.pb.txt"
-    enable: true
+    enable: 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/touch_screen_task/touch_screen_task_mujoco.pb.txt

@@ -2,9 +2,9 @@ touch_screen_task {
   id: "touch_screen"
 
   devices {
-    arm_id: "right_arm_mujoco"
+    arm_id: "mujoco_right_arm"
     dexhand_id: "mujoco_zero_touch_dexhand"
-    camera_id: "hand_cam"
+    camera_id: "mujoco_hand_cam"
   }
 
   initialization {
@@ -90,8 +90,8 @@ touch_screen_task {
   }
 
   retract {
-    twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 8.0
-    duration_s: 5.0
+    twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
+    acceleration: 5.0
+    duration_s: 2.0
   }
 }

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

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

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

@@ -36,6 +36,14 @@ public:
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
     Result protectiveStop() override { return emergencyStop(); }
+    Result recoverProtectiveStop(
+        const JointTrajectory&,
+        const MotionOptions&) override
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "protective recovery is not implemented for AuboArm");
+    }
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
     bool isProtectiveStopped() const override { return false; }

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

@@ -42,6 +42,14 @@ public:
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
     Result protectiveStop() override { return emergencyStop(); }
+    Result recoverProtectiveStop(
+        const JointTrajectory&,
+        const MotionOptions&) override
+    {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "protective recovery is not implemented for HuayanRobot");
+    }
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
     bool isProtectiveStopped() const override;

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

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

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

@@ -41,11 +41,14 @@ public:
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override { return emergencyStop(); }
+    Result protectiveStop() override;
+    Result recoverProtectiveStop(
+        const JointTrajectory& path,
+        const MotionOptions& options) override;
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
-    bool isProtectiveStopped() const override { return false; }
-    bool isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isProtectiveStopped() const override { return protective_stopped_.load(); }
+    bool isEmergencyStopped() const override { return emergency_stopped_.load(); }
     bool isFault() const override { return false; }
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
@@ -76,7 +79,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 +97,10 @@ public:
 
 private:
     bool containsJoint_(const std::string& joint_name) const;
+    bool safetyStopRequested_() const;
+    std::optional<Result> safetyStopResult_(const std::string& command,
+                                            bool interrupted = false) const;
+    Result quickStopMotors_();
     bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const;
     bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const;
     std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const;
@@ -126,7 +133,10 @@ private:
     mutable std::mutex mutex_;
     std::atomic<bool> busy_{false};
     double speed_scaling_{1.0};
-    bool emergency_stopped_{false};
+    std::atomic<bool> protective_stopped_{false};
+    std::atomic<bool> emergency_stopped_{false};
+    std::atomic<bool> protective_recovery_active_{false};
+    std::atomic<bool> protective_recovery_cancel_requested_{false};
     ServoOptions servo_options_;
 };
 

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

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

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

@@ -10,7 +10,6 @@
 #include <memory>
 #include <string>
 #include <thread>
-#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -147,17 +146,10 @@ protected:
             (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
             &motor_root_config));
 
-        std::unordered_set<std::string> right_arm_joints;
-        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());
+        motor_system_ = std::make_shared<MotorManager>(
+            "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
         ASSERT_NO_THROW(motor_system_->init());
-        world_ = MotorManager::mujocoWorldFor("mujoco_motors");
+        world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
         ASSERT_TRUE(world_);
         ASSERT_TRUE(world_->isLoaded());
 
@@ -187,7 +179,6 @@ protected:
         if (world_device_) {
             world_device_->stop();
         }
-        MotorManager::clearActiveJoints();
     }
 
     std::filesystem::path project_root_;

cmvr-es/devices/arm/robot_arm.h

@@ -34,6 +34,9 @@ public:
 
     virtual Result emergencyStop() = 0;
     virtual Result protectiveStop() = 0;
+    virtual Result recoverProtectiveStop(
+        const JointTrajectory& path,
+        const MotionOptions& options) = 0;
     virtual Result setSpeedScaling(double scaling) = 0;
     virtual double getSpeedScaling() const = 0;
     virtual bool isProtectiveStopped() const = 0;

cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h

@@ -5,10 +5,13 @@
 #pragma once
 
 #include <cstdint>
+#include <condition_variable>
+#include <chrono>
 #include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include <mujoco/mujoco.h>
@@ -57,6 +60,11 @@ private:
     bool initOffscreen_();
     void destroyOffscreen_();
     bool renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
+    void renderLoop_();
+    bool fetchCached_(cv::Mat& color,
+                      cv::Mat& depth,
+                      Rs2Intrinsics& intrinsics,
+                      bool consume_new_frame_only);
     bool ensureEncoder_(int width, int height, int fps);
     void setError_(const std::string& error);
     static void flipRgbAndDepth_(std::vector<unsigned char>& rgb,
@@ -65,6 +73,14 @@ private:
                                  int height);
     static void linearizeDepth_(const mjModel* model, std::vector<float>& depth);
 
+    struct CachedFrame {
+        cv::Mat color;
+        cv::Mat depth;
+        Rs2Intrinsics intrinsics{};
+        uint64_t frame_id{0};
+        bool valid{false};
+    };
+
 private:
     FetchRgbdFn fetch_rgbd_fn_;
     mutable std::mutex mtx_;
@@ -78,6 +94,7 @@ private:
     mjrContext context_{};
     bool scene_initialized_{false};
     bool context_initialized_{false};
+    mjData* render_data_{nullptr};
     int camera_id_{-1};
     int width_{640};
     int height_{480};
@@ -92,6 +109,13 @@ private:
     size_t stream_frame_index_{0};
     bool streaming_{false};
     std::shared_ptr<FfmpegEncoderInfo> rgb_encoder_;
+
+    mutable std::mutex cache_mtx_;
+    std::condition_variable cache_cv_;
+    CachedFrame latest_frame_;
+    std::thread render_thread_;
+    bool render_thread_running_{false};
+    bool render_stop_requested_{false};
 };
 
 } // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -106,10 +106,6 @@ bool MujocoCamera::init()
         fovy_deg_ = model->cam_fovy[camera_id_];
     }
 
-    if (!initOffscreen_()) {
-        return false;
-    }
-
     state_.is_initialized = true;
     state_.is_opened = true;
     state_.fps = positiveOrDefault(config_.render().fps(), 30);
@@ -125,37 +121,132 @@ bool MujocoCamera::init()
 
 bool MujocoCamera::start()
 {
-    if (!state_.is_initialized) {
+    bool initialized = false;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        initialized = state_.is_initialized;
+    }
+    if (!initialized) {
         if (!init()) {
             return false;
         }
     }
-    std::lock_guard<std::mutex> lock(mtx_);
+
     auto world = world_.lock();
     if (world && !world->isRunning() && !world->start()) {
         setError_("[MujocoCamera] failed to start MuJoCo world: " + world->lastError());
         return false;
     }
-    state_.is_streaming = true;
-    state_.is_opened = true;
+
+    bool use_external_frames = false;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        state_.is_streaming = true;
+        state_.is_opened = true;
+        use_external_frames = static_cast<bool>(fetch_rgbd_fn_);
+    }
+    std::thread stale_thread;
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        if (render_thread_running_) {
+            return true;
+        }
+        latest_frame_ = CachedFrame{};
+        last_frame_id_ = 0;
+        has_last_frame_id_ = false;
+        render_stop_requested_ = false;
+        render_thread_running_ = true;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        stale_thread = std::move(render_thread_);
+    }
+    if (stale_thread.joinable()) {
+        stale_thread.join();
+    }
+
+    try {
+        std::lock_guard<std::mutex> lock(mtx_);
+        render_thread_ = std::thread(&MujocoCamera::renderLoop_, this);
+    } catch (const std::exception& e) {
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            render_thread_running_ = false;
+            render_stop_requested_ = true;
+        }
+        cache_cv_.notify_all();
+        setError_("[MujocoCamera] failed to start render thread: " + std::string(e.what()));
+        return false;
+    }
+
+    // External PiP callbacks are not ready until the viewer enters its render
+    // loop, so let their polling thread warm up asynchronously.
+    if (use_external_frames) {
+        return true;
+    }
+
+    std::unique_lock<std::mutex> cache_lock(cache_mtx_);
+    const bool ready = cache_cv_.wait_for(
+        cache_lock,
+        std::chrono::seconds(5),
+        [this] { return latest_frame_.valid || !render_thread_running_ || render_stop_requested_; });
+    const bool has_frame = latest_frame_.valid;
+    cache_lock.unlock();
+    if (!ready || !has_frame) {
+        stop();
+        if (ready) {
+            setError_("[MujocoCamera] render thread stopped before producing a frame");
+        } else {
+            setError_("[MujocoCamera] timed out waiting for the first rendered frame");
+        }
+        return false;
+    }
     return true;
 }
 
 bool MujocoCamera::stop()
 {
-    std::lock_guard<std::mutex> lock(mtx_);
-    state_.is_streaming = false;
-    state_.is_opened = false;
-    destroyOffscreen_();
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_stop_requested_ = true;
+    }
+    cache_cv_.notify_all();
+
+    std::thread thread_to_join;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        state_.is_streaming = false;
+        state_.is_opened = false;
+        thread_to_join = std::move(render_thread_);
+    }
+    if (thread_to_join.joinable()) {
+        thread_to_join.join();
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_running_ = false;
+        latest_frame_ = CachedFrame{};
+    }
+    cache_cv_.notify_all();
     return true;
 }
 
 void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn)
 {
+    bool render_thread_active = false;
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_active = render_thread_running_;
+    }
+    if (render_thread_active) {
+        stop();
+    }
+
     std::lock_guard<std::mutex> lock(mtx_);
     fetch_rgbd_fn_ = std::move(fetch_rgbd_fn);
     if (fetch_rgbd_fn_) {
-        destroyOffscreen_();
         state_.is_initialized = true;
         state_.is_opened = true;
         state_.fps = positiveOrDefault(config_.render().fps(), 30);
@@ -203,9 +294,10 @@ void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics&
 
 bool MujocoCamera::startStreaming()
 {
-    if (!state_.is_initialized && !init()) {
+    if (!start()) {
         return false;
     }
+
     std::lock_guard<std::mutex> lock(mtx_);
     streaming_ = true;
     state_.is_streaming = true;
@@ -273,14 +365,29 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
 
 bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
 {
-    std::lock_guard<std::mutex> lock(mtx_);
-    if (fetch_rgbd_fn_) {
+    FetchRgbdFn fetch_rgbd_fn;
+    bool consume_new_frame_only = false;
+    bool render_thread_active = false;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        fetch_rgbd_fn = fetch_rgbd_fn_;
+        consume_new_frame_only = consume_new_frame_only_;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_active = render_thread_running_;
+    }
+
+    // Keep compatibility with callback-only cameras that have not been
+    // started. Once start() owns a polling thread, reads are cache-only.
+    if (fetch_rgbd_fn && !render_thread_active) {
         std::vector<unsigned char> rgb_raw;
         std::vector<float> depth_raw;
         int width = 0;
         int height = 0;
         std::uint64_t frame_id = 0;
-        if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
+        if (!fetch_rgbd_fn(rgb_raw, depth_raw, width, height, frame_id)) {
             return false;
         }
         if (width <= 0 || height <= 0) {
@@ -292,11 +399,14 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
         if (!depth_raw.empty() && static_cast<int>(depth_raw.size()) != width * height) {
             return false;
         }
-        if (consume_new_frame_only_ && has_last_frame_id_ && frame_id == last_frame_id_) {
-            return false;
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            if (consume_new_frame_only && has_last_frame_id_ && frame_id == last_frame_id_) {
+                return false;
+            }
+            last_frame_id_ = frame_id;
+            has_last_frame_id_ = true;
         }
-        last_frame_id_ = frame_id;
-        has_last_frame_id_ = true;
 
         cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
         cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
@@ -310,7 +420,31 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
         return true;
     }
 
-    return renderOffscreen_(color, depth, intrinsics);
+    return fetchCached_(color, depth, intrinsics, consume_new_frame_only);
+}
+
+bool MujocoCamera::fetchCached_(cv::Mat& color,
+                                cv::Mat& depth,
+                                Rs2Intrinsics& intrinsics,
+                                const bool consume_new_frame_only)
+{
+    std::lock_guard<std::mutex> lock(cache_mtx_);
+    if (!latest_frame_.valid || latest_frame_.color.empty()) {
+        return false;
+    }
+    if (consume_new_frame_only && has_last_frame_id_ &&
+        latest_frame_.frame_id == last_frame_id_) {
+        return false;
+    }
+
+    // cv::Mat copies are reference-counted; keep the cache immutable while the
+    // consumer reads the published frame and avoid a full image copy per poll.
+    color = latest_frame_.color;
+    depth = latest_frame_.depth;
+    intrinsics = latest_frame_.intrinsics;
+    last_frame_id_ = latest_frame_.frame_id;
+    has_last_frame_id_ = true;
+    return !color.empty();
 }
 
 bool MujocoCamera::initOffscreen_()
@@ -345,10 +479,25 @@ bool MujocoCamera::initOffscreen_()
         return false;
     }
 
-    std::lock_guard<std::mutex> world_lock(world->mutex());
-    const mjModel* model = world->model();
-    if (model == nullptr) {
-        setError_("[MujocoCamera] world model is null");
+    mjModel* model = nullptr;
+    {
+        std::lock_guard<std::mutex> world_lock(world->mutex());
+        model = world->model();
+        if (model == nullptr) {
+            setError_("[MujocoCamera] world model is null");
+            return false;
+        }
+
+        // MuJoCo clips rendering to the model's offscreen buffer. Make sure
+        // the buffer is large enough before creating this camera's context;
+        // otherwise a larger requested frame is only partially populated.
+        model->vis.global.offwidth = std::max(model->vis.global.offwidth, width_);
+        model->vis.global.offheight = std::max(model->vis.global.offheight, height_);
+    }
+
+    render_data_ = mj_makeData(model);
+    if (render_data_ == nullptr) {
+        setError_("[MujocoCamera] failed to allocate render data");
         return false;
     }
     mjv_makeScene(model, &scene_, kMaxGeom);
@@ -360,9 +509,116 @@ bool MujocoCamera::initOffscreen_()
         setError_("[MujocoCamera] MuJoCo offscreen buffer is not available");
         return false;
     }
+    if (context_.offWidth < width_ || context_.offHeight < height_) {
+        setError_("[MujocoCamera] offscreen buffer is smaller than requested frame: " +
+                  std::to_string(context_.offWidth) + "x" +
+                  std::to_string(context_.offHeight) + " < " +
+                  std::to_string(width_) + "x" + std::to_string(height_));
+        return false;
+    }
     return true;
 }
 
+void MujocoCamera::renderLoop_()
+{
+    FetchRgbdFn external_fetch;
+    {
+        std::lock_guard<std::mutex> lock(mtx_);
+        external_fetch = fetch_rgbd_fn_;
+    }
+
+    const bool use_external_frames = static_cast<bool>(external_fetch);
+    if (!use_external_frames && !initOffscreen_()) {
+        destroyOffscreen_();
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            render_thread_running_ = false;
+        }
+        cache_cv_.notify_all();
+        return;
+    }
+
+    const int fps = positiveOrDefault(config_.render().fps(), 30);
+    const auto period = std::chrono::duration<double>(1.0 / static_cast<double>(fps));
+    const auto period_ticks = std::chrono::duration_cast<std::chrono::steady_clock::duration>(period);
+    auto next_tick = std::chrono::steady_clock::now();
+
+    while (true) {
+        {
+            std::lock_guard<std::mutex> lock(cache_mtx_);
+            if (render_stop_requested_) {
+                break;
+            }
+        }
+
+        cv::Mat color;
+        cv::Mat depth;
+        Rs2Intrinsics intrinsics{};
+        uint64_t external_frame_id = 0;
+        bool got_frame = false;
+
+        if (use_external_frames) {
+            std::vector<unsigned char> rgb_raw;
+            std::vector<float> depth_raw;
+            int width = 0;
+            int height = 0;
+            if (external_fetch(rgb_raw, depth_raw, width, height, external_frame_id) &&
+                width > 0 && height > 0 &&
+                static_cast<int>(rgb_raw.size()) == width * height * 3 &&
+                (depth_raw.empty() || static_cast<int>(depth_raw.size()) == width * height)) {
+                cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
+                cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
+                if (!depth_raw.empty()) {
+                    cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
+                    depth = dep.clone();
+                }
+                fillIntrinsics(width, height, intrinsics);
+                got_frame = !color.empty();
+            }
+        } else {
+            got_frame = renderOffscreen_(color, depth, intrinsics);
+        }
+
+        if (got_frame) {
+            {
+                std::lock_guard<std::mutex> lock(cache_mtx_);
+                latest_frame_.color = std::move(color);
+                latest_frame_.depth = std::move(depth);
+                latest_frame_.intrinsics = intrinsics;
+                latest_frame_.frame_id = use_external_frames && external_frame_id != 0
+                    ? external_frame_id
+                    : latest_frame_.frame_id + 1;
+                latest_frame_.valid = true;
+            }
+            {
+                std::lock_guard<std::mutex> lock(mtx_);
+                clear_error_();
+            }
+            cache_cv_.notify_all();
+        }
+
+        next_tick += period_ticks;
+        std::unique_lock<std::mutex> lock(cache_mtx_);
+        if (cache_cv_.wait_until(lock, next_tick, [this] { return render_stop_requested_; })) {
+            break;
+        }
+
+        const auto now = std::chrono::steady_clock::now();
+        if (next_tick < now) {
+            next_tick = now + period_ticks;
+        }
+    }
+
+    if (!use_external_frames) {
+        destroyOffscreen_();
+    }
+    {
+        std::lock_guard<std::mutex> lock(cache_mtx_);
+        render_thread_running_ = false;
+    }
+    cache_cv_.notify_all();
+}
+
 void MujocoCamera::destroyOffscreen_()
 {
     if (window_ != nullptr) {
@@ -376,6 +632,10 @@ void MujocoCamera::destroyOffscreen_()
         mjv_freeScene(&scene_);
         scene_initialized_ = false;
     }
+    if (render_data_ != nullptr) {
+        mj_deleteData(render_data_);
+        render_data_ = nullptr;
+    }
     if (window_ != nullptr) {
         glfwDestroyWindow(window_);
         window_ = nullptr;
@@ -388,7 +648,8 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         setError_("[MujocoCamera] camera is not initialized: " + id_);
         return false;
     }
-    if (!initOffscreen_()) {
+    if (window_ == nullptr || !context_initialized_ || !scene_initialized_) {
+        setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_);
         return false;
     }
 
@@ -402,15 +663,27 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
     std::vector<unsigned char> rgb(static_cast<std::size_t>(width_) * height_ * 3);
     std::vector<float> depth_raw(static_cast<std::size_t>(width_) * height_);
 
+    mjModel* model = nullptr;
     {
-        std::lock_guard<std::mutex> world_lock(world->mutex());
-        mjModel* model = world->model();
-        mjData* data = world->data();
-        if (model == nullptr || data == nullptr) {
+        std::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
+        if (!world_lock.owns_lock()) {
+            // Never make the simulation wait for a camera frame. The next
+            // scheduled capture will use a newer state if this one is busy.
+            return false;
+        }
+        model = world->model();
+        const mjData* data = world->data();
+        if (model == nullptr || data == nullptr || render_data_ == nullptr) {
             setError_("[MujocoCamera] world model/data is null");
             return false;
         }
 
+        // Keep the world lock limited to the state copy. GPU rendering runs on
+        // the camera thread using its private data snapshot.
+        mjv_copyData(render_data_, model, data);
+    }
+
+    {
         camera_.type = mjCAMERA_FIXED;
         camera_.fixedcamid = camera_id_;
         camera_.trackbodyid = -1;
@@ -421,7 +694,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         viewport.width = width_;
         viewport.height = height_;
 
-        mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
+        mjv_updateScene(model, render_data_, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
         mjr_render(viewport, &scene_, &context_);
         mjr_readPixels(rgb.data(), depth_raw.data(), viewport, &context_);
         flipRgbAndDepth_(rgb, depth_raw, width_, height_);
@@ -429,13 +702,12 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
     }
 
     cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
-    color = rgb_mat.clone();
+    // mjr_readPixels returns RGB, while the rest of the camera API exposes
+    // OpenCV-compatible BGR frames (as UVC and RealSense do).
+    cv::cvtColor(rgb_mat, color, cv::COLOR_RGB2BGR);
     cv::Mat depth_mat(height_, width_, CV_32FC1, depth_raw.data());
     depth = depth_mat.clone();
     fillIntrinsics(width_, height_, intrinsics);
-    ++last_frame_id_;
-    has_last_frame_id_ = true;
-    clear_error_();
     return true;
 }
 

cmvr-es/devices/dexhand/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_subdirectory(rh56dftp_dexhand)
 add_subdirectory(px_6ax_gen3)
+add_subdirectory(zero_sim_touch_dexhand)
 
 add_library(dexhand INTERFACE)
 
@@ -9,6 +10,7 @@ target_link_libraries(dexhand
         INTERFACE
         cmvr_es::device::rh56dftp_dexhand
         cmvr_es::device::px_6ax_gen3
+        cmvr_es::device::zero_sim_touch_dexhand
         cmvr_es::proto
 )
 

cmvr-es/devices/dexhand/dexhand_factory.h

@@ -10,6 +10,7 @@
 #include "devices/dexhand/abstract_dexhand.h"
 #include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h"
 #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
+#include "devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h"
 
 namespace cmvr::device {
 
@@ -32,6 +33,10 @@ public:
             return std::make_shared<PX6AXGen3>(
                 backendWithId_(cfg.id(), cfg.px_6ax_gen3()));
 
+        case config::DexHandDeviceConfig::kZeroSimTouch:
+            return std::make_shared<ZeroSimTouchDexHand>(
+                backendWithId_(cfg.id(), cfg.zero_sim_touch()));
+
         case config::DexHandDeviceConfig::BACKEND_NOT_SET:
         default:
         {

cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt

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

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

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

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

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

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

@@ -1,6 +1,7 @@
 #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
 
 #include <algorithm>
+#include <array>
 #include <chrono>
 #include <cstdint>
 #include <iostream>
@@ -73,17 +74,42 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
         return;
     }
 
-    EXPECT_TRUE(runtime.writePdo<std::uint16_t>(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000));
-    EXPECT_TRUE(runtime.writePdo<std::int8_t>(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0));
+    const std::array command_writes{
+        EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
+            1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000),
+        EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
+            1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0),
+    };
+    auto invalid_writes = command_writes;
+    invalid_writes[1].bit_length = 16;
+
+    const auto generation_before = runtime.commandGeneration();
+    EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size()));
+    EXPECT_EQ(runtime.commandGeneration(), generation_before);
+    EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size()));
+    EXPECT_EQ(runtime.commandGeneration(), generation_before + 1);
 
     const int settle_ms = 1000;
     std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms));
+    EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1);
+    EXPECT_TRUE(runtime.isHealthy());
 
-    std::uint16_t statusword = 0;
-    EXPECT_TRUE(runtime.readPdo<std::uint16_t>(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword));
+    std::array feedback_reads{
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            1, msgs::CIA402_STATUS_WORD_6041, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
+            1, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
+    };
+    auto invalid_feedback_reads = feedback_reads;
+    invalid_feedback_reads[1].bit_length = 16;
+    EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(),
+                                        invalid_feedback_reads.size()));
+    ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size()));
 
-    std::int8_t mode_display = 0;
-    EXPECT_TRUE(runtime.readPdo<std::int8_t>(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display));
+    const auto statusword =
+        EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(feedback_reads[0]);
+    const auto mode_display =
+        EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(feedback_reads[1]);
 
     std::cout << "CIA402 statusword: 0x" << std::hex << statusword
               << ", mode display: " << std::dec << static_cast<int>(mode_display)

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

@@ -21,6 +21,7 @@ public:
     bool writeVelocityLimit(std::uint8_t node_id,
                             std::uint32_t velocity_limit) override;
     bool calibrateZero(std::uint8_t node_id,
+                       std::int64_t counts_per_joint_revolution,
                        std::int32_t& zeroed_position) override;
     bool brakeRelease(std::uint8_t node_id) override;
 

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

@@ -16,6 +16,7 @@ public:
     virtual bool writeVelocityLimit(std::uint8_t node_id,
                                     std::uint32_t velocity_limit) = 0;
     virtual bool calibrateZero(std::uint8_t node_id,
+                               std::int64_t counts_per_joint_revolution,
                                std::int32_t& zeroed_position) = 0;
     virtual bool brakeRelease(std::uint8_t node_id) = 0;
 };

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

@@ -90,8 +90,11 @@ bool EyouMotor::calibrateZeroQ()
         return false;
     }
 
+    const auto counts_per_joint_revolution = static_cast<std::int64_t>(
+        std::llround(encoder_counts_per_rev_ * gear_ratio_));
     std::int32_t zeroed_position = 0;
-    if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) {
+    if (!vendor_adapter_->calibrateZero(node_id_, counts_per_joint_revolution,
+                                        zeroed_position)) {
         CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name;
         return false;
     }

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

@@ -9,6 +9,7 @@
 
 #include "cmvr/msgs/cia402.pb.h"
 #include "common/base/logging/logger.h"
+#include "common/math/support_functions.h"
 #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h"
 
 namespace cmvr::device {
@@ -93,96 +94,250 @@ bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id,
 }
 
 bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id,
+                                      const std::int64_t counts_per_joint_revolution,
                                       std::int32_t& zeroed_position)
 {
     if (!bus_runtime_) {
         return false;
     }
 
+    const auto& zero_config = bus_runtime_->config().zero_calibration();
+    const auto home_offset_timeout =
+        std::chrono::milliseconds{zero_config.timeout_ms()};
+    const auto home_offset_poll_period =
+        std::chrono::milliseconds{zero_config.poll_period_ms()};
+    const auto home_offset_stable_samples = zero_config.stable_sample_count();
+    const auto home_offset_position_tolerance_counts =
+        zero_config.position_tolerance_counts();
+    const auto home_offset_stable_delta_counts =
+        zero_config.stable_delta_counts();
+
+    std::uint32_t original_soft_limit_state = 0;
+    std::int32_t original_home_offset = 0;
+    std::int32_t original_position = 0;
+    if (!bus_runtime_->readSdo<std::uint32_t>(
+            node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+            0x00, original_soft_limit_state) ||
+        !bus_runtime_->readSdo<std::int32_t>(
+            node_id, msgs::CIA402_HOME_OFFSET_607C,
+            0x00, original_home_offset) ||
+        !bus_runtime_->readSdo<std::int32_t>(
+            node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+            0x00, original_position)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to snapshot calibration state, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    // EYOU applies HomeOffset additively, so clearing it exposes this raw position.
+    const auto expected_cleared_position_wide =
+        static_cast<std::int64_t>(original_position) -
+        static_cast<std::int64_t>(original_home_offset);
+    if (expected_cleared_position_wide < std::numeric_limits<std::int32_t>::min() ||
+        expected_cleared_position_wide > std::numeric_limits<std::int32_t>::max()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] cleared position would overflow, node="
+                        << static_cast<int>(node_id)
+                        << ", original_position=" << original_position
+                        << ", original_home_offset=" << original_home_offset;
+        return false;
+    }
+    const auto expected_cleared_position =
+        static_cast<std::int32_t>(expected_cleared_position_wide);
+
+    const auto write_home_offset = [&](const std::int32_t value) {
+        return bus_runtime_->writeSdo<std::int32_t>(
+            node_id, msgs::CIA402_HOME_OFFSET_607C, 0x00, value);
+    };
+    const auto save_parameters = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(
+            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
+            0x01, 0x65766173);
+    };
+    const auto wait_for_soft_limit = [&](const std::uint32_t expected_state) {
+        const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
+        do {
+            std::uint32_t actual_soft_limit_state = 0;
+            if (bus_runtime_->readSdo<std::uint32_t>(
+                    node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                    0x00, actual_soft_limit_state) &&
+                actual_soft_limit_state == expected_state) {
+                return true;
+            }
+            std::this_thread::sleep_for(home_offset_poll_period);
+        } while (std::chrono::steady_clock::now() < deadline);
+        return false;
+    };
+    const auto restore_soft_limit = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(
+                   node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                   0x00, original_soft_limit_state) &&
+               wait_for_soft_limit(original_soft_limit_state);
+    };
+    const auto wait_for_position = [&](const char* phase,
+                                       const std::int32_t expected_offset,
+                                       const std::int32_t expected_position,
+                                       std::int32_t& observed_position) {
+        const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
+        std::uint32_t stable_samples = 0;
+        bool has_previous_position = false;
+        std::int32_t previous_position = 0;
+        std::int32_t observed_offset = 0;
+
+        do {
+            const bool read_ok =
+                bus_runtime_->readSdo<std::int32_t>(
+                    node_id, msgs::CIA402_HOME_OFFSET_607C,
+                    0x00, observed_offset) &&
+                bus_runtime_->readSdo<std::int32_t>(
+                    node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+                    0x00, observed_position);
+            const bool position_stable =
+                !has_previous_position ||
+                SupportFunctions::cyclicAbsoluteDifference(
+                    observed_position, previous_position,
+                    counts_per_joint_revolution) <= home_offset_stable_delta_counts;
+            const bool sample_matches =
+                read_ok && observed_offset == expected_offset &&
+                SupportFunctions::cyclicAbsoluteDifference(
+                    observed_position, expected_position,
+                    counts_per_joint_revolution) <= home_offset_position_tolerance_counts &&
+                position_stable;
+
+            stable_samples = sample_matches ? stable_samples + 1 : 0;
+            if (stable_samples >= home_offset_stable_samples) {
+                return true;
+            }
+
+            if (read_ok) {
+                previous_position = observed_position;
+                has_previous_position = true;
+            }
+            std::this_thread::sleep_for(home_offset_poll_period);
+        } while (std::chrono::steady_clock::now() < deadline);
+
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] timed out waiting for home offset state, node="
+                        << static_cast<int>(node_id)
+                        << ", phase=" << phase
+                        << ", expected_offset=" << expected_offset
+                        << ", actual_offset=" << observed_offset
+                        << ", expected_position=" << expected_position
+                        << ", actual_position=" << observed_position
+                        << ", cyclic_position_distance="
+                        << SupportFunctions::cyclicAbsoluteDifference(
+                               observed_position, expected_position,
+                               counts_per_joint_revolution)
+                        << ", counts_per_joint_revolution="
+                        << counts_per_joint_revolution
+                        << ", stable_samples=" << stable_samples;
+        return false;
+    };
+    // Follow EYOU's required clear -> set -> save sequence during rollback too.
+    const auto rollback = [&](const char* failed_phase) {
+        const bool clear_written = write_home_offset(0);
+        std::int32_t cleared_position = 0;
+        const bool clear_applied =
+            clear_written &&
+            wait_for_position("rollback_clear_home_offset", 0,
+                              expected_cleared_position, cleared_position);
+        const bool offset_written = write_home_offset(original_home_offset);
+        std::int32_t restored_position = 0;
+        const bool offset_applied =
+            offset_written &&
+            wait_for_position("rollback_apply_home_offset", original_home_offset,
+                              original_position, restored_position);
+        const bool parameters_saved = offset_written && save_parameters();
+        const bool saved_state_confirmed =
+            parameters_saved &&
+            wait_for_position("rollback_save_home_offset", original_home_offset,
+                              original_position, restored_position);
+        const bool soft_limit_restored = restore_soft_limit();
+        const bool rollback_ok = clear_applied && offset_applied &&
+                                 saved_state_confirmed && soft_limit_restored;
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] calibration failed and original state was "
+            << (rollback_ok ? "restored" : "not fully restored")
+            << ", node=" << static_cast<int>(node_id)
+            << ", phase=" << failed_phase
+            << ", original_home_offset=" << original_home_offset
+            << ", original_soft_limit_state=" << original_soft_limit_state
+            << ", clear_applied=" << clear_applied
+            << ", offset_applied=" << offset_applied
+            << ", saved_state_confirmed=" << saved_state_confirmed
+            << ", soft_limit_restored=" << soft_limit_restored;
+        return false;
+    };
+
     if (!bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
                                                0x00, 0)) {
+        const bool soft_limit_restored = restore_soft_limit();
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position "
                         << "limit before home offset calibration, node="
-                        << static_cast<int>(node_id);
+                        << static_cast<int>(node_id)
+                        << ", soft_limit_restored=" << soft_limit_restored;
         return false;
     }
+    if (!wait_for_soft_limit(0)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] software position limit did not disable, node="
+                        << static_cast<int>(node_id);
+        return rollback("disable_soft_limit");
+    }
 
-    if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
-                                              0x00, 0)) {
+    if (!write_home_offset(0)) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node="
                         << static_cast<int>(node_id);
-        return false;
+        return rollback("clear_home_offset");
     }
-    std::this_thread::sleep_for(std::chrono::milliseconds{50});
 
     std::int32_t actual_position = 0;
-    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
-                                             0x00, actual_position)) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
-                        << "after clearing home offset, node="
-                        << static_cast<int>(node_id);
-        return false;
+    if (!wait_for_position("clear_home_offset", 0,
+                           expected_cleared_position, actual_position)) {
+        return rollback("wait_for_cleared_position");
     }
-
     if (actual_position == std::numeric_limits<std::int32_t>::min()) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home "
                         << "offset calibration, node=" << static_cast<int>(node_id)
                         << ", actual_position=" << actual_position;
-        return false;
+        return rollback("negate_actual_position");
     }
 
     const auto home_offset = static_cast<std::int32_t>(-actual_position);
-    if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
-                                              0x00, home_offset)) {
+    if (!write_home_offset(home_offset)) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node="
                         << static_cast<int>(node_id)
                         << ", home_offset=" << home_offset;
-        return false;
+        return rollback("write_home_offset");
     }
 
-    if (!bus_runtime_->writeSdo<std::uint32_t>(
-            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
-            0x01,
-            0x65766173)) {
+    if (!wait_for_position("apply_home_offset", home_offset, 0,
+                           zeroed_position)) {
+        return rollback("wait_for_zero_before_save");
+    }
+
+    if (!save_parameters()) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node="
                         << static_cast<int>(node_id);
-        return false;
+        return rollback("save_parameters");
     }
-    std::this_thread::sleep_for(std::chrono::milliseconds{50});
 
-    std::int32_t home_offset_readback = 0;
-    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
-                                             0x00, home_offset_readback)) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node="
-                        << static_cast<int>(node_id);
-        return false;
+    if (!wait_for_position("save_home_offset", home_offset, 0,
+                           zeroed_position)) {
+        return rollback("wait_for_zero_after_save");
     }
-    if (home_offset_readback != home_offset) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node="
+
+    if (!restore_soft_limit()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position "
+                        << "limit after home offset calibration, node="
                         << static_cast<int>(node_id)
-                        << ", expected=" << home_offset
-                        << ", actual=" << home_offset_readback;
-        return false;
-    }
-
-    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
-                                             0x00, zeroed_position)) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
-                        << "after writing home offset, node="
-                        << static_cast<int>(node_id);
-        return false;
-    }
-    if (std::abs(static_cast<long long>(zeroed_position)) > 10000) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, "
-                        << "node=" << static_cast<int>(node_id)
-                        << ", actual_position_before=" << actual_position
-                        << ", home_offset=" << home_offset
-                        << ", home_offset_readback=" << home_offset_readback
-                        << ", actual_position_after=" << zeroed_position
-                        << ", tolerance_counts=" << 10000;
-        return false;
+                        << ", original_soft_limit_state=" << original_soft_limit_state;
+        return rollback("restore_soft_limit");
     }
 
+    CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node="
+                   << static_cast<int>(node_id)
+                   << ", original_home_offset=" << original_home_offset
+                   << ", cleared_position=" << actual_position
+                   << ", home_offset=" << home_offset
+                   << ", zeroed_position=" << zeroed_position
+                   << ", soft_limit_state=" << original_soft_limit_state;
     return true;
 }
 

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

@@ -7,6 +7,8 @@
 #include <limits>
 #include <memory>
 #include <thread>
+#include <utility>
+#include <vector>
 
 #include <gtest/gtest.h>
 
@@ -19,19 +21,23 @@
 namespace cmvr::device {
 namespace {
 
-constexpr const char* kMotorManagerId = "ethercat_motors";
+constexpr const char* kMotorManagerId = "right_arm_ethercat_motors";
 constexpr const char* kMotorConfigFile =
-    "devices/motor/ethercat_motors_two_real_test.pb.txt";
+    "devices/motor/ethercat_motors_four_real_test.pb.txt";
 constexpr std::array<int, 4> kFourMotorIds{1, 2, 3, 4};
 constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
-constexpr std::chrono::milliseconds kPrintPeriod{100};
 constexpr std::chrono::milliseconds kStatsSamplePeriod{10};
 constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500};
 constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
 constexpr double kPi = 3.14159265358979323846;
-constexpr double kFourMotorAmplitudeRad = 0.2;
-constexpr double kFourMotorPeriodS = 1.0;
+constexpr double kRaisedCosineCoefficientRad = 0.2;
+constexpr double kFourMotorPeriodS = 2.0;
 constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
+constexpr double kMinimumPositionExcursionRad = 0.2;
+constexpr double kMaximumAbsoluteTrackingErrorRad = 0.15;
+constexpr double kMaximumRmsTrackingErrorRad = 0.08;
+constexpr double kMaximumErrorSpreadRad = 0.10;
+constexpr double kFinalPositionToleranceRad = 0.05;
 
 class DeviceManagerDestroyGuard {
 public:
@@ -41,6 +47,61 @@ public:
     }
 };
 
+class MotorManagerStopGuard {
+public:
+    explicit MotorManagerStopGuard(std::shared_ptr<MotorManager> motor_manager)
+        : motor_manager_(std::move(motor_manager))
+    {
+    }
+
+    ~MotorManagerStopGuard()
+    {
+        if (motor_manager_ && !motor_manager_->stop()) {
+            std::cerr << "failed to stop motor manager during test cleanup" << std::endl;
+        }
+    }
+
+private:
+    std::shared_ptr<MotorManager> motor_manager_;
+};
+
+class MultiMotorSafetyGuard {
+public:
+    explicit MultiMotorSafetyGuard(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors)
+        : motors_(motors)
+    {
+    }
+
+    ~MultiMotorSafetyGuard()
+    {
+        if (armed_) {
+            stop();
+        }
+    }
+
+    bool stop()
+    {
+        bool all_ok = true;
+        for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
+            if (*it && !(*it)->quickStop()) {
+                all_ok = false;
+            }
+        }
+        for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
+            if (*it && !(*it)->torqueOff()) {
+                all_ok = false;
+            }
+        }
+        armed_ = !all_ok;
+        return all_ok;
+    }
+
+private:
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors_;
+    bool armed_{true};
+};
+
 struct TrackingErrorStats {
     std::int64_t sample_count{0};
     double sum_error{0.0};
@@ -108,8 +169,6 @@ config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig()
     config::DeviceManagerConfig config;
     config.set_name("eyou_motor_device_manager_real_test");
     config.set_version("test");
-    config.set_init_all_motors_when_no_active_joints(true);
-
     auto* motor_entry = config.add_devices();
     motor_entry->set_id(kMotorManagerId);
     motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
@@ -140,31 +199,16 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
         DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
     auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
     ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
 
-    // for (int motor_id = 1; motor_id <= 4; ++motor_id) {
-    //     printMotorState(motor_id, motor_manager->getMotor(motor_id));
-    // }
-
-    auto motor = motor_manager->getMotor(4);
-    motor->calibrateZeroQ();
-
-    ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
-    motor->commandProfileVelocity(-2,5);
-
-
-    for (int i = 1; i <= 50; ++i)
-    {
-        auto q = motor->getQ();
-        auto qd = motor->getQd();
-        std::cout << "q=" << q << ", qd=" << qd << std::endl;
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    for (const int motor_id : kFourMotorIds) {
+        auto motor = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
+        ASSERT_NE(motor, nullptr);
+        printMotorState(motor_id, motor);
     }
-    motor->quickStop();
-
 }
 
-TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
+TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory)
 {
     ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
     DeviceManagerDestroyGuard guard;
@@ -173,16 +217,21 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
         DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
     auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
     ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
 
-    std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
+    std::vector<std::shared_ptr<AbstractMotor>> motors(kFourMotorIds.size());
     for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) {
         const int motor_id = kFourMotorIds[i];
         motors[i] = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
         ASSERT_NE(motors[i], nullptr);
         printMotorState(motor_id, motors[i]);
     }
+    MultiMotorSafetyGuard safety_guard(motors);
 
     std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
+    for (const auto& motor : motors) {
+        ASSERT_TRUE(motor->torqueOff());
+    }
     for (std::size_t i = 0; i < motors.size(); ++i) {
         const int motor_id = kFourMotorIds[i];
         std::cout << "before calibrateZeroQ: ";
@@ -196,48 +245,64 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
         ASSERT_TRUE(motor->torqueOn());
     }
     for (const auto& motor : motors) {
-        motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+        ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
     }
 
-    std::array<double, kFourMotorIds.size()> center_q{};
-    for (std::size_t i = 0; i < motors.size(); ++i) {
-        center_q[i] = motors[i]->getQ();
-    }
+    std::vector<double> center_q;
+    std::vector<double> actual_qd;
+    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd));
 
     const double omega = 2.0 * kPi / kFourMotorPeriodS;
     std::cout << "command four motors in CSP, duration="
               << kFourMotorTrajectoryDuration.count()
               << " ms, command_period=" << kCyclicCommandPeriod.count()
-              << " ms, amplitude=" << kFourMotorAmplitudeRad
+              << " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad
+              << " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad
               << " rad, period=" << kFourMotorPeriodS
               << " s" << std::endl;
 
     const auto start_time = std::chrono::steady_clock::now();
-    const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod;
+    const auto end_time = start_time + kFourMotorTrajectoryDuration;
+    auto next_command_time = start_time;
+    auto next_stats_time = start_time;
+    std::uint64_t missed_command_deadlines = 0;
     std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
+    std::vector<double> target_q(motors.size(), 0.0);
+    std::vector<double> target_qd(motors.size(), 0.0);
+    std::vector<double> actual_q;
+    std::array<double, kFourMotorIds.size()> minimum_actual_q{};
+    std::array<double, kFourMotorIds.size()> maximum_actual_q{};
+    std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin());
+    std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin());
     double max_error_spread_rad = 0.0;
     double sum_error_spread_sq = 0.0;
     std::int64_t error_spread_sample_count = 0;
-    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
-        const auto elapsed = tick * kCyclicCommandPeriod;
-        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+    while (true) {
+        std::this_thread::sleep_until(next_command_time);
+        const auto now = std::chrono::steady_clock::now();
+        if (now > end_time) {
+            break;
+        }
+        const double t_s = std::chrono::duration<double>(now - start_time).count();
 
-        std::array<double, kFourMotorIds.size()> target_q{};
-        std::array<double, kFourMotorIds.size()> target_qd{};
         for (std::size_t i = 0; i < motors.size(); ++i) {
             const double theta = omega * t_s + kFourMotorPhaseRad[i];
-            target_q[i] = center_q[i] + kFourMotorAmplitudeRad * (1.0 - std::cos(theta));
-            target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta);
-            ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
+            target_q[i] =
+                center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta));
+            target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta);
         }
+        ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
 
-        if (elapsed.count() % kStatsSamplePeriod.count() == 0) {
+        if (now >= next_stats_time) {
+            ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
             double min_error = std::numeric_limits<double>::max();
             double max_error = std::numeric_limits<double>::lowest();
             for (std::size_t i = 0; i < motors.size(); ++i) {
                 const double theta = omega * t_s + kFourMotorPhaseRad[i];
-                const double error = motors[i]->getQ() - target_q[i];
+                const double error = actual_q[i] - target_q[i];
                 error_stats[i].add(error, theta);
+                minimum_actual_q[i] = std::min(minimum_actual_q[i], actual_q[i]);
+                maximum_actual_q[i] = std::max(maximum_actual_q[i], actual_q[i]);
                 min_error = std::min(min_error, error);
                 max_error = std::max(max_error, error);
             }
@@ -245,32 +310,25 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
             max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
             sum_error_spread_sq += error_spread * error_spread;
             ++error_spread_sample_count;
+            next_stats_time = now + kStatsSamplePeriod;
         }
 
-        // if (elapsed.count() % kPrintPeriod.count() == 0) {
-        //     std::cout << "t=" << elapsed.count() << " ms" << std::endl;
-        //     for (std::size_t i = 0; i < motors.size(); ++i) {
-        //         std::cout << "  motor_id=" << kFourMotorIds[i]
-        //                   << ", target_q=" << target_q[i]
-        //                   << " rad, target_qd=" << target_qd[i]
-        //                   << " rad/s, q=" << motors[i]->getQ()
-        //                   << " rad, qd=" << motors[i]->getQd()
-        //                   << " rad/s" << std::endl;
-        //     }
-        // }
-
-        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
-    }
-
-    const auto hold_start_time = std::chrono::steady_clock::now();
-    const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod;
-    for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) {
-        for (std::size_t i = 0; i < motors.size(); ++i) {
-            ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0));
+        next_command_time += kCyclicCommandPeriod;
+        const auto command_complete_time = std::chrono::steady_clock::now();
+        if (next_command_time <= command_complete_time) {
+            const auto skipped_periods =
+                (command_complete_time - next_command_time) / kCyclicCommandPeriod + 1;
+            missed_command_deadlines += static_cast<std::uint64_t>(skipped_periods);
+            next_command_time += skipped_periods * kCyclicCommandPeriod;
         }
-        std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod);
     }
 
+    std::copy(center_q.begin(), center_q.end(), target_q.begin());
+    std::fill(target_qd.begin(), target_qd.end(), 0.0);
+    ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
+    std::this_thread::sleep_for(kHoldAfterTrajectoryDuration);
+    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
+
     std::cout << "after four motor CSP trajectory" << std::endl;
     for (std::size_t i = 0; i < motors.size(); ++i) {
         printMotorState(kFourMotorIds[i], motors[i]);
@@ -284,6 +342,7 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
     std::cout << "four motor CSP tracking error statistics, sample_period="
               << kStatsSamplePeriod.count()
               << " ms, samples=" << error_stats.front().sample_count
+              << ", missed_command_deadlines=" << missed_command_deadlines
               << ", max_error_spread=" << max_error_spread_rad
               << " rad, rms_error_spread=" << rms_error_spread
               << " rad" << std::endl;
@@ -303,7 +362,19 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
                   << " rad (" << radToDeg(relative_phase)
                   << " deg, " << relative_phase_ms
                   << " ms)" << std::endl;
+        EXPECT_GE(maximum_actual_q[i] - minimum_actual_q[i],
+                  kMinimumPositionExcursionRad)
+            << "motor_id=" << kFourMotorIds[i] << " did not complete enough motion";
+        EXPECT_LE(error_stats[i].max_abs_error,
+                  kMaximumAbsoluteTrackingErrorRad)
+            << "motor_id=" << kFourMotorIds[i] << " exceeded maximum tracking error";
+        EXPECT_LE(error_stats[i].rms(), kMaximumRmsTrackingErrorRad)
+            << "motor_id=" << kFourMotorIds[i] << " exceeded RMS tracking error";
+        EXPECT_NEAR(actual_q[i], center_q[i], kFinalPositionToleranceRad)
+            << "motor_id=" << kFourMotorIds[i] << " did not return to its start position";
     }
+    EXPECT_LE(max_error_spread_rad, kMaximumErrorSpreadRad);
+    EXPECT_TRUE(safety_guard.stop());
 }
 
 } // namespace cmvr::device

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

@@ -23,7 +23,6 @@ namespace cmvr::device {
 namespace {
 
 constexpr int kMotorId = 1;
-constexpr std::chrono::milliseconds kModeSettleDelay{100};
 constexpr std::chrono::milliseconds kCommandSamplePeriod{100};
 constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
 constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000};
@@ -46,14 +45,20 @@ config::MotorGroupConfig createSingleSlaveGroup()
     ethercat->set_slave_state_poll_period_ms(10);
 
     auto* cia402 = ethercat->mutable_cia402();
-    cia402->set_profile_position_trigger_delay_ms(2);
     cia402->set_state_transition_timeout_ms(1200);
     cia402->set_velocity_stop_timeout_ms(2000);
     cia402->set_status_poll_period_ms(10);
     cia402->set_stopped_velocity_tolerance_rad_s(0.001);
 
+    auto* zero_calibration = ethercat->mutable_zero_calibration();
+    zero_calibration->set_timeout_ms(2000);
+    zero_calibration->set_poll_period_ms(10);
+    zero_calibration->set_stable_sample_count(5);
+    zero_calibration->set_position_tolerance_counts(10000);
+    zero_calibration->set_stable_delta_counts(1000);
+
     auto* dc = ethercat->mutable_dc();
-    dc->set_enable(true);
+    dc->set_enable(false);
     dc->set_reference_motor_id(kMotorId);
     dc->set_sync0_cycle_us(1000);
     dc->set_sync0_shift_us(0);
@@ -113,10 +118,10 @@ config::MotorConfigItem createMotorConfig()
     config::MotorConfigItem config;
     config.set_id(kMotorId);
     config.set_joint_name("ethercat_test_joint");
-    config.set_limit_q_lb(-6.14);
-    config.set_limit_q_ub(6.14);
+    config.set_limit_q_lb(-36.14);
+    config.set_limit_q_ub(36.14);
     config.set_limit_qd(10.0);
-    config.set_limit_qdd(10.0);
+    config.set_limit_qdd(100.0);
     config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
     config.set_gear_ratio(kDefaultGearRatio);
     return config;
@@ -233,6 +238,9 @@ TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter)
     ASSERT_TRUE(motor->calibrateZeroQ());
     printMotorState("after calibrateZeroQ", *motor);
     ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
+    ASSERT_TRUE(motor->commandProfilePosition(1.5,0.8,3.0));
+    sampleMotorState(*motor, kFeedbackSampleDuration);
 }
 
 TEST(EyouMotorRealTest, CommandProfilePosition)
@@ -245,8 +253,7 @@ TEST(EyouMotorRealTest, CommandProfilePosition)
     ASSERT_NE(motor, nullptr);
 
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
 
     ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
     sampleMotorState(*motor, kFeedbackSampleDuration);
@@ -261,9 +268,10 @@ TEST(EyouMotorRealTest, CommandProfileVelocity)
     auto motor = createMotor(runtime);
     ASSERT_NE(motor, nullptr);
 
+
+
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
 
     std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
     ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
@@ -283,21 +291,20 @@ TEST(EyouMotorRealTest, CommandCyclicPosition)
     auto motor = createMotor(runtime);
     ASSERT_NE(motor, nullptr);
 
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
 
-    const std::chrono::milliseconds trajectory_duration{15000};
+    const std::chrono::milliseconds trajectory_duration{12000};
     const double period_s = 6.0;
-    const double amplitude_rad = 3;
-    const double phase_rad = 0.0;
+    const double excursion_rad = 3.0;
     const double center_q = motor->getQ();
     const double omega = 2.0 * kPi / period_s;
 
-    std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q
+    std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q
               << " rad, period=" << period_s
-              << " s, amplitude=" << amplitude_rad
-              << " rad, phase=" << phase_rad
+              << " s, excursion=" << excursion_rad
               << " rad, command_period=" << kCyclicCommandPeriod.count()
               << " ms" << std::endl;
 
@@ -306,9 +313,11 @@ TEST(EyouMotorRealTest, CommandCyclicPosition)
     for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
         const auto elapsed = tick * kCyclicCommandPeriod;
         const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
-        const double theta = omega * t_s + phase_rad;
-        const double target_q = center_q + amplitude_rad * std::sin(theta);
-        const double target_qd = amplitude_rad * omega * std::cos(theta);
+        const double theta = omega * t_s;
+        const double target_q =
+            center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta));
+        const double target_qd =
+            0.5 * excursion_rad * omega * std::sin(theta);
 
         ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
         if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
@@ -331,19 +340,22 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
     auto motor = createMotor(runtime);
     ASSERT_NE(motor, nullptr);
 
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
 
-    const std::chrono::milliseconds trajectory_duration{15000};
-    const double period_s = 6.0;
-    const double velocity_amplitude_rad_s = 5.0;
+    const std::chrono::milliseconds trajectory_duration{12000};
+    const double period_s = 5.0;
+    const double excursion_rad = 5.0;
     const double phase_rad = 0.0;
     const double omega = 2.0 * kPi / period_s;
+    const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega;
 
     std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
               << " s, velocity_amplitude=" << velocity_amplitude_rad_s
-              << " rad/s, phase=" << phase_rad
+              << " rad/s, excursion=" << excursion_rad
+              << " rad, phase=" << phase_rad
               << " rad, command_period=" << kCyclicCommandPeriod.count()
               << " ms" << std::endl;
 
@@ -361,6 +373,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
                       << " ms, target_qd=" << target_qd
                       << " rad/s";
             printMotorState("", *motor);
+            printRawEthercatFeedback("raw feedback", runtime);
         }
         std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
     }
@@ -368,6 +381,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
     std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl;
     ASSERT_TRUE(motor->commandCyclicVelocity(0.0));
     sampleMotorState(*motor, std::chrono::milliseconds{500});
+    printRawEthercatFeedback("raw feedback after stop", runtime);
 }
 
 TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
@@ -382,8 +396,7 @@ TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
 
     const std::chrono::milliseconds run_duration{2000};
     const double period_s = 6.0;
@@ -438,8 +451,7 @@ TEST(EyouMotorRealTest, QuickStopInProfilePosition)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
 
     const std::chrono::milliseconds quick_stop_time{1000};
     const double start_q = motor->getQ();
@@ -478,8 +490,7 @@ TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
 
     const std::chrono::milliseconds quick_stop_time{2000};
     const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
@@ -515,8 +526,7 @@ TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
 
     const std::chrono::milliseconds run_duration{2000};
     const double start_q = motor->getQ();

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

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

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

@@ -28,17 +28,14 @@ namespace cmvr::device {
 std::mutex MotorManager::registry_mutex_;
 std::unordered_map<std::string, std::weak_ptr<MotorManager>> MotorManager::managers_;
 std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> MotorManager::mujoco_world_registry_;
-std::unordered_map<std::string, MotorManager::ActiveJointSelection> MotorManager::active_joints_;
 
-MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg)
-    : cfg_(cfg)
+MotorManager::MotorManager(std::string id,
+                           const config::MotorConfig& cfg,
+                           std::string selected_group_id)
+    : cfg_(cfg),
+      selected_group_id_(std::move(selected_group_id))
 {
     id_ = std::move(id);
-    if (!cfg_.id().empty() && cfg_.id() != id_) {
-        CMVR_LOG(ERROR) << "[MotorManager] config id '" << cfg_.id()
-                        << "' does not match device id '" << id_ << "'";
-        id_.clear();
-    }
 }
 
 MotorManager::~MotorManager() = default;
@@ -52,6 +49,14 @@ bool MotorManager::init()
         CMVR_LOG(ERROR) << "[MotorManager] id is empty";
         return false;
     }
+    if (selected_group_id_.empty()) {
+        CMVR_LOG(ERROR) << "[MotorManager] selected motor group id is empty: " << id_;
+        return false;
+    }
+    if (cfg_.motor_groups_size() == 0) {
+        CMVR_LOG(ERROR) << "[MotorManager] no motor groups configured: " << id_;
+        return false;
+    }
 
     bus_runtimes_.clear();
     bus_runtimes_.reserve(static_cast<std::size_t>(cfg_.motor_groups_size()));
@@ -62,25 +67,27 @@ bool MotorManager::init()
     }
 
     bool all_ok = true;
+    std::size_t selected_group_count = 0;
     for (const auto& motor_group_cfg : cfg_.motor_groups()) {
         const auto& group_name = motor_group_cfg.id();
-        if (group_name.empty()) {
-            CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_;
-            all_ok = false;
+        if (group_name != selected_group_id_) {
             continue;
         }
+        ++selected_group_count;
         if (!motor_group_cfg.has_motors()) {
             CMVR_LOG(ERROR) << "[MotorManager] motor group missing motors: " << group_name;
             all_ok = false;
             continue;
         }
 
-        std::vector<config::MotorConfigItem> selected_motor_cfgs;
-        const bool selected_active_group = selectActiveMotors_(
-            group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs);
-        if (!selected_active_group) {
+        if (motor_group_cfg.motors().motors_size() == 0) {
+            CMVR_LOG(ERROR) << "[MotorManager] motor group has no motors: " << group_name;
+            all_ok = false;
             continue;
         }
+        std::vector<config::MotorConfigItem> selected_motor_cfgs(
+            motor_group_cfg.motors().motors().begin(),
+            motor_group_cfg.motors().motors().end());
 
         if (!applyConfiguredJointLimits_(motor_group_cfg, selected_motor_cfgs)) {
             all_ok = false;
@@ -137,6 +144,12 @@ bool MotorManager::init()
         bus_runtimes_.push_back(std::move(bus_runtime));
     }
 
+    if (selected_group_count != 1) {
+        CMVR_LOG(ERROR) << "[MotorManager] selected motor group '" << selected_group_id_
+                        << "' must occur exactly once in config: " << id_;
+        all_ok = false;
+    }
+
     if (!all_ok) {
         for (auto& bus_runtime : bus_runtimes_) {
             if (bus_runtime) {
@@ -299,68 +312,6 @@ std::shared_ptr<simulate::MujocoWorld> MotorManager::mujocoWorldFor(const std::s
     return it->second.lock();
 }
 
-void MotorManager::setActiveJoints(const std::string& motor_manager_id,
-                                  ActiveJointSelection group_joints)
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_[motor_manager_id] = std::move(group_joints);
-}
-
-void MotorManager::clearActiveJoints()
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_.clear();
-}
-
-bool MotorManager::selectActiveMotors_(
-    const std::string& group_name,
-    const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
-    std::vector<config::MotorConfigItem>& selected) const
-{
-    selected.clear();
-
-    ActiveJointSelection selection;
-    {
-        std::lock_guard<std::mutex> lock(registry_mutex_);
-        const auto it = active_joints_.find(id_);
-        if (it != active_joints_.end()) {
-            selection = it->second;
-        }
-    }
-
-    if (selection.empty()) {
-        CMVR_LOG(WARNING) << "[MotorManager] No active joints selected for motor manager " << id_
-                          << ", motor group " << group_name << " will not initialize motors.";
-        return false;
-    }
-
-    const auto group_it = selection.find(group_name);
-    if (group_it == selection.end() || group_it->second.empty()) {
-        return false;
-    }
-
-    for (const auto& motor_cfg : source) {
-        if (group_it->second.count(motor_cfg.joint_name()) > 0) {
-            selected.push_back(motor_cfg);
-        }
-    }
-
-    if (selected.size() != group_it->second.size()) {
-        std::unordered_set<std::string> found;
-        for (const auto& motor_cfg : selected) {
-            found.insert(motor_cfg.joint_name());
-        }
-        for (const auto& joint_name : group_it->second) {
-            if (found.count(joint_name) == 0) {
-                CMVR_LOG(ERROR) << "[MotorManager] active joint '" << joint_name
-                                << "' not found in motor group '" << group_name << "'";
-                return false;
-            }
-        }
-    }
-    return !selected.empty();
-}
-
 bool MotorManager::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
                                               std::vector<config::MotorConfigItem>& selected) const
 {

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

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

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

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

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

@@ -18,17 +18,12 @@
 #include "devices/speaker/abstract_speaker.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 #include "common/config/config_files.h"
-#include "cmvr/config/arm_config/arm_config.pb.h"
-#include "cmvr/config/motor_config/motor_config.pb.h"
 
 using namespace std;
 using namespace cmvr::device;
 
 namespace {
 
-using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
-using MotorJointSelections = std::unordered_map<std::string, GroupJointSelection>;
-
 void logSection(const char* title)
 {
     CMVR_LOG(INFO) << "---------------- " << title << " ----------------";
@@ -63,43 +58,6 @@ const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType
     }
 }
 
-bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group,
-                        const std::string& joint_name)
-{
-    for (const auto& motor : motor_group.motors().motors()) {
-        if (motor.joint_name() == joint_name) {
-            return true;
-        }
-    }
-    return false;
-}
-
-void addAllMotorJoints(const std::string& motor_system_id,
-                       const cmvr::config::MotorRootConfig& root_cfg,
-                       MotorJointSelections& selections)
-{
-    auto& group_selection = selections[motor_system_id];
-    for (const auto& motor_group : root_cfg.motor().motor_groups()) {
-        if (motor_group.id().empty()) {
-            continue;
-        }
-
-        auto& selected_joints = group_selection[motor_group.id()];
-        for (const auto& motor : motor_group.motors().motors()) {
-            if (!motor.joint_name().empty()) {
-                selected_joints.insert(motor.joint_name());
-            }
-        }
-        if (selected_joints.empty()) {
-            group_selection.erase(motor_group.id());
-        }
-    }
-
-    if (group_selection.empty()) {
-        selections.erase(motor_system_id);
-    }
-}
-
 } // namespace
 
 template std::shared_ptr<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
@@ -125,7 +83,6 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
     dev_factory_ = std::make_unique<DeviceFactory>();
     logSection("Device Plan");
     log_device_plan_();
-    pre_scan_robot_arm_dependencies_();
     logSection("Initialize Devices");
     init_devices_();
     configure_mujoco_viewer_pip_();
@@ -150,7 +107,6 @@ DeviceManager& DeviceManager::getInstance() {
 void DeviceManager::destroyInstance() {
     std::lock_guard lock(init_mutex_);
     instance_.reset();
-    MotorManager::clearActiveJoints();
 }
 
 void DeviceManager::start(){
@@ -283,153 +239,6 @@ void DeviceManager::log_device_plan_() const
     CMVR_LOG(INFO) << "[DeviceManager]: Device plan end";
 }
 
-void DeviceManager::pre_scan_robot_arm_dependencies_() const
-{
-    MotorJointSelections selections;
-    std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
-
-    for (const auto& entry : cfg_.devices()) {
-        if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) {
-            continue;
-        }
-        if (entry.id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
-            return;
-        }
-        if (entry.config_file().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
-            return;
-        }
-
-        config::MotorRootConfig root_cfg;
-        if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file();
-            return;
-        }
-        if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
-                            << "' does not match config id '" << root_cfg.motor().id() << "'";
-            return;
-        }
-        motor_roots.emplace(entry.id(), std::move(root_cfg));
-    }
-
-    for (const auto& entry : cfg_.devices()) {
-        if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM) {
-            continue;
-        }
-        if (entry.id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty";
-            return;
-        }
-        if (entry.config_file().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id();
-            return;
-        }
-
-        config::ArmRootConfig root_cfg;
-        if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file();
-            return;
-        }
-
-        const config::RobotArmConfig* arm_cfg = nullptr;
-        for (const auto& candidate : root_cfg.arm().robot_arms()) {
-            if (candidate.id() == entry.id()) {
-                arm_cfg = &candidate;
-                break;
-            }
-        }
-        if (!arm_cfg) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id()
-                            << "' not found in config: " << entry.config_file();
-            return;
-        }
-
-        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) {
-            continue;
-        }
-        if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id();
-            return;
-        }
-
-        const auto& motor_config = arm_cfg->motor();
-        if (motor_config.motor_system_id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id();
-            return;
-        }
-        if (motor_config.motor_group_ids_size() == 0) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id();
-            return;
-        }
-        if (motor_config.joint_names_size() == 0) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id();
-            return;
-        }
-
-        const auto motor_root_it = motor_roots.find(motor_config.motor_system_id());
-        if (motor_root_it == motor_roots.end()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
-                            << "' depends on disabled or missing MotorManager: "
-                            << motor_config.motor_system_id();
-            return;
-        }
-
-        std::unordered_set<std::string> allowed_groups;
-        allowed_groups.reserve(static_cast<size_t>(motor_config.motor_group_ids_size()));
-        for (const auto& group_id : motor_config.motor_group_ids()) {
-            if (group_id.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id();
-                return;
-            }
-            allowed_groups.insert(group_id);
-        }
-
-        auto& group_selection = selections[motor_config.motor_system_id()];
-        for (const auto& joint_name : motor_config.joint_names()) {
-            if (joint_name.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id();
-                return;
-            }
-
-            std::string matched_group;
-            for (const auto& motor_group : motor_root_it->second.motor().motor_groups()) {
-                const auto& group_id = motor_group.id();
-                if (allowed_groups.count(group_id) == 0) {
-                    continue;
-                }
-                if (motorGroupHasJoint(motor_group, joint_name)) {
-                    matched_group = group_id;
-                    break;
-                }
-            }
-
-            if (matched_group.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
-                                << "' joint '" << joint_name
-                                << "' not found in configured motor_group_ids";
-                return;
-            }
-            group_selection[matched_group].insert(joint_name);
-        }
-    }
-
-    if (selections.empty() && cfg_.init_all_motors_when_no_active_joints()) {
-        CMVR_LOG(INFO) << "[DeviceManager]: No active motor joints from RobotArm; "
-                       << "initialize all configured motors because "
-                       << "init_all_motors_when_no_active_joints=true";
-        for (const auto& [motor_system_id, root_cfg] : motor_roots) {
-            addAllMotorJoints(motor_system_id, root_cfg, selections);
-        }
-    }
-
-    MotorManager::clearActiveJoints();
-    for (auto& [motor_system_id, group_selection] : selections) {
-        MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
-    }
-}
-
 void DeviceManager::init_devices_() {
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable()) {

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

@@ -109,6 +109,8 @@ namespace cmvr {
         mjvScene pip_scene_;
         bool pip_scene_inited_ = false;
         mjModel *pip_scene_model_ = nullptr;
+        mjData *pip_render_data_ = nullptr;
+        mjModel *pip_render_data_model_ = nullptr;
         mutable std::mutex pip_rgb_mtx_;
         std::vector<unsigned char> pip_rgb_;
         std::vector<float> pip_depth_;     // 新增：z-buffer

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

@@ -76,6 +76,11 @@ namespace cmvr {
             mjv_freeScene(&pip_scene_);
             pip_scene_inited_ = false;
         }
+        if (pip_render_data_ != nullptr) {
+            mj_deleteData(pip_render_data_);
+            pip_render_data_ = nullptr;
+            pip_render_data_model_ = nullptr;
+        }
     }
 
     mjModel* MuJocoViewer::model() const {
@@ -207,7 +212,33 @@ namespace cmvr {
         display_rect.left = left;
         display_rect.bottom = bottom;
 
-        const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
+        // Copy only the visualization state while synchronized with Simulate.
+        // Keep scene update, GPU rendering and pixel readback out of this lock
+        // so the world sync thread cannot hold the simulation mutex while
+        // waiting for the viewer.
+        {
+            std::unique_lock<std::recursive_mutex> lock(sim_->mtx, std::try_to_lock);
+            if (lock.owns_lock()) {
+                if (pip_render_data_model_ != render_model) {
+                    if (pip_render_data_ != nullptr) {
+                        mj_deleteData(pip_render_data_);
+                        pip_render_data_ = nullptr;
+                    }
+                    pip_render_data_ = mj_makeData(render_model);
+                    pip_render_data_model_ = render_model;
+                }
+                if (pip_render_data_ != nullptr) {
+                    mjv_copyData(pip_render_data_, render_model, render_data);
+                }
+            }
+        }
+
+        // The simulation lock is intentionally non-blocking.  If the sync
+        // thread owns it, use the last complete snapshot so every back buffer
+        // still gets the PiP overlay; skip only until the first snapshot exists.
+        if (pip_render_data_ == nullptr || pip_render_data_model_ != render_model) {
+            return;
+        }
 
         if (!pip_scene_inited_ || pip_scene_model_ != render_model) {
             if (pip_scene_inited_) {
@@ -222,7 +253,7 @@ namespace cmvr {
         pip_cam_.fixedcamid = pip_camera_id_;
         pip_cam_.trackbodyid = -1;
 
-        mjv_updateScene(render_model, render_data, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
+        mjv_updateScene(render_model, pip_render_data_, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
 
         auto& context = sim_->platform_ui->mjr_context();
         const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;

cmvr-es/task/CMakeLists.txt

@@ -19,22 +19,22 @@ target_link_libraries(task
 add_library(cmvr_es::task ALIAS task)
 install(TARGETS task LIBRARY DESTINATION lib)
 
-#add_executable(touch_screen_task_test
-#        touch_screen_task/src/touch_screen_task_test.cpp
-#)
-#
-#target_link_libraries(touch_screen_task_test PRIVATE
-#        cmvr_es::task
-#        cmvr_es::device::arm
-#        cmvr_es::device::motor_manager
-#        cmvr_es::device::mujoco_motor_driver
-#        cmvr_es::device::mujoco_camera
-#        cmvr_es::mujoco_viewer
-#        cmvr_es::proto
-#        cmvr_es::device_manager
-#        cmvr_es::service
-#        gtest
-#        gtest_main
-#        pthread
-#        glog
-#)
+add_executable(touch_screen_task_test
+        touch_screen_task/src/touch_screen_task_test.cpp
+)
+
+target_link_libraries(touch_screen_task_test PRIVATE
+        cmvr_es::task
+        cmvr_es::device::arm
+        cmvr_es::device::motor_manager
+        cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device::mujoco_camera
+        cmvr_es::mujoco_viewer
+        cmvr_es::proto
+        cmvr_es::device_manager
+        cmvr_es::service
+        gtest
+        gtest_main
+        pthread
+        glog
+)

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

@@ -1,9 +1,14 @@
 #ifndef CMVR_ES_SELF_COLLISION_TASK_H
 #define CMVR_ES_SELF_COLLISION_TASK_H
 
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
+#include <vector>
 
 #include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h"
 #include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
@@ -20,10 +25,22 @@ enum class CollisionSafetyLevel {
     STOP,
 };
 
+enum class ProtectiveRecoveryState {
+    IDLE = 0,
+    AVAILABLE,
+    RECOVERING,
+    SUCCEEDED,
+    FAILED,
+};
+
 struct SelfCollisionTaskStatus {
     CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN};
     SelfCollisionResult result;
     bool stop_latched{false};
+    std::uint64_t event_id{0};
+    ProtectiveRecoveryState recovery_state{ProtectiveRecoveryState::IDLE};
+    std::size_t recovery_sample_count{0};
+    std::string recovery_error;
 };
 
 class SelfCollisionTask final : public Task {
@@ -46,11 +63,15 @@ public:
     std::string detailStatusString() const override;
 
     SelfCollisionTaskStatus latestStatus() const;
+    device::Result requestRecovery(std::uint64_t event_id);
 
 private:
     static bool validateConfig(const config::SelfCollisionTaskConfig& config,
                                std::string* error);
     static const char* safetyLevelToString(CollisionSafetyLevel level);
+    static const char* recoveryStateToString(ProtectiveRecoveryState state);
+    void recordJointSample_(const device::JointGroupState& joint_state,
+                            DistanceSamplingPolicy::Clock::time_point now);
 
     config::SelfCollisionTaskConfig config_;
     std::string id_;
@@ -59,8 +80,16 @@ private:
     DistanceSamplingPolicy sampling_;
 
     mutable std::mutex mutex_;
+    std::condition_variable recovery_cv_;
     TaskState state_{TaskState::UNINITIALIZED};
     SelfCollisionTaskStatus latest_status_{};
+    std::deque<device::JointTrajectoryPoint> joint_history_;
+    device::JointTrajectory recovery_path_;
+    DistanceSamplingPolicy::Clock::time_point history_epoch_{};
+    std::optional<DistanceSamplingPolicy::Clock::time_point> recovery_clear_since_;
+    double recovery_best_distance_m_{0.0};
+    bool recovery_clear_confirmed_{false};
+    std::uint64_t next_event_id_{1};
     std::string last_error_;
 };
 

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

@@ -1,5 +1,7 @@
 #include "task/self_collision_task/include/self_collision_task.h"
 
+#include <algorithm>
+#include <chrono>
 #include <cmath>
 #include <iomanip>
 #include <sstream>
@@ -53,6 +55,31 @@ bool SelfCollisionTask::validateConfig(const config::SelfCollisionTaskConfig& co
         safety.warning_distance_m() < safety.stop_distance_m()) {
         return fail("warning_distance_m must be finite and not less than stop_distance_m");
     }
+    const auto& recovery = config.recovery();
+    if (!std::isfinite(recovery.clear_distance_m()) ||
+        recovery.clear_distance_m() <= safety.warning_distance_m()) {
+        return fail("recovery clear_distance_m must be finite and greater than warning_distance_m");
+    }
+    if (!std::isfinite(recovery.stable_period_s()) ||
+        recovery.stable_period_s() <= 0.0) {
+        return fail("recovery stable_period_s must be finite and positive");
+    }
+    if (!std::isfinite(recovery.max_joint_velocity_rad_s()) ||
+        recovery.max_joint_velocity_rad_s() <= 0.0) {
+        return fail("recovery max_joint_velocity_rad_s must be finite and positive");
+    }
+    if (!std::isfinite(recovery.max_joint_acceleration_rad_s2()) ||
+        recovery.max_joint_acceleration_rad_s2() <= 0.0) {
+        return fail("recovery max_joint_acceleration_rad_s2 must be finite and positive");
+    }
+    if (!std::isfinite(recovery.history_duration_s()) ||
+        recovery.history_duration_s() <= 0.0) {
+        return fail("recovery history_duration_s must be finite and positive");
+    }
+    if (!std::isfinite(recovery.max_distance_regression_m()) ||
+        recovery.max_distance_regression_m() < 0.0) {
+        return fail("recovery max_distance_regression_m must be finite and non-negative");
+    }
     for (const auto& pair : config.checker().ignored_pairs()) {
         if (pair.first().empty() || pair.second().empty() || pair.first() == pair.second()) {
             return fail("ignored_pairs entries require two different non-empty links");
@@ -121,6 +148,13 @@ bool SelfCollisionTask::init()
         std::lock_guard lock(mutex_);
         arm_ = std::move(arm);
         latest_status_ = {};
+        joint_history_.clear();
+        recovery_path_.clear();
+        history_epoch_ = DistanceSamplingPolicy::Clock::now();
+        recovery_clear_since_.reset();
+        recovery_best_distance_m_ = 0.0;
+        recovery_clear_confirmed_ = false;
+        next_event_id_ = 1;
         last_error_.clear();
         state_ = TaskState::IDLE;
     }
@@ -144,6 +178,12 @@ bool SelfCollisionTask::start()
     }
     sampling_.reset();
     latest_status_ = {};
+    joint_history_.clear();
+    recovery_path_.clear();
+    history_epoch_ = DistanceSamplingPolicy::Clock::now();
+    recovery_clear_since_.reset();
+    recovery_best_distance_m_ = 0.0;
+    recovery_clear_confirmed_ = false;
     last_error_.clear();
     state_ = TaskState::RUNNING;
     return true;
@@ -161,27 +201,49 @@ bool SelfCollisionTask::step(const double dt)
         arm = arm_;
     }
 
-    const auto joint_state = arm->getJointState();
-    CollisionGeometrySnapshot snapshot;
-    std::string error;
-    if (!checker_.makeSnapshot(joint_state.position, &snapshot, &error)) {
-        std::lock_guard lock(mutex_);
-        last_error_ = std::move(error);
-        state_ = TaskState::FAILED;
+    const auto fail_monitoring = [&](std::string error) {
+        bool cancel_recovery = false;
+        {
+            std::lock_guard lock(mutex_);
+            cancel_recovery =
+                latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING;
+            if (cancel_recovery) {
+                latest_status_.recovery_state = ProtectiveRecoveryState::FAILED;
+                latest_status_.recovery_error = error;
+            }
+            last_error_ = std::move(error);
+            state_ = TaskState::FAILED;
+            recovery_cv_.notify_all();
+        }
+        if (cancel_recovery) {
+            (void)arm->protectiveStop();
+        }
         return false;
+    };
+
+    const auto joint_state = arm->getJointState();
+    const auto model = arm->getRobotModel();
+    if (!joint_state.validForModel(model)) {
+        return fail_monitoring("Robot arm returned an invalid joint state");
     }
 
     const auto now = DistanceSamplingPolicy::Clock::now();
+    recordJointSample_(joint_state, now);
+
+    CollisionGeometrySnapshot snapshot;
+    std::string error;
+    if (!checker_.makeSnapshot(joint_state.position, &snapshot, &error)) {
+        return fail_monitoring(std::move(error));
+    }
+
     if (!sampling_.shouldCheck(snapshot, now)) {
         return true;
     }
 
     SelfCollisionResult result = checker_.check(snapshot);
     if (!result.valid) {
-        std::lock_guard lock(mutex_);
-        last_error_ = result.error.empty() ? "Self-collision distance check failed" : result.error;
-        state_ = TaskState::FAILED;
-        return false;
+        return fail_monitoring(
+            result.error.empty() ? "Self-collision distance check failed" : result.error);
     }
     sampling_.markChecked(snapshot, now);
 
@@ -193,6 +255,7 @@ bool SelfCollisionTask::step(const double dt)
     }
 
     bool trigger_stop = false;
+    bool abort_recovery = false;
     CollisionSafetyLevel previous_level = CollisionSafetyLevel::UNKNOWN;
     {
         std::lock_guard lock(mutex_);
@@ -202,9 +265,63 @@ bool SelfCollisionTask::step(const double dt)
         previous_level = latest_status_.level;
         latest_status_.level = level;
         latest_status_.result = result;
-        if (level == CollisionSafetyLevel::STOP && !latest_status_.stop_latched) {
+
+        if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) {
+            if (arm->isEmergencyStopped()) {
+                latest_status_.recovery_state = ProtectiveRecoveryState::FAILED;
+                latest_status_.recovery_error =
+                    "Protective recovery interrupted by emergency stop";
+                recovery_clear_since_.reset();
+                abort_recovery = true;
+                recovery_cv_.notify_all();
+            } else if (result.minimum_distance_m +
+                           config_.recovery().max_distance_regression_m() <
+                       recovery_best_distance_m_) {
+                std::ostringstream stream;
+                stream << "Protective recovery distance regressed from "
+                       << recovery_best_distance_m_ << " m to "
+                       << result.minimum_distance_m << " m";
+                latest_status_.recovery_state = ProtectiveRecoveryState::FAILED;
+                latest_status_.recovery_error = stream.str();
+                recovery_clear_since_.reset();
+                abort_recovery = true;
+                recovery_cv_.notify_all();
+            } else {
+                recovery_best_distance_m_ = std::max(
+                    recovery_best_distance_m_, result.minimum_distance_m);
+                if (result.minimum_distance_m >=
+                    config_.recovery().clear_distance_m()) {
+                    if (!recovery_clear_since_) {
+                        recovery_clear_since_ = now;
+                    } else if (std::chrono::duration<double>(
+                                   now - *recovery_clear_since_).count() >=
+                               config_.recovery().stable_period_s()) {
+                        recovery_clear_confirmed_ = true;
+                        recovery_cv_.notify_all();
+                    }
+                } else {
+                    recovery_clear_since_.reset();
+                    recovery_clear_confirmed_ = false;
+                }
+            }
+        } else if (level == CollisionSafetyLevel::STOP &&
+                   !latest_status_.stop_latched) {
             latest_status_.stop_latched = true;
+            latest_status_.event_id = next_event_id_++;
+            latest_status_.recovery_state = ProtectiveRecoveryState::AVAILABLE;
+            latest_status_.recovery_error.clear();
+            recovery_path_.assign(joint_history_.begin(), joint_history_.end());
+            latest_status_.recovery_sample_count = recovery_path_.size();
             trigger_stop = true;
+        } else if (!latest_status_.stop_latched &&
+                   result.minimum_distance_m >=
+                       config_.recovery().clear_distance_m()) {
+            joint_history_.clear();
+            device::JointTrajectoryPoint sample;
+            sample.time_s = std::chrono::duration<double>(now - history_epoch_).count();
+            sample.position = joint_state.position;
+            sample.velocity = joint_state.velocity;
+            joint_history_.push_back(std::move(sample));
         }
     }
 
@@ -220,7 +337,9 @@ bool SelfCollisionTask::step(const double dt)
         }
     }
 
-    if (trigger_stop) {
+    if (abort_recovery) {
+        (void)arm->protectiveStop();
+    } else if (trigger_stop) {
         const auto stop_result = arm->protectiveStop();
         if (!stop_result.ok()) {
             std::lock_guard lock(mutex_);
@@ -232,11 +351,165 @@ bool SelfCollisionTask::step(const double dt)
     return true;
 }
 
-void SelfCollisionTask::stop()
+void SelfCollisionTask::recordJointSample_(
+    const device::JointGroupState& joint_state,
+    const DistanceSamplingPolicy::Clock::time_point now)
 {
     std::lock_guard lock(mutex_);
-    if (state_ != TaskState::FAILED) {
-        state_ = TaskState::STOPPED;
+    if (state_ != TaskState::RUNNING || latest_status_.stop_latched) {
+        return;
+    }
+
+    device::JointTrajectoryPoint sample;
+    sample.time_s = std::chrono::duration<double>(now - history_epoch_).count();
+    sample.position = joint_state.position;
+    sample.velocity = joint_state.velocity;
+    if (!joint_history_.empty() && sample.time_s <= joint_history_.back().time_s) {
+        return;
+    }
+    joint_history_.push_back(std::move(sample));
+
+    const double oldest_time_s = joint_history_.back().time_s -
+                                 config_.recovery().history_duration_s();
+    while (joint_history_.size() > 1 &&
+           joint_history_.front().time_s < oldest_time_s) {
+        joint_history_.pop_front();
+    }
+}
+
+device::Result SelfCollisionTask::requestRecovery(const std::uint64_t event_id)
+{
+    std::shared_ptr<device::RobotArm> arm;
+    device::JointTrajectory path;
+    device::MotionOptions options;
+    {
+        std::lock_guard lock(mutex_);
+        if (state_ != TaskState::RUNNING) {
+            return device::Result::failure(
+                device::ArmErrorCode::RobotNotReady,
+                "Protective recovery rejected: collision task is not running");
+        }
+        if (!latest_status_.stop_latched ||
+            latest_status_.recovery_state != ProtectiveRecoveryState::AVAILABLE) {
+            return device::Result::failure(
+                device::ArmErrorCode::CommandRejected,
+                "Protective recovery rejected: no recoverable collision stop is available");
+        }
+        if (event_id == 0 || event_id != latest_status_.event_id) {
+            return device::Result::failure(
+                device::ArmErrorCode::InvalidArgument,
+                "Protective recovery rejected: event_id does not match the active stop");
+        }
+        if (!arm_ || !arm_->isProtectiveStopped() || arm_->isEmergencyStopped()) {
+            return device::Result::failure(
+                arm_ && arm_->isEmergencyStopped()
+                    ? device::ArmErrorCode::RobotInEmergencyStop
+                    : device::ArmErrorCode::RobotNotReady,
+                "Protective recovery rejected: arm safety state is invalid");
+        }
+        if (recovery_path_.size() < 2) {
+            latest_status_.recovery_state = ProtectiveRecoveryState::FAILED;
+            latest_status_.recovery_error =
+                "Protective recovery path contains fewer than two samples";
+            return device::Result::failure(
+                device::ArmErrorCode::CommandRejected,
+                latest_status_.recovery_error);
+        }
+
+        arm = arm_;
+        path = recovery_path_;
+        options.velocity = config_.recovery().max_joint_velocity_rad_s();
+        options.acceleration =
+            config_.recovery().max_joint_acceleration_rad_s2();
+        latest_status_.recovery_state = ProtectiveRecoveryState::RECOVERING;
+        latest_status_.recovery_error.clear();
+        recovery_best_distance_m_ = latest_status_.result.minimum_distance_m;
+        recovery_clear_since_.reset();
+        recovery_clear_confirmed_ = false;
+    }
+
+    CMVR_LOG(INFO) << "[SelfCollisionTask] recovery requested event_id="
+                   << event_id << ", samples=" << path.size()
+                   << ", max_joint_velocity_rad_s="
+                   << options.velocity
+                   << ", max_joint_acceleration_rad_s2="
+                   << options.acceleration;
+    const auto playback_result = arm->recoverProtectiveStop(path, options);
+    if (!playback_result.ok()) {
+        std::lock_guard lock(mutex_);
+        if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) {
+            latest_status_.recovery_state = ProtectiveRecoveryState::FAILED;
+            latest_status_.recovery_error = playback_result.message;
+        }
+        const std::string recovery_error = latest_status_.recovery_error;
+        recovery_cv_.notify_all();
+        return device::Result::failure(playback_result.code, recovery_error);
+    }
+
+    {
+        std::unique_lock lock(mutex_);
+        const auto clear_timeout = std::chrono::duration<double>(
+            config_.recovery().stable_period_s() + 2.0);
+        const bool completed = recovery_cv_.wait_for(lock, clear_timeout, [&] {
+            return state_ != TaskState::RUNNING || recovery_clear_confirmed_ ||
+                   latest_status_.recovery_state != ProtectiveRecoveryState::RECOVERING;
+        });
+        if (!completed || !recovery_clear_confirmed_) {
+            if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) {
+                latest_status_.recovery_state = ProtectiveRecoveryState::FAILED;
+                latest_status_.recovery_error = completed
+                    ? "Protective recovery ended before the clear distance was confirmed"
+                    : "Protective recovery clear-distance confirmation timed out";
+            }
+            const std::string error = latest_status_.recovery_error;
+            lock.unlock();
+            (void)arm->protectiveStop();
+            return device::Result::failure(
+                device::ArmErrorCode::CommandFailed, error);
+        }
+
+        latest_status_.stop_latched = false;
+        latest_status_.recovery_state = ProtectiveRecoveryState::SUCCEEDED;
+        latest_status_.recovery_error.clear();
+        joint_history_.clear();
+        joint_history_.push_back(path.front());
+        recovery_path_.clear();
+        latest_status_.recovery_sample_count = 0;
+    }
+
+    const auto unlock_result = arm->unlockProtectiveStop();
+    if (!unlock_result.ok()) {
+        std::lock_guard lock(mutex_);
+        latest_status_.stop_latched = true;
+        latest_status_.recovery_state = ProtectiveRecoveryState::FAILED;
+        latest_status_.recovery_error =
+            "Failed to unlock protective stop: " + unlock_result.message;
+        return device::Result::failure(
+            unlock_result.code, latest_status_.recovery_error);
+    }
+
+    CMVR_LOG(INFO) << "[SelfCollisionTask] recovery completed event_id="
+                   << event_id << ", clear_distance_m="
+                   << config_.recovery().clear_distance_m();
+    return device::Result::success();
+}
+
+void SelfCollisionTask::stop()
+{
+    std::shared_ptr<device::RobotArm> arm;
+    bool cancel_recovery = false;
+    {
+        std::lock_guard lock(mutex_);
+        cancel_recovery =
+            latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING;
+        arm = arm_;
+        if (state_ != TaskState::FAILED) {
+            state_ = TaskState::STOPPED;
+        }
+        recovery_cv_.notify_all();
+    }
+    if (cancel_recovery && arm) {
+        (void)arm->protectiveStop();
     }
 }
 
@@ -274,13 +547,20 @@ std::string SelfCollisionTask::detailStatusString() const
     }
     std::ostringstream stream;
     stream << taskStateToString(state_)
-           << " level=" << safetyLevelToString(latest_status_.level);
+           << " level=" << safetyLevelToString(latest_status_.level)
+           << " stop_latched=" << latest_status_.stop_latched
+           << " event_id=" << latest_status_.event_id
+           << " recovery="
+           << recoveryStateToString(latest_status_.recovery_state);
     if (latest_status_.result.valid) {
         stream << " distance_m=" << std::setprecision(6)
                << latest_status_.result.minimum_distance_m
                << " pair=" << latest_status_.result.first
                << "/" << latest_status_.result.second;
     }
+    if (!latest_status_.recovery_error.empty()) {
+        stream << " recovery_error=" << latest_status_.recovery_error;
+    }
     return stream.str();
 }
 
@@ -301,4 +581,17 @@ const char* SelfCollisionTask::safetyLevelToString(const CollisionSafetyLevel le
     return "UNKNOWN";
 }
 
+const char* SelfCollisionTask::recoveryStateToString(
+    const ProtectiveRecoveryState state)
+{
+    switch (state) {
+        case ProtectiveRecoveryState::IDLE: return "IDLE";
+        case ProtectiveRecoveryState::AVAILABLE: return "AVAILABLE";
+        case ProtectiveRecoveryState::RECOVERING: return "RECOVERING";
+        case ProtectiveRecoveryState::SUCCEEDED: return "SUCCEEDED";
+        case ProtectiveRecoveryState::FAILED: return "FAILED";
+    }
+    return "UNKNOWN";
+}
+
 } // namespace cmvr::task

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

@@ -385,6 +385,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
 bool TouchScreenTask::touch(const int u, const int v) {
     std::lock_guard<std::mutex> lock(mutex_);
     if (isBusyUnlocked()) {
+        last_status_ = Status::TASK_BUSY;
         return false;
     }
     return startFromPixelUnlocked(u, v);

cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp

@@ -4,7 +4,6 @@
 #include <array>
 #include <chrono>
 #include <cmath>
-#include <condition_variable>
 #include <cstdint>
 #include <filesystem>
 #include <iostream>
@@ -28,10 +27,10 @@
 #include "devices/arm/robot_arm_factory.h"
 #include "devices/camera/mujoco_camera/include/mujoco_camera.h"
 #include "devices/motor/manager/include/motor_manager.h"
-#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
 #include "manager/device_manager/include/device_manager.h"
 #include "manager/task_manager/include/task_manager.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
 namespace {
 
@@ -68,178 +67,6 @@ std::filesystem::path findProjectRoot()
     return search(std::filesystem::path(__FILE__).parent_path());
 }
 
-class TouchMujocoViewer final : public cmvr::MuJocoViewer {
-public:
-    TouchMujocoViewer(const std::string& model_path,
-                      std::shared_ptr<cmvr::device::MujocoJointBridge> bridge)
-        : MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge))
-    {
-        position_actuator_ids_.fill(-1);
-        qpos_ids_.fill(-1);
-        qvel_ids_.fill(-1);
-    }
-
-    std::shared_ptr<cmvr::device::MujocoCamera> waitCameraReady(
-        const std::chrono::milliseconds timeout)
-    {
-        std::unique_lock<std::mutex> lock(camera_mutex_);
-        if (!camera_cv_.wait_for(lock, timeout, [this] { return camera_ != nullptr; })) {
-            return nullptr;
-        }
-        return camera_;
-    }
-
-protected:
-    void initOnce(mjModel* model, mjData* data) override
-    {
-        setupCamera(2.5, -160.0, -25.0);
-        enablePiPCamera("hand_cam");
-
-        bool valid = true;
-        for (std::size_t i = 0; i < kDof; ++i) {
-            const std::string actuator_name = std::string(kJointNames[i]) + "_pos";
-            position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str());
-            const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]);
-            if (position_actuator_ids_[i] < 0 || joint_id < 0) {
-                valid = false;
-                continue;
-            }
-            qpos_ids_[i] = model->jnt_qposadr[joint_id];
-            qvel_ids_[i] = model->jnt_dofadr[joint_id];
-            position_reference_[i] = data->qpos[qpos_ids_[i]];
-        }
-
-        const int hand_cam_id = mj_name2id(model, mjOBJ_CAMERA, "hand_cam");
-        if (hand_cam_id < 0) {
-            valid = false;
-        }
-
-        auto camera = std::make_shared<cmvr::device::MujocoCamera>(
-            [this](std::vector<unsigned char>& rgb,
-                   std::vector<float>& depth,
-                   int& width,
-                   int& height,
-                   std::uint64_t& frame_id) {
-                return getPiPCameraRGBD(rgb, depth, width, height, frame_id);
-            });
-        if (hand_cam_id >= 0) {
-            camera->setFovyDeg(model->cam_fovy[hand_cam_id]);
-        }
-        camera->setConsumeNewFrameOnly(false);
-        {
-            std::lock_guard<std::mutex> lock(camera_mutex_);
-            camera_ = std::move(camera);
-        }
-        camera_cv_.notify_all();
-        bridge_->markReady(valid);
-    }
-
-    void controlCallback(mjModel* model, mjData* data) override
-    {
-        std::vector<double> measured_position(kDof, 0.0);
-        std::vector<double> measured_velocity(kDof, 0.0);
-        for (std::size_t i = 0; i < kDof; ++i) {
-            if (qpos_ids_[i] >= 0) {
-                measured_position[i] = data->qpos[qpos_ids_[i]];
-            }
-            if (qvel_ids_[i] >= 0) {
-                measured_velocity[i] = data->qvel[qvel_ids_[i]];
-            }
-        }
-        bridge_->publishMeasured(measured_position, measured_velocity);
-
-        const auto commands = bridge_->commands();
-        for (std::size_t i = 0; i < kDof; ++i) {
-            const int actuator_id = position_actuator_ids_[i];
-            if (actuator_id < 0) {
-                continue;
-            }
-
-            if (commands.mode[i] == cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
-                if (last_mode_[i] != cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) {
-                    position_reference_[i] = data->qpos[qpos_ids_[i]];
-                }
-                position_reference_[i] += commands.velocity[i] * model->opt.timestep;
-            } else {
-                position_reference_[i] = commands.position[i];
-            }
-
-            const double lower = model->actuator_ctrlrange[2 * actuator_id];
-            const double upper = model->actuator_ctrlrange[2 * actuator_id + 1];
-            position_reference_[i] = std::clamp(position_reference_[i], lower, upper);
-            data->ctrl[actuator_id] = position_reference_[i];
-            last_mode_[i] = commands.mode[i];
-        }
-    }
-
-private:
-    std::shared_ptr<cmvr::device::MujocoJointBridge> bridge_;
-    std::array<int, kDof> position_actuator_ids_{};
-    std::array<int, kDof> qpos_ids_{};
-    std::array<int, kDof> qvel_ids_{};
-    std::array<double, kDof> position_reference_{};
-    std::array<cmvr::msgs::RunMode, kDof> last_mode_{};
-    std::mutex camera_mutex_;
-    std::condition_variable camera_cv_;
-    std::shared_ptr<cmvr::device::MujocoCamera> camera_{nullptr};
-};
-
-class ZeroTouchDexHand final : public cmvr::device::AbstractDexHand {
-public:
-    ZeroTouchDexHand()
-    {
-        id_ = "mujoco_zero_touch_dexhand";
-    }
-
-    std::string typeName() const override { return "ZeroTouchDexHand"; }
-    Status state() const override { return Status::STREAMING; }
-    std::string lastError() const override { return {}; }
-
-    void setAngles(const std::vector<int>& finger_joint_angles) override
-    {
-        (void)finger_joint_angles;
-    }
-
-    void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override
-    {
-        polling_regions_ = regions;
-    }
-
-    std::vector<TactileRegionData> getSensorData() override
-    {
-        std::vector<TactileRegionData> data;
-        if (polling_regions_.empty()) {
-            polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP});
-        }
-        data.reserve(polling_regions_.size());
-        for (const auto& region : polling_regions_) {
-            data.push_back(getSensorData(region.first, region.second));
-        }
-        return data;
-    }
-
-    TactileRegionData getSensorData(const FingerType finger, const TactileRegion region) override
-    {
-        tactile_points_[0] = getResultantForce(finger, region);
-        TactileMatrixView view;
-        view.data = tactile_points_.data();
-        view.rows = 1;
-        view.cols = 1;
-        return {finger, region, view, "mujoco_touch_tip"};
-    }
-
-    ResultantForce getResultantForce(const FingerType finger, const TactileRegion region) override
-    {
-        (void)finger;
-        (void)region;
-        return TactilePoint::fromFz(0);
-    }
-
-private:
-    std::vector<TactileRegionKey> polling_regions_;
-    std::array<TactilePoint, 1> tactile_points_{};
-};
-
 struct TagCenterPixel {
     int tag_id{-1};
     int u{0};
@@ -459,36 +286,75 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
     cmvr::config::DeviceManagerConfig device_manager_config;
     device_manager_config.set_name("touch_screen_mujoco_test");
     device_manager_config.set_version("test");
+    auto* world_entry = device_manager_config.add_devices();
+    world_entry->set_id("mujoco_world");
+    world_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD);
+    world_entry->set_config_file("devices/mujoco/mujoco_world.pb.txt");
+    world_entry->set_enable(true);
     auto* motor_entry = device_manager_config.add_devices();
-    motor_entry->set_id("mujoco_motors");
+    motor_entry->set_id("right_arm_mujoco_motors");
     motor_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
     motor_entry->set_config_file("devices/motor/mujoco_motors.pb.txt");
     motor_entry->set_enable(true);
     auto* arm_entry = device_manager_config.add_devices();
-    arm_entry->set_id("right_arm_mujoco");
+    arm_entry->set_id("mujoco_right_arm");
     arm_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM);
     arm_entry->set_config_file("devices/arm/arm_mujoco.pb.txt");
     arm_entry->set_enable(true);
+    auto* dexhand_entry = device_manager_config.add_devices();
+    dexhand_entry->set_id("mujoco_zero_touch_dexhand");
+    dexhand_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND);
+    dexhand_entry->set_config_file("devices/dexhand/dexhand.pb.txt");
+    dexhand_entry->set_enable(true);
 
     auto& device_manager = cmvr::device::DeviceManager::getInstance(device_manager_config);
-    auto motor_system = device_manager.getDevice<cmvr::device::MotorManager>("mujoco_motors");
+    auto motor_system = device_manager.getDevice<cmvr::device::MotorManager>("right_arm_mujoco_motors");
     if (!motor_system) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: mujoco_motors";
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: right_arm_mujoco_motors";
         return;
     }
-    auto bridge = cmvr::device::MotorManager::mujocoBridgeFor("mujoco_motors");
-    if (!bridge) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors";
+    auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
+    if (!world || !world->isLoaded() || !world->isRunning()) {
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco world is not ready: mujoco_world";
         return;
     }
-    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("right_arm_mujoco");
+    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
     if (!arm) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: right_arm_mujoco";
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: mujoco_right_arm";
         return;
     }
+    // The task config still uses the historical ID; keep it as a test-only alias.
+    device_manager.registerDevice("right_arm_mujoco", arm);
 
-    TouchMujocoViewer viewer(
-        (project_root / "model/xiaoyan_description/dual_arm.xml").string(), bridge);
+    cmvr::MuJocoViewer viewer(world);
+    viewer.setupCamera(2.5, -160.0, -25.0);
+    viewer.enablePiPCamera("hand_cam");
+
+    auto camera = std::make_shared<cmvr::device::MujocoCamera>(
+        [&viewer](std::vector<unsigned char>& rgb,
+                  std::vector<float>& depth,
+                  int& width,
+                  int& height,
+                  std::uint64_t& frame_id) {
+            return viewer.getPiPCameraRGBD(rgb, depth, width, height, frame_id);
+        });
+    camera->setConsumeNewFrameOnly(true);
+    {
+        std::lock_guard<std::mutex> lock(world->mutex());
+        const auto* model = world->model();
+        const int hand_cam_id = model == nullptr
+            ? -1
+            : mj_name2id(model, mjOBJ_CAMERA, "hand_cam");
+        if (hand_cam_id < 0) {
+            CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MuJoCo camera not found: hand_cam";
+            return;
+        }
+        camera->setFovyDeg(model->cam_fovy[hand_cam_id]);
+    }
+    if (!camera->init()) {
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Failed to initialize MuJoCo camera";
+        return;
+    }
 
     struct Outcome {
         bool init_ok{false};
@@ -506,17 +372,8 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
 
     std::thread scenario([&] {
         try {
-            if (!bridge->waitUntilReady(std::chrono::seconds(10))) {
-                throw std::runtime_error("MuJoCo right-arm joints, actuators, or hand_cam are not ready");
-            }
-            auto camera = viewer.waitCameraReady(std::chrono::seconds(10));
-            if (!camera) {
-                throw std::runtime_error("MuJoCo camera is not ready");
-            }
-
             const auto touch_config = loadMujocoTouchConfig(project_root);
             device_manager.registerDevice(touch_config.devices().camera_id(), camera);
-            device_manager.registerDevice(std::make_shared<ZeroTouchDexHand>());
 
             cmvr::config::TaskManagerConfig task_manager_config;
             auto* task_entry = task_manager_config.add_tasks();
@@ -533,6 +390,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
                 throw std::runtime_error("TouchScreenTask not found: " + touch_config.id());
             }
             outcome.init_ok = task->lastStatus() != cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED;
+
             task_manager.startRunTask();
 
             TagCenterPixel target_pixel;
@@ -548,10 +406,31 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
                       << ", v=" << target_pixel.v
                       << std::endl;
 
+            std::cout << "[TouchScreenTaskMujocoTest] before task->touch, phase="
+                      << cmvr::task::TouchScreenTask::phaseToString(task->phase())
+                      << ", status="
+                      << cmvr::task::TouchScreenTask::statusToString(task->lastStatus())
+                      << std::endl;
+            const auto touch_start = std::chrono::steady_clock::now();
             outcome.touch_ok = task->touch(target_pixel.u, target_pixel.v);
+            std::cout << "[TouchScreenTaskMujocoTest] after task->touch, ok="
+                      << (outcome.touch_ok ? 1 : 0)
+                      << ", elapsed_ms="
+                      << std::chrono::duration<double, std::milli>(
+                             std::chrono::steady_clock::now() - touch_start)
+                             .count()
+                      << ", phase="
+                      << cmvr::task::TouchScreenTask::phaseToString(task->phase())
+                      << ", status="
+                      << cmvr::task::TouchScreenTask::statusToString(task->lastStatus())
+                      << std::endl;
             if (!outcome.touch_ok) {
                 outcome.final_status = task->lastStatus();
-                return;
+                task_manager.stopRunTask();
+                viewer.requestStop();
+                throw std::runtime_error(
+                    "task->touch failed, status=" +
+                    std::string(cmvr::task::TouchScreenTask::statusToString(outcome.final_status)));
             }
 
             const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(20);
@@ -604,6 +483,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
                 cmvr::task::TaskManager::getInstance().stopRunTask();
             } catch (...) {
             }
+            viewer.requestStop();
             outcome.worker_error = error.what();
             std::cerr << "[TouchScreenTaskMujocoTest] scenario error: " << outcome.worker_error
                       << "\nClose the MuJoCo viewer window to finish gtest." << std::endl;

dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf

@@ -1,3 +1,3 @@
-MASTER0_DEVICE="a0:ad:9f:c4:c2:2c"
+MASTER0_DEVICE="42:e6:6d:44:c1:0f"
 DEVICE_MODULES="generic"
 UPDOWN_INTERFACES="eno1"

docs/ethercat_motor_tutorial.md

@@ -75,6 +75,14 @@ motor {
         stopped_velocity_tolerance_rad_s: 0.001
       }
 
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 1000
+      }
+
       slaves { motor_id: 1 alias: 0 position: 0 }
       slaves { motor_id: 2 alias: 0 position: 1 }
       slaves { motor_id: 3 alias: 0 position: 2 }

model/gen2/assets/10100.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "b26d4d3d1e4a010d0d624309",
+    "elementId": "384bd645b0ca4a89ac420dd3",
+    "fullConfiguration": "default",
+    "id": "M4Rmvs8/9nOQASQoz",
+    "isStandardContent": false,
+    "name": "10100 <1>",
+    "partId": "JyD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/10100__2.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "b26d4d3d1e4a010d0d624309",
+    "elementId": "384bd645b0ca4a89ac420dd3",
+    "fullConfiguration": "default",
+    "id": "MHaJriwfEEKCaNP0z",
+    "isStandardContent": false,
+    "name": "10100 <2>",
+    "partId": "J5D",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/10100__3.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "8073bb18db19f7fa7997ad99",
+    "documentMicroversion": "428a1bfe2c594dd041131bcd",
+    "documentVersion": "102e666e83d6b78c25c9ce78",
+    "elementId": "5decfde3994a7031e4d53265",
+    "fullConfiguration": "default",
+    "id": "MMSYOwzMqbgilS4dL",
+    "isStandardContent": false,
+    "name": "10100 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/1020001.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "387522184bef9b82eac1fdfc",
+    "documentMicroversion": "caf63b0301e581fe867e46ba",
+    "documentVersion": "961b145da2fcbaf35ddb6766",
+    "elementId": "58a93d37d32edd9f1f80c4a6",
+    "fullConfiguration": "default",
+    "id": "MtGpd/bL/Q+stBW6B",
+    "isStandardContent": false,
+    "name": "1020001 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/arm_link_1.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "M77c5kvnmFYX3+E1/",
+    "isStandardContent": false,
+    "name": "arm_link_1 <2>",
+    "partId": "RHDD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/arm_link_2.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MAagFsEi4vOqUYBnE",
+    "isStandardContent": false,
+    "name": "arm_link_2 <2>",
+    "partId": "JvD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/arm_link_3.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MODZ2heuMlDmDK6JN",
+    "isStandardContent": false,
+    "name": "arm_link_3 <2>",
+    "partId": "RRBD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/arm_link_4.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MmEi+ZosIdG9Wp+iP",
+    "isStandardContent": false,
+    "name": "arm_link_4 <2>",
+    "partId": "RwCD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/arm_link_5.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MBPP2k1l9Xc6L3QvC",
+    "isStandardContent": false,
+    "name": "arm_link_5 <2>",
+    "partId": "RxCD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/arm_link_6.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MIcvuqnQVxIZF1ql0",
+    "isStandardContent": false,
+    "name": "arm_link_6 <2>",
+    "partId": "RyCD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/arm_link_7.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "Mv//Etw9ckikOO5Dz",
+    "isStandardContent": false,
+    "name": "arm_link_7 <2>",
+    "partId": "RrCD",
+    "suppressed": false,
+    "type": "Part"
+}