feat(gen2): add MuJoCo collision recovery support

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

cmvr-es/devices/arm/robot_arm.h

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

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

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

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

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

model/gen2/assets/10100.part

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

model/gen2/assets/10100__2.part

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

model/gen2/assets/10100__3.part

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

model/gen2/assets/1020001.part

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

model/gen2/assets/arm_link_1.part

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

model/gen2/assets/arm_link_2.part

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

model/gen2/assets/arm_link_3.part

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

model/gen2/assets/arm_link_4.part

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

model/gen2/assets/arm_link_5.part

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

model/gen2/assets/arm_link_6.part

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

model/gen2/assets/arm_link_7.part

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

model/gen2/assets/body_link.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "0384359606fafa70e61c4a9c",
+    "fullConfiguration": "default",
+    "id": "MdJXCUi9nHtYjnfh4",
+    "isStandardContent": false,
+    "name": "body_link <1>",
+    "partId": "RgQD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/ethercat挂杆_上.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "b26d4d3d1e4a010d0d624309",
+    "elementId": "2b20557d3415aa6a7868cfec",
+    "fullConfiguration": "default",
+    "id": "M6a8atbQgryvAh+2G",
+    "isStandardContent": false,
+    "name": "ethercat\u6302\u6746-\u4e0a <1>",
+    "partId": "RMDD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/ethercat挂杆_下.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "b26d4d3d1e4a010d0d624309",
+    "elementId": "2b20557d3415aa6a7868cfec",
+    "fullConfiguration": "default",
+    "id": "M6IPrfKh9NKHG6KNc",
+    "isStandardContent": false,
+    "name": "ethercat\u6302\u6746-\u4e0b <1>",
+    "partId": "RMDH",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "JTQwc2NwPU9ITmFJVkFLb3VXVUNGQnNjWjhrOE1zRG1nMFNRSU9JOUxqWXlZc1dGZEklM0Q7QXZlcmFnZURpYW1ldGVyPTAuMDAzNjAwMDAwMDAwMDAwMDAwMyttZXRlcjtCYXNpY0RpYW1ldGVyPTAuMDAzK21ldGVyO0hlYWREaWFtZXRlcj0wLjAwNTUrbWV0ZXI7SGVhZEZpbGxldD0zLjBFLTQrbWV0ZXI7SGVhZEhlaWdodD0wLjAwMyttZXRlcjtIZXhEZXB0aD0wLjAwMTMwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMjUrbWV0ZXI7TGVuZ3RoPTAuMDI1K21ldGVyO1BpdGNoPTUuMEUtNCttZXRlcjtUaHJlYWRMZW5ndGg9MC4wMTgwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7VHJhbnNpdGlvbkxlbmd0aD01LjFFLTQrbWV0ZXI7VHJpYW5nbGVIZWlnaHQ9NC4zMzAxMjdFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTEuMEUtNCttZXRlcg",
+    "documentId": "da5fe16b33cc63bf8b9e7e78",
+    "documentMicroversion": "e36ce5d010b02c39f1a32b51",
+    "documentVersion": "dc1a15dae669d740ea2d555e",
+    "elementId": "5b44a050e0b24df3e47c76dc",
+    "fullConfiguration": "JTQwc2NwPU9ITmFJVkFLb3VXVUNGQnNjWjhrOE1zRG1nMFNRSU9JOUxqWXlZc1dGZEklM0Q7QXZlcmFnZURpYW1ldGVyPTAuMDAzNjAwMDAwMDAwMDAwMDAwMyttZXRlcjtCYXNpY0RpYW1ldGVyPTAuMDAzK21ldGVyO0hlYWREaWFtZXRlcj0wLjAwNTUrbWV0ZXI7SGVhZEZpbGxldD0zLjBFLTQrbWV0ZXI7SGVhZEhlaWdodD0wLjAwMyttZXRlcjtIZXhEZXB0aD0wLjAwMTMwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMjUrbWV0ZXI7TGVuZ3RoPTAuMDI1K21ldGVyO1BpdGNoPTUuMEUtNCttZXRlcjtUaHJlYWRMZW5ndGg9MC4wMTgwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7VHJhbnNpdGlvbkxlbmd0aD01LjFFLTQrbWV0ZXI7VHJpYW5nbGVIZWlnaHQ9NC4zMzAxMjdFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTEuMEUtNCttZXRlcg",
+    "id": "Mz77l4SnYYxPYDxga",
+    "isStandardContent": true,
+    "name": "Hex socket head cap screw M3x0.50 x 25 x 18 <2>",
+    "partId": "JHD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "JTQwc2NwPXZaeDNQUzV0NWU2YlI2R3BJR25ZZ1ZoczdzREJNOWpvdmNXYiUyQjVIU1JnOCUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDM2MDAwMDAwMDAwMDAwMDAzK21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDMrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA1NSttZXRlcjtIZWFkRmlsbGV0PTMuMEUtNCttZXRlcjtIZWFkSGVpZ2h0PTAuMDAzK21ldGVyO0hleERlcHRoPTAuMDAxMzAwMDAwMDAwMDAwMDAwMittZXRlcjtIZXhTaXplPTAuMDAyNSttZXRlcjtMZW5ndGg9MC4wNSttZXRlcjtQaXRjaD01LjBFLTQrbWV0ZXI7VGhyZWFkTGVuZ3RoPTAuMDE4MDAwMDAwMDAwMDAwMDAyK21ldGVyO1RyYW5zaXRpb25MZW5ndGg9NS4xRS00K21ldGVyO1RyaWFuZ2xlSGVpZ2h0PTQuMzMwMTI3RS00K21ldGVyO1VuZGVySGVhZEZpbGxldD0xLjBFLTQrbWV0ZXI",
+    "documentId": "da5fe16b33cc63bf8b9e7e78",
+    "documentMicroversion": "e36ce5d010b02c39f1a32b51",
+    "documentVersion": "dc1a15dae669d740ea2d555e",
+    "elementId": "5b44a050e0b24df3e47c76dc",
+    "fullConfiguration": "JTQwc2NwPXZaeDNQUzV0NWU2YlI2R3BJR25ZZ1ZoczdzREJNOWpvdmNXYiUyQjVIU1JnOCUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDM2MDAwMDAwMDAwMDAwMDAzK21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDMrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA1NSttZXRlcjtIZWFkRmlsbGV0PTMuMEUtNCttZXRlcjtIZWFkSGVpZ2h0PTAuMDAzK21ldGVyO0hleERlcHRoPTAuMDAxMzAwMDAwMDAwMDAwMDAwMittZXRlcjtIZXhTaXplPTAuMDAyNSttZXRlcjtMZW5ndGg9MC4wNSttZXRlcjtQaXRjaD01LjBFLTQrbWV0ZXI7VGhyZWFkTGVuZ3RoPTAuMDE4MDAwMDAwMDAwMDAwMDAyK21ldGVyO1RyYW5zaXRpb25MZW5ndGg9NS4xRS00K21ldGVyO1RyaWFuZ2xlSGVpZ2h0PTQuMzMwMTI3RS00K21ldGVyO1VuZGVySGVhZEZpbGxldD0xLjBFLTQrbWV0ZXI",
+    "id": "MzS3hV4uHEP51PCyW",
+    "isStandardContent": true,
+    "name": "Hex socket head cap screw M3x0.50 x 50 x 18 <5>",
+    "partId": "JHD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "JTQwc2NwPWxOSElNUXhVSTdPY3d2VXJqaDNJcVhHZHV3elVUZWYlMkY1N3cydklXNTZ1ZyUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDQ3K21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDQrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA3K21ldGVyO0hlYWRGaWxsZXQ9NC4wRS00K21ldGVyO0hlYWRIZWlnaHQ9MC4wMDQrbWV0ZXI7SGV4RGVwdGg9MC4wMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMyttZXRlcjtMZW5ndGg9MC4wMDgrbWV0ZXI7UGl0Y2g9Ny4wRS00K21ldGVyO1RocmVhZExlbmd0aD0wLjAwOCttZXRlcjtUcmFuc2l0aW9uTGVuZ3RoPTYuMEUtNCttZXRlcjtUcmlhbmdsZUhlaWdodD02LjA2MjE3NzhFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTIuMEUtNCttZXRlcg",
+    "documentId": "da5fe16b33cc63bf8b9e7e78",
+    "documentMicroversion": "e36ce5d010b02c39f1a32b51",
+    "documentVersion": "dc1a15dae669d740ea2d555e",
+    "elementId": "5b44a050e0b24df3e47c76dc",
+    "fullConfiguration": "JTQwc2NwPWxOSElNUXhVSTdPY3d2VXJqaDNJcVhHZHV3elVUZWYlMkY1N3cydklXNTZ1ZyUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDQ3K21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDQrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA3K21ldGVyO0hlYWRGaWxsZXQ9NC4wRS00K21ldGVyO0hlYWRIZWlnaHQ9MC4wMDQrbWV0ZXI7SGV4RGVwdGg9MC4wMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMyttZXRlcjtMZW5ndGg9MC4wMDgrbWV0ZXI7UGl0Y2g9Ny4wRS00K21ldGVyO1RocmVhZExlbmd0aD0wLjAwOCttZXRlcjtUcmFuc2l0aW9uTGVuZ3RoPTYuMEUtNCttZXRlcjtUcmlhbmdsZUhlaWdodD02LjA2MjE3NzhFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTIuMEUtNCttZXRlcg",
+    "id": "Mwr1cbw7PCKF93OWT",
+    "isStandardContent": true,
+    "name": "Hex socket head cap screw M4x0.70 x 8 <7>",
+    "partId": "JHD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j2关节.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "8b7e66f798cbb082509293d8",
+    "documentMicroversion": "887ff4907089063547dfb683",
+    "documentVersion": "05b6b05241fd0e72cdcb2a58",
+    "elementId": "a4d6cca76f08dc255e08b999",
+    "fullConfiguration": "default",
+    "id": "MPjW04ncnuOxOzkZ0",
+    "isStandardContent": false,
+    "name": "J2\u5173\u8282 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j2关节盖板.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "639734872365a5c6ab1e6dc0",
+    "documentMicroversion": "87a57a1cf4702ec8bbc72b83",
+    "documentVersion": "bc5b603bcba394f302ef13b6",
+    "elementId": "2a770d046c759ce59d9fb447",
+    "fullConfiguration": "default",
+    "id": "M3lE/jcPA4MBiXRl+",
+    "isStandardContent": false,
+    "name": "J2\u5173\u8282\u76d6\u677f <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j3关节.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "15fc539038cd4e87718802cf",
+    "documentMicroversion": "dd0168517fb915b74ab9f650",
+    "documentVersion": "c78f7c31e667644647771463",
+    "elementId": "cce50e82bbd0f737c1086291",
+    "fullConfiguration": "default",
+    "id": "ML+TSApNzCC0D52rC",
+    "isStandardContent": false,
+    "name": "J3\u5173\u8282 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j4关节.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "7b3f5b2e877e0152778a9ca2",
+    "documentMicroversion": "0f0440eac84e1ad6c9e4d950",
+    "documentVersion": "6d55eb8af3364ff18181e064",
+    "elementId": "deecc12ddde91edb0deb20c9",
+    "fullConfiguration": "default",
+    "id": "MGEe7vYxsi98IEyH5",
+    "isStandardContent": false,
+    "name": "J4\u5173\u8282 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j4轴承支撑.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "38be5a8344ea08045e614079",
+    "fullConfiguration": "default",
+    "id": "MMhyQcMwjwq7kE227",
+    "isStandardContent": false,
+    "name": "J4\u8f74\u627f\u652f\u6491 <1>",
+    "partId": "RWBD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j4轴承支撑__2.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "a51a941965828d24d7f9ab81",
+    "fullConfiguration": "default",
+    "id": "MsSXN2rPmtY0dur6R",
+    "isStandardContent": false,
+    "name": "J4\u8f74\u627f\u652f\u6491 <2>",
+    "partId": "RMCD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j4轴承盖板.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "38be5a8344ea08045e614079",
+    "fullConfiguration": "default",
+    "id": "MKrYD2pl4tMWf+WQQ",
+    "isStandardContent": false,
+    "name": "J4\u8f74\u627f\u76d6\u677f <1>",
+    "partId": "RjBD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j4轴承盖板__2.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "a51a941965828d24d7f9ab81",
+    "fullConfiguration": "default",
+    "id": "MqrQMlBzm6clwBYhN",
+    "isStandardContent": false,
+    "name": "J4\u8f74\u627f\u76d6\u677f <2>",
+    "partId": "RLCD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j5主动法兰.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "95a669e868ab89618b756342",
+    "fullConfiguration": "default",
+    "id": "MkPc/e6vvvzfReU1g",
+    "isStandardContent": false,
+    "name": "J5\u4e3b\u52a8\u6cd5\u5170 <2>",
+    "partId": "JHD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j5从动轴心盖板.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "3ed0f82d52069da883c2af7a",
+    "fullConfiguration": "default",
+    "id": "M++0c8F/OA1VcSYPL",
+    "isStandardContent": false,
+    "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <2>",
+    "partId": "R1BD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j5从动轴心盖板__2.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "3ed0f82d52069da883c2af7a",
+    "fullConfiguration": "default",
+    "id": "M+lDhmwR1TmaWXh/y",
+    "isStandardContent": false,
+    "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <1>",
+    "partId": "R0BD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j5从动轴心盖板__3.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "08e441b1ea6915f4c8e5b947",
+    "fullConfiguration": "default",
+    "id": "Mb3FWJ4KHbCKAG26B",
+    "isStandardContent": false,
+    "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <3>",
+    "partId": "RDBD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j5从动轴心盖板__4.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "69b5318f86ab1549d2294bb3",
+    "elementId": "08e441b1ea6915f4c8e5b947",
+    "fullConfiguration": "default",
+    "id": "MjEKCYXkYWHg0gqf+",
+    "isStandardContent": false,
+    "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <4>",
+    "partId": "RSBD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j5关节.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "6738eff574c7bac74278c035",
+    "documentMicroversion": "7b5e69869d9b02be1ce0581d",
+    "documentVersion": "a0db86e78c7d2dc110a9791b",
+    "elementId": "60347bdca2a24e7714baa4d6",
+    "fullConfiguration": "default",
+    "id": "MjQlv+qNUV6RZuD6P",
+    "isStandardContent": false,
+    "name": "J5\u5173\u8282 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j6轴承支撑.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "ec3810b640410692b6d75e6d",
+    "documentMicroversion": "2677b5c61e0ce625e61f2de1",
+    "documentVersion": "260d5320b9d212f7376901b9",
+    "elementId": "9b8fab3d8fa54635da3b9281",
+    "fullConfiguration": "default",
+    "id": "M1oxqvy7AYCLJRufY",
+    "isStandardContent": false,
+    "name": "J6\u8f74\u627f\u652f\u6491 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j6轴承支撑1.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "aab3081dc8a1b17396b04ac8",
+    "documentMicroversion": "989f9da17530022533d4b1b5",
+    "documentVersion": "431a46f24ed967bcf94385d0",
+    "elementId": "55a51320ef47236b54856bc1",
+    "fullConfiguration": "default",
+    "id": "MG/AppKFXBSV39O6i",
+    "isStandardContent": false,
+    "name": "J6\u8f74\u627f\u652f\u64911 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/j7关节a.part

