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

@@ -20,80 +20,12 @@ 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)
@@ -150,64 +82,6 @@ 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,8 +53,6 @@ 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,14 +61,9 @@ 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()) {
+    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
-        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_();
 
@@ -108,7 +103,15 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
-    requestStop_(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();
     return Result::success();
 }
 
@@ -189,26 +192,27 @@ void CartesianVelocityController::workerLoop_()
             }
 
             if (!planner_->updateSpeedLAcceleration(acceleration)) {
-                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
-                    abortCommand_();
+                    std::lock_guard<std::mutex> lock(mutex_);
+                    command_active_ = false;
+                    sendZero_();
+                    busy_.store(false);
                     break;
                 }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
-                requestStop_();
+                sendZero_();
-                continue;
+                busy_.store(false);
+                return;
             }
 
             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";
-                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                sendZero_();
-                    abortCommand_();
+                busy_.store(false);
-                    break;
+                return;
-                }
-                requestStop_();
-                continue;
             }
 
             std::vector<double> qd_cmd;
@@ -218,12 +222,9 @@ void CartesianVelocityController::workerLoop_()
                                 << target_twist.vz << ", " << target_twist.wx << ", "
                                 << target_twist.wy << ", " << target_twist.wz
                                 << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
-                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                sendZero_();
-                    abortCommand_();
+                busy_.store(false);
-                    break;
+                return;
-                }
-                requestStop_();
-                continue;
             }
 
             JointVelocityCommand velocity_command;
@@ -232,12 +233,9 @@ void CartesianVelocityController::workerLoop_()
             if (!send_result.ok()) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
                                 << send_result.message;
-                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
+                sendZero_();
-                    abortCommand_();
+                busy_.store(false);
-                    break;
+                return;
-                }
-                requestStop_();
-                continue;
             }
 
             if (twistNorm_(target_twist) < config_.stop_twist_norm &&
@@ -262,32 +260,6 @@ 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,14 +14,3 @@ 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,11 +932,7 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         qdot = applyJointAccelerationLimits_(qdot, reference, dt);
     }
     const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    // During a stop, the limiter intentionally commands a near-zero residual
+    if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
-    // 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,15 +1,18 @@
 #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;
@@ -20,137 +23,9 @@ public:
                            const JointPositionCommand& target,
                            const MotionOptions& options,
                            double speed_scaling,
-                           JointTrajectory& trajectory) = 0;
+                           std::vector<JointTrajectorySample>& samples) = 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,19 +21,9 @@ public:
                    const JointPositionCommand& target,
                    const MotionOptions& options,
                    double speed_scaling,
-                   JointTrajectory& trajectory) override;
+                   std::vector<JointTrajectorySample>& samples) 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,9 +1,6 @@
 #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 {
 
@@ -42,215 +39,36 @@ 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,
-                                         JointTrajectory& trajectory)
+                                         std::vector<JointTrajectorySample>& samples)
 {
-    trajectory.clear();
+    samples.clear();
     if (!planner_ || start.empty() || start.size() != target.position.size() ||
         options.velocity <= 0.0 || options.acceleration <= 0.0) {
         return false;
     }
 
-    cmvr::TrajPtr raw_trajectory;
+    cmvr::TrajPtr 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, raw_trajectory)) {
+    if (!planner_->plan(start, target.position, trajectory)) {
         return false;
     }
 
-    return sampleTrajectory_(planner_, raw_trajectory, trajectory);
+    const auto raw_samples = planner_->sampleTrajectory(trajectory, sample_period_s_);
-}
+    samples.reserve(raw_samples.size());
-
+    for (const auto& sample : raw_samples) {
-bool ToppraJointMotionPlanner::planReplay(
+        JointTrajectorySample dst;
-    const std::vector<double>& current_position,
+        dst.t = sample.t;
-    const JointTrajectory& recorded_trajectory,
+        dst.position = toStdVector(sample.q);
-    const MotionOptions& options,
+        dst.velocity = toStdVector(sample.qd);
-    JointTrajectory& replay_trajectory)
+        samples.push_back(std::move(dst));
-{
-    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

@@ -1,139 +0,0 @@
-#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,14 +21,3 @@ 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,18 +109,37 @@ namespace cmvr {
         static void sanitizeVsq(toppra::Vector &v);
 
 
-        // Joint-space chord length keeps the parameterization independent of
+        // centripetal 弦长（alpha=0.5），生成严格递增 S
-        // how densely the same geometric path is sampled.
         static std::vector<toppra::value_type>
-        makeSChordLength(const std::vector<Eigen::VectorXd> &q) {
+        makeS_centripetal(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] + (q[i] - q[i - 1]).norm();
+                S[i] = S[i - 1] + chord(q[i], q[i - 1]);
+                if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12;
             }
             return S;
         }
 
+        // 等距参数（简单稳妥）
+        static inline std::vector<toppra::value_type> makeS_equal(size_t M) {
+            std::vector<toppra::value_type> S(M);
+            for (size_t i = 0; i < M; ++i) S[i] = static_cast<toppra::value_type>(i);
+            return S;
+        }
+
+        // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds
+        static inline void normalize_and_floor_S(std::vector<toppra::value_type> &S, double ds_min = 0.2) {
+            for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0];
+            double L = S.back();
+            if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L;
+            for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min;
+        }
+
         // Catmull–Rom（centripetal）估计结点几何速度 v（端点=0）
         static std::vector<Eigen::VectorXd>
         estimateVelsCatmull(const std::vector<Eigen::VectorXd> &q,
@@ -140,16 +159,14 @@ 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) {
-                const double ds0 = std::max<double>(S[i] - S[i - 1], 1e-12);
+                double d0 = (q[i] - q[i - 1]).norm();
-                const double ds1 = std::max<double>(S[i + 1] - S[i], 1e-12);
+                double d1 = (q[i + 1] - q[i]).norm();
-                const double slope0 = (q[i] - q[i - 1]).norm() / ds0;
+                double d = std::max(std::min(d0, d1), 1e-12);
-                const double slope1 = (q[i + 1] - q[i]).norm() / ds1;
+                double vmax = k * d;
-                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,117 +5,10 @@
 #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)) {
@@ -161,39 +54,22 @@ namespace cmvr {
     bool ToppraJointTrajectoryPlanner::plan(const std::vector<std::vector<double>>& waypoints,
                              TrajPtr& traj_out) {
         traj_out.reset();
-        if (waypoints.size() < 2) return false;
+        const size_t M = waypoints.size();
+        if (M < 2) return false;
         const size_t DoF = waypoints.front().size();
-        if (DoF == 0) return false;
+        for (const auto& w : waypoints) if (w.size()!=DoF) 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(waypoints.size());
+        std::vector<Eigen::VectorXd> q; q.reserve(M);
-        constexpr double kDuplicateDistance = 1e-10;
+        for (const auto& w : waypoints)
-        for (const auto& waypoint : waypoints) {
+            q.emplace_back(Eigen::Map<const Eigen::VectorXd>(w.data(), DoF));
-            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();
 
-        const std::vector<toppra::value_type> S = M == 2
+        // 生成 S
-            ? std::vector<toppra::value_type>{0.0, 1.0}
+//        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
-            : makeSChordLength(q);
+//                                                   : makeS_centripetal(q);
+        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
+                                                   : makeS_equal(M);
 
         // 几何路径
         auto path = buildPathUnified(q, S);
@@ -210,23 +86,8 @@ namespace cmvr {
 
         // TOPPRA
         toppra::algorithm::TOPPRA algo{constraints, path};
-        auto solve_once = [&](const int requested_intervals)->bool{
+        auto solve_once = [&](int N)->bool{
-            const int segment_count = static_cast<int>(M - 1);
+            algo.setN(N);
-            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;
         };
@@ -238,21 +99,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()) {
-            candidate = std::make_shared<ConstAccelTraj>(std::move(ca));
+            traj_out = std::make_shared<ConstAccelTraj>(std::move(ca));
-        } else {
+            return true;
+        }
         sanitizeVsq(vsq);
         try {
-                candidate = std::make_shared<SplineTraj>(path, grid, vsq);
+            traj_out = std::make_shared<SplineTraj>(path, grid, vsq);
-                (void) candidate->timeInterval();
+            (void) traj_out->timeInterval();
+            return true;
         } catch (...) {
             return false;
         }
     }
-        return enforceSampledLimits(candidate, v_max_, a_max_, M, traj_out);
-    }
 
 
     bool ToppraJointTrajectoryPlanner::plan(const std::vector<double>& start_joints,
@@ -365,7 +225,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].transpose();
+            Eigen::RowVectorXd A0 = (q[k] - A1.transpose()*S[k]).transpose();
             seg.row(0)=A1; seg.row(1)=A0;
             segs.emplace_back(std::move(seg));
         }
@@ -377,7 +237,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, S, /*k=*/1.0);
+        clampNodeVels(v, q, /*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);
@@ -422,7 +282,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, S, /*k=*/1.0);
+        clampNodeVels(v, q, /*k=*/1.0);
         auto a = estimateAccelsSecondDiff(q, S);
 
         toppra::Matrices segs; segs.reserve(M-1);
@@ -441,11 +301,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 / std::pow(ds, 5)).transpose();
+            seg.row(0)=C5.transpose();
-            seg.row(1)=(C4 / std::pow(ds, 4)).transpose();
+            seg.row(1)=C4.transpose();
-            seg.row(2)=(C3 / std::pow(ds, 3)).transpose();
+            seg.row(2)=C3.transpose();
-            seg.row(3)=(a0 / 2.0).transpose();
+            seg.row(3)=A2.transpose();
-            seg.row(4)=v0.transpose();
+            seg.row(4)=A1.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

@@ -1,228 +0,0 @@
-#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,12 +29,6 @@ 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,7 +3,6 @@
 //
 
 #pragma once
-#include <cstdint>
 #include <cmath>
 #include <vector>
 #include <algorithm>
@@ -15,25 +14,6 @@ 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

@@ -1,34 +0,0 @@
-#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,14 +112,6 @@ 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: "right_arm_can_motors"
+      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can_motors"
+      motor_group_ids: "right_arm_can"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"

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

@@ -1,151 +0,0 @@
-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: "right_arm_mujoco_motors"
+      motor_system_id: "mujoco_motors"
-      motor_group_ids: "right_arm_mujoco_motors"
+      motor_group_ids: "mujoco_right_arm"
       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: "right_arm_mujoco_motors"
+      motor_system_id: "mujoco_motors"
-      motor_group_ids: "right_arm_mujoco_motors"
+      motor_group_ids: "mujoco_right_arm"
       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: "right_arm_can_motors"
+      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can_motors"
+      motor_group_ids: "right_arm_can"
       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: 480
+        width: 1280
-        height: 320
+        height: 720
         fps: 30
         codec: "H264"
         enable_stream_timestamp: true

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

@@ -38,9 +38,4 @@ 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_motors"
+    id: "right_arm_ethercat"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@@ -14,20 +14,13 @@ 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_two_real_test.pb.txt