@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "41e7681508301de01c2f4658",
+    "documentMicroversion": "e0b55cb820aadaa0f053d95b",
+    "documentVersion": "9b9bafefdd3dbfa2715fbe98",
+    "elementId": "28e76f0bd60c1f60bb95ed8c",
+    "fullConfiguration": "default",
+    "id": "MjmlvjYKU6BQqrtfC",
+    "isStandardContent": false,
+    "name": "J7\u5173\u8282A <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/jetson安装杆_右.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "b26d4d3d1e4a010d0d624309",
+    "elementId": "2b20557d3415aa6a7868cfec",
+    "fullConfiguration": "default",
+    "id": "MK0IY8wljRAjD4RsR",
+    "isStandardContent": false,
+    "name": "jetson\u5b89\u88c5\u6746-\u53f3 <1>",
+    "partId": "RwLD",
+    "suppressed": false,
+    "type": "Part"
+}

model/gen2/assets/jetson安装杆_左.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "b26d4d3d1e4a010d0d624309",
+    "elementId": "2b20557d3415aa6a7868cfec",
+    "fullConfiguration": "default",
+    "id": "Mmwlrnd+kZGc2EiJK",
+    "isStandardContent": false,
+    "name": "jetson\u5b89\u88c5\u6746-\u5de6 <1>",
+    "partId": "R3LD",
+    "suppressed": false,
+    "type": "Part"
+}