@@ -2,7 +2,7 @@ motor {
   id: "ethercat_motors"
 
   motor_groups {
-    id: "right_arm_ethercat_motors"
+    id: "right_arm_ethercat"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@@ -14,20 +14,13 @@ 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: "right_arm_mujoco_motors"
+    id: "mujoco_right_arm"
     bus_type: MOTOR_BUS_MUJOCO
     vendor: MOTOR_VENDOR_MUJOCO
     protocol: MOTOR_PROTOCOL_MUJOCO

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

@@ -1,35 +0,0 @@
-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_motors"
+    id: "left_arm_can"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -26,7 +26,7 @@ motor {
   }
 
   motor_groups {
-    id: "right_arm_can_motors"
+    id: "right_arm_can"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -56,7 +56,7 @@ motor {
   }
 
   motor_groups {
-    id: "head_can_motors"
+    id: "head_can"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -78,7 +78,7 @@ motor {
   }
 
   motor_groups {
-    id: "waist_can_motors"
+    id: "waist_can"
     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: true
+    show_time: false
     show_level: true
     show_thread_id: false
     show_source_location: true

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

@@ -2,39 +2,41 @@ 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: true
+    enable: false
   }
 
   devices {
-    id: "right_arm_mujoco_motors"
+    id: "mujoco_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/mujoco_motors.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "mujoco_right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm_mujoco_qp.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "mujoco_viewer"
     type: DEVICE_TYPE_MUJOCO_VIEWER
     config_file: "devices/mujoco/mujoco_viewer.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "mujoco_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -67,42 +69,14 @@ device_manager {
   }
 
   devices {
-    id: "mujoco_zero_touch_dexhand"
+    id: "ti5_motors"
-    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: "right_arm_can_motors"
+    id: "ethercat_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
@@ -126,7 +100,7 @@ device_manager {
     id: "huayan_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/huayan_arm.pb.txt"
-    enable: false
+    enable: true
   }
 
   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_mujoco.pb.txt"
+    config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
-    enable: true
+    enable: false
   }
   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: false
+    enable: true
   }
 }

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

@@ -21,14 +21,4 @@ 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

@@ -1,37 +0,0 @@
-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: "mujoco_right_arm"
+    arm_id: "right_arm_mujoco"
     dexhand_id: "mujoco_zero_touch_dexhand"
-    camera_id: "mujoco_hand_cam"
+    camera_id: "hand_cam"
   }
 
   initialization {
@@ -90,8 +90,8 @@ touch_screen_task {
   }
 
   retract {
-    twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
+    twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 5.0
+    acceleration: 8.0
-    duration_s: 2.0
+    duration_s: 5.0
   }
 }

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

@@ -4,58 +4,10 @@ add_library(aubo_arm SHARED
 
 target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
+set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include)
-set(AUBO_SDK_INCLUDE_DIR ${AUBO_SDK_ROOT}/include)
+set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib)
-set(AUBO_SDK_LIB_DIR ${AUBO_SDK_ROOT}/lib)
 
-if (EXISTS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk/aubo_sdkConfig.cmake")
+if (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h")
-    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,14 +36,6 @@ 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,14 +42,6 @@ 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,21 +34,3 @@ 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,14 +41,11 @@ public:
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override;
+    Result protectiveStop() override { return emergencyStop(); }
-    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 protective_stopped_.load(); }
+    bool isProtectiveStopped() const override { return false; }
-    bool isEmergencyStopped() const override { return emergency_stopped_.load(); }
+    bool isEmergencyStopped() const override { return emergency_stopped_; }
     bool isFault() const override { return false; }
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
@@ -79,7 +76,7 @@ public:
     Result brakeRelease() override;
     Result shutdown() override;
     Result clearFault() override { return Result::success(); }
-    Result unlockProtectiveStop() override;
+    Result unlockProtectiveStop() override { return Result::success(); }
     Result loadProgram(const std::string& program_name) override;
     Result playProgram() override;
     Result pauseProgram() override;
@@ -97,10 +94,6 @@ 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;
@@ -133,10 +126,7 @@ private:
     mutable std::mutex mutex_;
     std::atomic<bool> busy_{false};
     double speed_scaling_{1.0};
-    std::atomic<bool> protective_stopped_{false};
+    bool emergency_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,7 +2,6 @@
 
 #include <algorithm>
 #include <chrono>
-#include <cmath>
 #include <Eigen/Dense>
 #include <stdexcept>
 #include <thread>
@@ -30,11 +29,6 @@ struct BusyGuard {
     ~BusyGuard() { busy.store(false); }
 };
 
-struct AtomicFlagGuard {
-    std::atomic<bool>& flag;
-    ~AtomicFlagGuard() { flag.store(false); }
-};
-
 } // namespace
 
 MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
@@ -139,15 +133,12 @@ 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.protective_stopped = protective_stopped;
+    state.emergency_stopped = emergency_stopped_;
-    state.emergency_stopped = emergency_stopped;
     state.speed_scaling = speed_scaling_;
     state.robot_mode = RobotMode::Idle;
     state.safety_mode = getSafetyMode();
@@ -196,13 +187,7 @@ CartesianPose MotorRobotArm::getTcpPose(const FrameType frame) const
 
 SafetyMode MotorRobotArm::getSafetyMode() const
 {
-    if (emergency_stopped_.load()) {
+    return emergency_stopped_ ? SafetyMode::EmergencyStop : SafetyMode::Normal;
-        return SafetyMode::EmergencyStop;
-    }
-    if (protective_stopped_.load()) {
-        return SafetyMode::ProtectiveStop;
-    }
-    return SafetyMode::Normal;
 }
 
 Result MotorRobotArm::torqueOn()
@@ -217,7 +202,7 @@ Result MotorRobotArm::torqueOn()
                                    "failed to torque on motor for joint: " + joint_name);
         }
     }
-    emergency_stopped_.store(false);
+    emergency_stopped_ = false;
     return Result::success();
 }
 
@@ -271,194 +256,6 @@ 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) {
@@ -469,32 +266,10 @@ Result MotorRobotArm::quickStopMotors_()
                                    "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) {
@@ -506,9 +281,6 @@ 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);
@@ -522,7 +294,7 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
     BusyGuard busy_guard{busy_};
     std::lock_guard<std::mutex> lock(mutex_);
 
-    JointTrajectory samples;
+    std::vector<JointTrajectorySample> samples;
     if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) {
         return Result::failure(ArmErrorCode::CommandFailed, "[MotorRobotArm] moveJ planner failed: " + id_);
     }
@@ -551,9 +323,6 @@ 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");
@@ -568,8 +337,7 @@ 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].time_s > 0.0
+            const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t
-                                      ? 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)));
@@ -583,9 +351,6 @@ 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);
@@ -623,9 +388,6 @@ 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);
@@ -635,9 +397,6 @@ 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();
     }
@@ -679,13 +438,8 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                           << ", executable_path_m=" << trajectory.executable_path_length;
     }
 
-    if (executeMoveLTrajectory_(trajectory)) {
+    return executeMoveLTrajectory_(trajectory) ? Result::success()
-        return Result::success();
+                                               : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
-    }
-    if (const auto stopped = safetyStopResult_("moveL", true)) {
-        return *stopped;
-    }
-    return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
 }
 
 Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@@ -693,9 +447,6 @@ 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");
     }
@@ -735,9 +486,6 @@ 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);
@@ -1042,9 +790,6 @@ 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

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

cmvr-es/devices/arm/robot_arm.h

@@ -34,9 +34,6 @@ 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,13 +5,10 @@
 #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>
@@ -60,11 +57,6 @@ 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,
@@ -73,14 +65,6 @@ 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_;
@@ -94,7 +78,6 @@ 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};
@@ -109,13 +92,6 @@ 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,6 +106,10 @@ 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);
@@ -121,132 +125,37 @@ bool MujocoCamera::init()
 
 bool MujocoCamera::start()
 {
-    bool initialized = false;
+    if (!state_.is_initialized) {
-    {
-        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;
     }
-
-    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(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_);
+    destroyOffscreen_();
-    }
-    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);
@@ -294,10 +203,9 @@ void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics&
 
 bool MujocoCamera::startStreaming()
 {
-    if (!start()) {
+    if (!state_.is_initialized && !init()) {
         return false;
     }
-
     std::lock_guard<std::mutex> lock(mtx_);
     streaming_ = true;
     state_.is_streaming = true;
@@ -365,29 +273,14 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
 
 bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
 {
-    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_;
+    if (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) {
@@ -399,14 +292,11 @@ 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_) {
-            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;
-        }
 
         cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
         cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
@@ -420,31 +310,7 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
         return true;
     }
 
-    return fetchCached_(color, depth, intrinsics, consume_new_frame_only);
+    return renderOffscreen_(color, depth, intrinsics);
-}
-
-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_()
@@ -479,27 +345,12 @@ bool MujocoCamera::initOffscreen_()
         return false;
     }
 
-    mjModel* model = nullptr;
-    {
     std::lock_guard<std::mutex> world_lock(world->mutex());
-        model = world->model();
+    const mjModel* 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);
     scene_initialized_ = true;
     mjr_makeContext(model, &context_, mjFONTSCALE_150);
@@ -509,116 +360,9 @@ 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) {
@@ -632,10 +376,6 @@ 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;
@@ -648,8 +388,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         setError_("[MujocoCamera] camera is not initialized: " + id_);
         return false;
     }
-    if (window_ == nullptr || !context_initialized_ || !scene_initialized_) {
+    if (!initOffscreen_()) {
-        setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_);
         return false;
     }
 
@@ -663,27 +402,15 @@ 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::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
+        std::lock_guard<std::mutex> world_lock(world->mutex());
-        if (!world_lock.owns_lock()) {
+        mjModel* model = world->model();
-            // Never make the simulation wait for a camera frame. The next
+        mjData* data = world->data();
-            // scheduled capture will use a newer state if this one is busy.
+        if (model == nullptr || data == nullptr) {
-            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;
@@ -694,7 +421,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         viewport.width = width_;
         viewport.height = height_;
 
-        mjv_updateScene(model, render_data_, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
+        mjv_updateScene(model, 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_);
@@ -702,12 +429,13 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
     }
 
     cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
-    // mjr_readPixels returns RGB, while the rest of the camera API exposes
+    color = rgb_mat.clone();
-    // 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,6 +1,5 @@
 add_subdirectory(rh56dftp_dexhand)
 add_subdirectory(px_6ax_gen3)
-add_subdirectory(zero_sim_touch_dexhand)
 
 add_library(dexhand INTERFACE)
 
@@ -10,7 +9,6 @@ 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,7 +10,6 @@
 #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 {
 
@@ -33,10 +32,6 @@ 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

@@ -1,13 +0,0 @@
-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

@@ -1,57 +0,0 @@
-#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

@@ -1,96 +0,0 @@
-#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,7 +1,6 @@
 #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
 
 #include <algorithm>
-#include <array>
 #include <chrono>
 #include <cstdint>
 #include <iostream>
@@ -74,42 +73,17 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
         return;
     }
 
-    const std::array command_writes{
+    EXPECT_TRUE(runtime.writePdo<std::uint16_t>(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000));
-        EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
+    EXPECT_TRUE(runtime.writePdo<std::int8_t>(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0));
-            1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000),
-        EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
-            1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0),
-    };
-    auto invalid_writes = command_writes;
-    invalid_writes[1].bit_length = 16;
-
-    const auto generation_before = runtime.commandGeneration();
-    EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size()));
-    EXPECT_EQ(runtime.commandGeneration(), generation_before);
-    EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size()));
-    EXPECT_EQ(runtime.commandGeneration(), generation_before + 1);
 
     const int settle_ms = 1000;
     std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms));
-    EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1);
-    EXPECT_TRUE(runtime.isHealthy());
 
-    std::array feedback_reads{
+    std::uint16_t statusword = 0;
-        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+    EXPECT_TRUE(runtime.readPdo<std::uint16_t>(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword));
-            1, msgs::CIA402_STATUS_WORD_6041, 0x00),
-        EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
-            1, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
-    };
-    auto invalid_feedback_reads = feedback_reads;
-    invalid_feedback_reads[1].bit_length = 16;
-    EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(),
-                                        invalid_feedback_reads.size()));
-    ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size()));
 
-    const auto statusword =
+    std::int8_t mode_display = 0;
-        EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(feedback_reads[0]);
+    EXPECT_TRUE(runtime.readPdo<std::int8_t>(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display));
-    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,7 +21,6 @@ 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,7 +16,6 @@ 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,11 +90,8 @@ 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_, counts_per_joint_revolution,
+    if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) {
-                                        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,7 +9,6 @@
 
 #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 {
@@ -94,250 +93,96 @@ 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 (!write_home_offset(0)) {
+    if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
+                                              0x00, 0)) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node="
                         << static_cast<int>(node_id);
-        return rollback("clear_home_offset");
+        return false;
     }
+    std::this_thread::sleep_for(std::chrono::milliseconds{50});
 
     std::int32_t actual_position = 0;
-    if (!wait_for_position("clear_home_offset", 0,
+    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
-                           expected_cleared_position, actual_position)) {
+                                             0x00, actual_position)) {
-        return rollback("wait_for_cleared_position");
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
+                        << "after clearing home offset, node="
+                        << static_cast<int>(node_id);
+        return false;
     }
+
     if (actual_position == std::numeric_limits<std::int32_t>::min()) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home "
                         << "offset calibration, node=" << static_cast<int>(node_id)
                         << ", actual_position=" << actual_position;
-        return rollback("negate_actual_position");
+        return false;
     }
 
     const auto home_offset = static_cast<std::int32_t>(-actual_position);
-    if (!write_home_offset(home_offset)) {
+    if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
+                                              0x00, home_offset)) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node="
                         << static_cast<int>(node_id)
                         << ", home_offset=" << home_offset;
-        return rollback("write_home_offset");
+        return false;
     }
 
-    if (!wait_for_position("apply_home_offset", home_offset, 0,
+    if (!bus_runtime_->writeSdo<std::uint32_t>(
-                           zeroed_position)) {
+            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
-        return rollback("wait_for_zero_before_save");
+            0x01,
-    }
+            0x65766173)) {
-
-    if (!save_parameters()) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node="
                         << static_cast<int>(node_id);
-        return rollback("save_parameters");
+        return false;
     }
+    std::this_thread::sleep_for(std::chrono::milliseconds{50});
 
-    if (!wait_for_position("save_home_offset", home_offset, 0,
+    std::int32_t home_offset_readback = 0;
-                           zeroed_position)) {
+    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
-        return rollback("wait_for_zero_after_save");
+                                             0x00, home_offset_readback)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node="
+                        << static_cast<int>(node_id);
+        return false;
     }
-
+    if (home_offset_readback != home_offset) {
-    if (!restore_soft_limit()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node="
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position "
-                        << "limit after home offset calibration, node="
                         << static_cast<int>(node_id)
-                        << ", original_soft_limit_state=" << original_soft_limit_state;
+                        << ", expected=" << home_offset
-        return rollback("restore_soft_limit");
+                        << ", actual=" << home_offset_readback;
+        return false;
     }
 
-    CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node="
+    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
-                   << static_cast<int>(node_id)
+                                             0x00, zeroed_position)) {
-                   << ", original_home_offset=" << original_home_offset
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
-                   << ", cleared_position=" << 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
-                   << ", zeroed_position=" << zeroed_position
+                        << ", home_offset_readback=" << home_offset_readback
-                   << ", soft_limit_state=" << original_soft_limit_state;
+                        << ", actual_position_after=" << zeroed_position
+                        << ", tolerance_counts=" << 10000;
+        return false;
+    }
+
     return true;
 }
 

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

@@ -7,8 +7,6 @@
 #include <limits>
 #include <memory>
 #include <thread>
-#include <utility>
-#include <vector>
 
 #include <gtest/gtest.h>
 
@@ -21,23 +19,19 @@
 namespace cmvr::device {
 namespace {
 
-constexpr const char* kMotorManagerId = "right_arm_ethercat_motors";
+constexpr const char* kMotorManagerId = "ethercat_motors";
 constexpr const char* kMotorConfigFile =
-    "devices/motor/ethercat_motors_four_real_test.pb.txt";
+    "devices/motor/ethercat_motors_two_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 kRaisedCosineCoefficientRad = 0.2;
+constexpr double kFourMotorAmplitudeRad = 0.2;
-constexpr double kFourMotorPeriodS = 2.0;
+constexpr double kFourMotorPeriodS = 1.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:
@@ -47,61 +41,6 @@ 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};
@@ -169,6 +108,8 @@ 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);
@@ -199,16 +140,31 @@ 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 (const int motor_id : kFourMotorIds) {
+    // for (int motor_id = 1; motor_id <= 4; ++motor_id) {
-        auto motor = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
+    //     printMotorState(motor_id, motor_manager->getMotor(motor_id));
-        ASSERT_NE(motor, nullptr);
+    // }
-        printMotorState(motor_id, motor);
+
+    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));
     }
+    motor->quickStop();
+
 }
 
-TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory)
+TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
 {
     ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
     DeviceManagerDestroyGuard guard;
@@ -217,21 +173,16 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajec
         DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
     auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
     ASSERT_NE(motor_manager, nullptr);
-    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
 
-    std::vector<std::shared_ptr<AbstractMotor>> motors(kFourMotorIds.size());
+    std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
     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: ";
@@ -245,64 +196,48 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajec
         ASSERT_TRUE(motor->torqueOn());
     }
     for (const auto& motor : motors) {
-        ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
+        motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
     }
 
-    std::vector<double> center_q;
+    std::array<double, kFourMotorIds.size()> center_q{};
-    std::vector<double> actual_qd;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
-    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd));
+        center_q[i] = motors[i]->getQ();
+    }
 
     const double omega = 2.0 * kPi / kFourMotorPeriodS;
     std::cout << "command four motors in CSP, duration="
               << kFourMotorTrajectoryDuration.count()
               << " ms, command_period=" << kCyclicCommandPeriod.count()
-              << " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad
+              << " ms, amplitude=" << kFourMotorAmplitudeRad
-              << " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad
               << " rad, period=" << kFourMotorPeriodS
               << " s" << std::endl;
 
     const auto start_time = std::chrono::steady_clock::now();
-    const auto end_time = start_time + kFourMotorTrajectoryDuration;
+    const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod;
-    auto next_command_time = start_time;
-    auto next_stats_time = start_time;
-    std::uint64_t missed_command_deadlines = 0;
     std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
-    std::vector<double> target_q(motors.size(), 0.0);
-    std::vector<double> target_qd(motors.size(), 0.0);
-    std::vector<double> actual_q;
-    std::array<double, kFourMotorIds.size()> minimum_actual_q{};
-    std::array<double, kFourMotorIds.size()> maximum_actual_q{};
-    std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin());
-    std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin());
     double max_error_spread_rad = 0.0;
     double sum_error_spread_sq = 0.0;
     std::int64_t error_spread_sample_count = 0;
-    while (true) {
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
-        std::this_thread::sleep_until(next_command_time);
+        const auto elapsed = tick * kCyclicCommandPeriod;
-        const auto now = std::chrono::steady_clock::now();
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
-        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] =
+            target_q[i] = center_q[i] + kFourMotorAmplitudeRad * (1.0 - std::cos(theta));
-                center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta));
+            target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta);
-            target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta);
+            ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
         }
-        ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
 
-        if (now >= next_stats_time) {
+        if (elapsed.count() % kStatsSamplePeriod.count() == 0) {
-            ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
             double min_error = std::numeric_limits<double>::max();
             double max_error = std::numeric_limits<double>::lowest();
             for (std::size_t i = 0; i < motors.size(); ++i) {
                 const double theta = omega * t_s + kFourMotorPhaseRad[i];
-                const double error = actual_q[i] - target_q[i];
+                const double error = motors[i]->getQ() - 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);
             }
@@ -310,24 +245,31 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajec
             max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
             sum_error_spread_sq += error_spread * error_spread;
             ++error_spread_sample_count;
-            next_stats_time = now + kStatsSamplePeriod;
         }
 
-        next_command_time += kCyclicCommandPeriod;
+        // if (elapsed.count() % kPrintPeriod.count() == 0) {
-        const auto command_complete_time = std::chrono::steady_clock::now();
+        //     std::cout << "t=" << elapsed.count() << " ms" << std::endl;
-        if (next_command_time <= command_complete_time) {
+        //     for (std::size_t i = 0; i < motors.size(); ++i) {
-            const auto skipped_periods =
+        //         std::cout << "  motor_id=" << kFourMotorIds[i]
-                (command_complete_time - next_command_time) / kCyclicCommandPeriod + 1;
+        //                   << ", target_q=" << target_q[i]
-            missed_command_deadlines += static_cast<std::uint64_t>(skipped_periods);
+        //                   << " rad, target_qd=" << target_qd[i]
-            next_command_time += skipped_periods * kCyclicCommandPeriod;
+        //                   << " 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);
     }
 
-    std::copy(center_q.begin(), center_q.end(), target_q.begin());
+    const auto hold_start_time = std::chrono::steady_clock::now();
-    std::fill(target_qd.begin(), target_qd.end(), 0.0);
+    const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod;
-    ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
+    for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) {
-    std::this_thread::sleep_for(kHoldAfterTrajectoryDuration);
+        for (std::size_t i = 0; i < motors.size(); ++i) {
-    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
+            ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0));
+        }
+        std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
 
     std::cout << "after four motor CSP trajectory" << std::endl;
     for (std::size_t i = 0; i < motors.size(); ++i) {
@@ -342,7 +284,6 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajec
     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;
@@ -362,19 +303,7 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajec
                   << " 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,6 +23,7 @@ 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};
@@ -45,20 +46,14 @@ 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(false);
+    dc->set_enable(true);
     dc->set_reference_motor_id(kMotorId);
     dc->set_sync0_cycle_us(1000);
     dc->set_sync0_shift_us(0);
@@ -118,10 +113,10 @@ config::MotorConfigItem createMotorConfig()
     config::MotorConfigItem config;
     config.set_id(kMotorId);
     config.set_joint_name("ethercat_test_joint");
-    config.set_limit_q_lb(-36.14);
+    config.set_limit_q_lb(-6.14);
-    config.set_limit_q_ub(36.14);
+    config.set_limit_q_ub(6.14);
     config.set_limit_qd(10.0);
-    config.set_limit_qdd(100.0);
+    config.set_limit_qdd(10.0);
     config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
     config.set_gear_ratio(kDefaultGearRatio);
     return config;
@@ -238,9 +233,6 @@ 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)
@@ -253,7 +245,8 @@ TEST(EyouMotorRealTest, CommandProfilePosition)
     ASSERT_NE(motor, nullptr);
 
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
+    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
     ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
     sampleMotorState(*motor, kFeedbackSampleDuration);
@@ -268,10 +261,9 @@ TEST(EyouMotorRealTest, CommandProfileVelocity)
     auto motor = createMotor(runtime);
     ASSERT_NE(motor, nullptr);
 
-
-
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
+    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
     std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
     ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
@@ -291,20 +283,21 @@ TEST(EyouMotorRealTest, CommandCyclicPosition)
     auto motor = createMotor(runtime);
     ASSERT_NE(motor, nullptr);
 
-    ASSERT_TRUE(motor->torqueOff());
-    ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
-    const std::chrono::milliseconds trajectory_duration{12000};
+    const std::chrono::milliseconds trajectory_duration{15000};
     const double period_s = 6.0;
-    const double excursion_rad = 3.0;
+    const double amplitude_rad = 3;
+    const double phase_rad = 0.0;
     const double center_q = motor->getQ();
     const double omega = 2.0 * kPi / period_s;
 
-    std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q
+    std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q
               << " rad, period=" << period_s
-              << " s, excursion=" << excursion_rad
+              << " s, amplitude=" << amplitude_rad
+              << " rad, phase=" << phase_rad
               << " rad, command_period=" << kCyclicCommandPeriod.count()
               << " ms" << std::endl;
 
@@ -313,11 +306,9 @@ 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;
+        const double theta = omega * t_s + phase_rad;
-        const double target_q =
+        const double target_q = center_q + amplitude_rad * std::sin(theta);
-            center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta));
+        const double target_qd = amplitude_rad * omega * 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) {
@@ -340,22 +331,19 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
     auto motor = createMotor(runtime);
     ASSERT_NE(motor, nullptr);
 
-    ASSERT_TRUE(motor->torqueOff());
-    ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
-    const std::chrono::milliseconds trajectory_duration{12000};
+    const std::chrono::milliseconds trajectory_duration{15000};
-    const double period_s = 5.0;
+    const double period_s = 6.0;
-    const double excursion_rad = 5.0;
+    const double velocity_amplitude_rad_s = 5.0;
     const double phase_rad = 0.0;
     const double omega = 2.0 * kPi / period_s;
-    const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega;
 
     std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
               << " s, velocity_amplitude=" << velocity_amplitude_rad_s
-              << " rad/s, excursion=" << excursion_rad
+              << " rad/s, phase=" << phase_rad
-              << " rad, phase=" << phase_rad
               << " rad, command_period=" << kCyclicCommandPeriod.count()
               << " ms" << std::endl;
 
@@ -373,7 +361,6 @@ 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);
     }
@@ -381,7 +368,6 @@ 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)
@@ -396,7 +382,8 @@ TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
     const std::chrono::milliseconds run_duration{2000};
     const double period_s = 6.0;
@@ -451,7 +438,8 @@ TEST(EyouMotorRealTest, QuickStopInProfilePosition)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
+    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
     const std::chrono::milliseconds quick_stop_time{1000};
     const double start_q = motor->getQ();
@@ -490,7 +478,8 @@ TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
+    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
     const std::chrono::milliseconds quick_stop_time{2000};
     const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
@@ -526,7 +515,8 @@ TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
     ASSERT_TRUE(motor->torqueOff());
     ASSERT_TRUE(motor->calibrateZeroQ());
     ASSERT_TRUE(motor->torqueOn());
-    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
 
     const std::chrono::milliseconds run_duration{2000};
     const double start_q = motor->getQ();

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

@@ -5,6 +5,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <unordered_set>
 #include <unordered_map>
 #include <vector>
 
@@ -31,9 +32,7 @@ class AbstractMotorBusRuntime;
 class MotorManager final : public AbstractDevice,
                            public std::enable_shared_from_this<MotorManager> {
 public:
-    MotorManager(std::string id,
+    MotorManager(std::string id, const config::MotorConfig& cfg);
-                 const config::MotorConfig& cfg,
-                 std::string selected_group_id);
     ~MotorManager() override;
 
     DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; }
@@ -57,7 +56,16 @@ 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_(
@@ -81,7 +89,6 @@ 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_;
@@ -91,6 +98,7 @@ 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,14 +28,17 @@ 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,
+MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg)
-                           const config::MotorConfig& cfg,
+    : cfg_(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;
@@ -49,14 +52,6 @@ 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()));
@@ -67,27 +62,25 @@ 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 != selected_group_id_) {
+        if (group_name.empty()) {
+            CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_;
+            all_ok = false;
             continue;
         }
-        ++selected_group_count;
         if (!motor_group_cfg.has_motors()) {
             CMVR_LOG(ERROR) << "[MotorManager] motor group missing motors: " << group_name;
             all_ok = false;
             continue;
         }
 
-        if (motor_group_cfg.motors().motors_size() == 0) {
+        std::vector<config::MotorConfigItem> selected_motor_cfgs;
-            CMVR_LOG(ERROR) << "[MotorManager] motor group has no motors: " << group_name;
+        const bool selected_active_group = selectActiveMotors_(
-            all_ok = false;
+            group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs);
+        if (!selected_active_group) {
             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;
@@ -144,12 +137,6 @@ 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) {
@@ -312,6 +299,68 @@ 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,6 +8,7 @@
 #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"
@@ -48,6 +49,7 @@ 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

@@ -228,11 +228,11 @@ DeviceFactory::DeviceFactory()
     registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
 	                    [](const auto& entry) {
 	                        if (entry.id().empty()) {
-                            CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id is required";
+	                            CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required";
 	                            return DeviceRecord{};
 	                        }
 	                        if (entry.config_file().empty()) {
-                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for motor group ID: " << entry.id();
+	                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id();
 	                            return DeviceRecord{};
 	                        }
 	                        config::MotorRootConfig root_cfg;
@@ -240,27 +240,8 @@ DeviceFactory::DeviceFactory()
 	                            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>(
+                        auto device = std::make_shared<MotorManager>(entry.id(), root_cfg.motor());
-                            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,12 +18,17 @@
 #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 << " ----------------";
@@ -58,6 +63,43 @@ 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);
@@ -83,6 +125,7 @@ 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_();
@@ -107,6 +150,7 @@ DeviceManager& DeviceManager::getInstance() {
 void DeviceManager::destroyInstance() {
     std::lock_guard lock(init_mutex_);
     instance_.reset();
+    MotorManager::clearActiveJoints();
 }
 
 void DeviceManager::start(){
@@ -239,6 +283,153 @@ 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,8 +109,6 @@ 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,11 +76,6 @@ 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 {
@@ -212,33 +207,7 @@ namespace cmvr {
         display_rect.left = left;
         display_rect.bottom = bottom;
 
-        // Copy only the visualization state while synchronized with Simulate.
+        const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
-        // 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_) {
@@ -253,7 +222,7 @@ namespace cmvr {
         pip_cam_.fixedcamid = pip_camera_id_;
         pip_cam_.trackbodyid = -1;
 
-        mjv_updateScene(render_model, pip_render_data_, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
+        mjv_updateScene(render_model, 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
+#add_executable(touch_screen_task_test
-        touch_screen_task/src/touch_screen_task_test.cpp
+#        touch_screen_task/src/touch_screen_task_test.cpp
-)
+#)
-
+#
-target_link_libraries(touch_screen_task_test PRIVATE
+#target_link_libraries(touch_screen_task_test PRIVATE
-        cmvr_es::task
+#        cmvr_es::task
-        cmvr_es::device::arm
+#        cmvr_es::device::arm
-        cmvr_es::device::motor_manager
+#        cmvr_es::device::motor_manager
-        cmvr_es::device::mujoco_motor_driver
+#        cmvr_es::device::mujoco_motor_driver
-        cmvr_es::device::mujoco_camera
+#        cmvr_es::device::mujoco_camera
-        cmvr_es::mujoco_viewer
+#        cmvr_es::mujoco_viewer
-        cmvr_es::proto
+#        cmvr_es::proto
-        cmvr_es::device_manager
+#        cmvr_es::device_manager
-        cmvr_es::service
+#        cmvr_es::service
-        gtest
+#        gtest
-        gtest_main
+#        gtest_main
-        pthread
+#        pthread
-        glog
+#        glog
-)
+#)

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

@@ -1,14 +1,9 @@
 #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"
@@ -25,22 +20,10 @@ 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 {
@@ -63,15 +46,11 @@ 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_;
@@ -80,16 +59,8 @@ 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,7 +1,5 @@
 #include "task/self_collision_task/include/self_collision_task.h"
 
-#include <algorithm>
-#include <chrono>
 #include <cmath>
 #include <iomanip>
 #include <sstream>
@@ -55,31 +53,6 @@ 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");
@@ -148,13 +121,6 @@ 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;
     }
@@ -178,12 +144,6 @@ 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;
@@ -201,49 +161,27 @@ bool SelfCollisionTask::step(const double dt)
         arm = arm_;
     }
 
-    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));
+        std::lock_guard lock(mutex_);
+        last_error_ = std::move(error);
+        state_ = TaskState::FAILED;
+        return false;
     }
 
+    const auto now = DistanceSamplingPolicy::Clock::now();
     if (!sampling_.shouldCheck(snapshot, now)) {
         return true;
     }
 
     SelfCollisionResult result = checker_.check(snapshot);
     if (!result.valid) {
-        return fail_monitoring(
+        std::lock_guard lock(mutex_);
-            result.error.empty() ? "Self-collision distance check failed" : result.error);
+        last_error_ = result.error.empty() ? "Self-collision distance check failed" : result.error;
+        state_ = TaskState::FAILED;
+        return false;
     }
     sampling_.markChecked(snapshot, now);
 
@@ -255,7 +193,6 @@ bool SelfCollisionTask::step(const double dt)
     }
 
     bool trigger_stop = false;
-    bool abort_recovery = false;
     CollisionSafetyLevel previous_level = CollisionSafetyLevel::UNKNOWN;
     {
         std::lock_guard lock(mutex_);
@@ -265,63 +202,9 @@ 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));
         }
     }
 
@@ -337,9 +220,7 @@ bool SelfCollisionTask::step(const double dt)
         }
     }
 
-    if (abort_recovery) {
+    if (trigger_stop) {
-        (void)arm->protectiveStop();
-    } else if (trigger_stop) {
         const auto stop_result = arm->protectiveStop();
         if (!stop_result.ok()) {
             std::lock_guard lock(mutex_);
@@ -351,166 +232,12 @@ bool SelfCollisionTask::step(const double dt)
     return true;
 }
 
-void SelfCollisionTask::recordJointSample_(
-    const device::JointGroupState& joint_state,
-    const DistanceSamplingPolicy::Clock::time_point now)
-{
-    std::lock_guard lock(mutex_);
-    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();
-    }
 }
 
 TaskState SelfCollisionTask::state() const
@@ -547,20 +274,13 @@ 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();
 }
 
@@ -581,17 +301,4 @@ 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,7 +385,6 @@ 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,6 +4,7 @@
 #include <array>
 #include <chrono>
 #include <cmath>
+#include <condition_variable>
 #include <cstdint>
 #include <filesystem>
 #include <iostream>
@@ -27,10 +28,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 {
 
@@ -67,6 +68,178 @@ 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};
@@ -286,75 +459,36 @@ 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("right_arm_mujoco_motors");
+    motor_entry->set_id("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("mujoco_right_arm");
+    arm_entry->set_id("right_arm_mujoco");
     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>("right_arm_mujoco_motors");
+    auto motor_system = device_manager.getDevice<cmvr::device::MotorManager>("mujoco_motors");
     if (!motor_system) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: right_arm_mujoco_motors";
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: mujoco_motors";
         return;
     }
-    auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
+    auto bridge = cmvr::device::MotorManager::mujocoBridgeFor("mujoco_motors");
-    if (!world || !world->isLoaded() || !world->isRunning()) {
+    if (!bridge) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco world is not ready: mujoco_world";
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors";
         return;
     }
-    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
+    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("right_arm_mujoco");
     if (!arm) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: mujoco_right_arm";
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: right_arm_mujoco";
         return;
     }
-    // The task config still uses the historical ID; keep it as a test-only alias.
-    device_manager.registerDevice("right_arm_mujoco", arm);
 
-    cmvr::MuJocoViewer viewer(world);
+    TouchMujocoViewer viewer(
-    viewer.setupCamera(2.5, -160.0, -25.0);
+        (project_root / "model/xiaoyan_description/dual_arm.xml").string(), bridge);
-    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};
@@ -372,8 +506,17 @@ 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();
@@ -390,7 +533,6 @@ 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;
@@ -406,31 +548,10 @@ 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();
-                task_manager.stopRunTask();
+                return;
-                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);
@@ -483,7 +604,6 @@ 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="42:e6:6d:44:c1:0f"
+MASTER0_DEVICE="a0:ad:9f:c4:c2:2c"
 DEVICE_MODULES="generic"
 UPDOWN_INTERFACES="eno1"

docs/ethercat_motor_tutorial.md

@@ -75,14 +75,6 @@ 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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,14 +0,0 @@
-{
-    "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

@@ -1,14 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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

@@ -1,13 +0,0 @@
-{
-    "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"
-}