feat: add RM75 O6 force-spike guard for 2026-07-31

RM/CMakeLists.txt

@@ -111,6 +111,16 @@ target_include_directories(rm_o6_bus_scanner
 )
 target_compile_features(rm_o6_bus_scanner PUBLIC cxx_std_17)
 
+add_library(rm_force_safety_core
+    src/modules/force_safety/force_spike_detector.cpp
+    src/core/force_spike_hand_guard.cpp
+)
+target_include_directories(rm_force_safety_core
+    PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+target_compile_features(rm_force_safety_core PUBLIC cxx_std_17)
+
 add_library(rm_arm_o6_task_core
     src/core/arm_o6_trajectory_task.cpp
 )
@@ -122,6 +132,7 @@ target_compile_features(rm_arm_o6_task_core PUBLIC cxx_std_17)
 target_link_libraries(rm_arm_o6_task_core
     PUBLIC
         rm_drag_teach_core
+        rm_force_safety_core
         rm_linker_hand_adapter
         Threads::Threads
 )
@@ -560,6 +571,26 @@ if(RM_BUILD_TESTING)
     add_test(NAME rm_trajectory_playback_report_store_tests
         COMMAND rm_trajectory_playback_report_store_tests)
 
+    add_executable(rm_force_spike_detector_tests
+        tests/test_force_spike_detector.cpp
+    )
+    target_link_libraries(rm_force_spike_detector_tests
+        PRIVATE
+            rm_force_safety_core
+    )
+    add_test(NAME rm_force_spike_detector_tests
+        COMMAND rm_force_spike_detector_tests)
+
+    add_executable(rm_force_spike_hand_guard_tests
+        tests/test_force_spike_hand_guard.cpp
+    )
+    target_link_libraries(rm_force_spike_hand_guard_tests
+        PRIVATE
+            rm_force_safety_core
+    )
+    add_test(NAME rm_force_spike_hand_guard_tests
+        COMMAND rm_force_spike_hand_guard_tests)
+
     add_executable(rm_arm_o6_trajectory_task_tests
         tests/test_arm_o6_trajectory_task.cpp
     )
@@ -570,6 +601,20 @@ if(RM_BUILD_TESTING)
     add_test(NAME rm_arm_o6_trajectory_task_tests
         COMMAND rm_arm_o6_trajectory_task_tests)
 
+    add_executable(rm_trajectory_replay_force_spike_thumb_index_tests
+        tests/test_trajectory_replay_force_spike_thumb_index.cpp
+    )
+    target_link_libraries(rm_trajectory_replay_force_spike_thumb_index_tests
+        PRIVATE
+            rm_arm_o6_task_core
+    )
+    target_compile_definitions(rm_trajectory_replay_force_spike_thumb_index_tests
+        PRIVATE
+            RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
+    )
+    add_test(NAME rm_trajectory_replay_force_spike_thumb_index_tests
+        COMMAND rm_trajectory_replay_force_spike_thumb_index_tests)
+
     add_executable(rm_o6_bus_scanner_tests
         tests/test_o6_bus_scanner.cpp
     )

RM/include/rm_control/core/arm_o6_trajectory_task.h

 #pragma once
 
 #include "rm_control/common/result.h"
+#include "rm_control/core/force_spike_hand_guard.h"
 #include "rm_control/core/hardware_execution_gate.h"
 #include "rm_control/interfaces/dexterous_hand.h"
 #include "rm_control/interfaces/robot_arm.h"
@@ -42,6 +43,8 @@ struct ArmO6TrajectoryTaskConfig {
     double maximum_absolute_force_newton = 100.0;
     double maximum_absolute_torque_newton_meter = 20.0;
     std::uint16_t grip_position_tolerance = 5U;
+    bool open_hand_on_force_spike_during_replay = true;
+    ForceSpikeHandGuardConfig force_spike_hand_guard_config;
     std::chrono::milliseconds origin_status_poll_period{100};
     std::chrono::milliseconds origin_motion_timeout{300000};
     std::chrono::milliseconds program_status_poll_period{100};
@@ -127,6 +130,9 @@ struct ArmO6TrajectoryTaskReport {
     bool returned_from_trajectory_end = false;
     bool return_observed_running_state = false;
     bool observed_running_state = false;
+    bool force_spike_open_command_sent = false;
+    bool return_force_spike_open_command_sent = false;
+    bool forward_force_spike_open_command_sent = false;
 };
 
 /**

RM/include/rm_control/core/force_spike_hand_guard.h

+#pragma once
+
+#include "rm_control/common/result.h"
+#include "rm_control/interfaces/dexterous_hand.h"
+#include "rm_control/interfaces/robot_arm.h"
+#include "rm_control/modules/force_safety/force_spike_detector.h"
+
+namespace rm_control::core {
+
+interfaces::O6MotionCommand BuildDefaultO6FiveFingerOpenCommand();
+
+struct ForceSpikeHandGuardConfig {
+    modules::force_safety::ForceSpikeDetectorConfig detector;
+    interfaces::O6MotionCommand open_hand_command =
+        BuildDefaultO6FiveFingerOpenCommand();
+    bool latch_after_successful_open_command = true;
+};
+
+struct ForceSpikeHandGuardStepReport {
+    modules::force_safety::ForceSpikeDetectionReport detection;
+    bool open_command_sent = false;
+    bool open_command_suppressed_by_latch = false;
+};
+
+common::Result<void> ValidateForceSpikeHandGuardConfig(
+    const ForceSpikeHandGuardConfig &config);
+
+class ForceSpikeHandGuard final {
+public:
+    ForceSpikeHandGuard(interfaces::IRobotArm &robot,
+                        interfaces::IDexterousHand &hand,
+                        ForceSpikeHandGuardConfig config) noexcept;
+
+    common::Result<ForceSpikeHandGuardStepReport>
+    CheckForceOnceAndOpenHandOnSpike();
+
+    void ResetOpenCommandLatch() noexcept;
+
+private:
+    interfaces::IRobotArm &robot_;
+    interfaces::IDexterousHand &hand_;
+    ForceSpikeHandGuardConfig config_;
+    modules::force_safety::ForceSpikeDetectorState detector_state_;
+    bool open_command_latched_ = false;
+};
+
+} // namespace rm_control::core

RM/include/rm_control/modules/force_safety/force_spike_detector.h

+#pragma once
+
+#include "rm_control/common/result.h"
+#include "rm_control/common/robot_types.h"
+
+#include <array>
+#include <cstddef>
+
+namespace rm_control::modules::force_safety {
+
+enum class ForceDataSignal {
+    Raw,
+    Zero,
+    WorkZero,
+    ToolZero,
+};
+
+struct ForceSpikeDetectorConfig {
+    ForceDataSignal signal = ForceDataSignal::WorkZero;
+    float maximum_force_delta_newton = 5.0F;
+    float maximum_torque_delta_newton_meter = 0.8F;
+};
+
+struct ForceSpikeDetectorState {
+    bool has_previous_sample = false;
+    std::array<float, 6U> previous_sample{};
+};
+
+struct ForceSpikeDetectionReport {
+    bool baseline_initialized = false;
+    bool spike_detected = false;
+    std::size_t trigger_axis = 0U;
+    float measured_delta = 0.0F;
+    float configured_limit = 0.0F;
+    std::array<float, 6U> previous_sample{};
+    std::array<float, 6U> current_sample{};
+};
+
+common::Result<void> ValidateForceSpikeDetectorConfig(
+    const ForceSpikeDetectorConfig &config);
+
+std::array<float, 6U> SelectForceSignal(
+    const common::ForceData &force_data,
+    ForceDataSignal signal);
+
+common::Result<ForceSpikeDetectionReport> UpdateForceSpikeDetector(
+    ForceSpikeDetectorState *state,
+    const std::array<float, 6U> &current_sample,
+    const ForceSpikeDetectorConfig &config);
+
+} // namespace rm_control::modules::force_safety

RM/src/core/arm_o6_trajectory_task.cpp

@@ -33,6 +33,7 @@ struct ProgramMonitorResult {
     common::ControllerProgramRunState final_state;
     ControllerProgramTimingReport timing;
     bool observed_running_state = false;
+    bool force_spike_open_command_sent = false;
 };
 
 struct ProgramTimingAccumulator {
@@ -138,6 +139,14 @@ common::Result<void> ValidateO6MotionConfig(const ArmO6TrajectoryTaskConfig &con
     return common::Result<void>::success();
 }
 
+common::Result<void> ValidateForceSpikeGuardConfigForTrajectoryReplay(
+    const ArmO6TrajectoryTaskConfig &config) {
+    if (!config.open_hand_on_force_spike_during_replay) {
+        return common::Result<void>::success();
+    }
+    return ValidateForceSpikeHandGuardConfig(config.force_spike_hand_guard_config);
+}
+
 common::Result<void> ValidateTaskConfigImpl(const ArmO6TrajectoryTaskConfig &config) {
     const bool speeds_valid = config.origin_speed_percent >= kMinimumSpeedPercent &&
                               config.origin_speed_percent <= kMaximumSpeedPercent &&
@@ -160,11 +169,16 @@ common::Result<void> ValidateTaskConfigImpl(const ArmO6TrajectoryTaskConfig &con
         config.program_execution_timeout.count() > 0 &&
         config.hand_status_poll_period.count() >= 0 && config.hand_motion_timeout.count() >= 0;
     const auto o6_motion_check = ValidateO6MotionConfig(config);
+    const auto force_spike_guard_check =
+        ValidateForceSpikeGuardConfigForTrajectoryReplay(config);
     if (!speeds_valid || !program_id_valid || !scalar_limits_valid || !timing_valid ||
-        !o6_motion_check.ok) {
+        !o6_motion_check.ok || !force_spike_guard_check.ok) {
         if (!o6_motion_check.ok) {
             return o6_motion_check;
         }
+        if (!force_spike_guard_check.ok) {
+            return force_spike_guard_check;
+        }
         return common::Result<void>::failure(
             "RM75 O6 task configuration contains an invalid speed, limit, program id, or timeout");
     }
@@ -560,7 +574,8 @@ common::Result<ProgramMonitorResult> MonitorControllerProgram(
     int expected_program_id,
     int maximum_speed_percent,
     const std::string &phase_name,
-    const ArmO6TrajectoryTaskConfig &config) {
+    const ArmO6TrajectoryTaskConfig &config,
+    ForceSpikeHandGuard *force_spike_hand_guard) {
     const auto monitor_started_at = std::chrono::steady_clock::now();
     const auto deadline = monitor_started_at + config.program_execution_timeout;
     ProgramTimingAccumulator timing;
@@ -598,6 +613,18 @@ common::Result<ProgramMonitorResult> MonitorControllerProgram(
                 phase_name + " controller program returned unknown raw state " +
                 std::to_string(state.value.raw_state));
         }
+        if (force_spike_hand_guard != nullptr) {
+            const auto force_guard =
+                force_spike_hand_guard->CheckForceOnceAndOpenHandOnSpike();
+            if (!force_guard.ok) {
+                return common::Result<ProgramMonitorResult>::failure(
+                    phase_name + " force spike hand guard failed: " +
+                    force_guard.error_message);
+            }
+            if (force_guard.value.open_command_sent) {
+                monitor.force_spike_open_command_sent = true;
+            }
+        }
         if (std::chrono::steady_clock::now() >= deadline) {
             return common::Result<ProgramMonitorResult>::failure(
                 phase_name +
@@ -925,7 +952,8 @@ common::Result<ProgramMonitorResult> ExecuteSavedControllerProgramLeg(
     const ArmO6TrajectoryTaskConfig &config,
     const HardwareExecutionGateInput &gate_input,
     const common::RobotJointLimits &joint_limits,
-    TrajectoryEndpointCompletionMode endpoint_completion_mode) {
+    TrajectoryEndpointCompletionMode endpoint_completion_mode,
+    ForceSpikeHandGuard *force_spike_hand_guard) {
     if (leg.trajectory == nullptr || leg.phase_name == nullptr) {
         return common::Result<ProgramMonitorResult>::failure(
             "internal round-trip controller program leg is invalid");
@@ -960,7 +988,7 @@ common::Result<ProgramMonitorResult> ExecuteSavedControllerProgramLeg(
     }
     const auto monitor = MonitorControllerProgram(
         robot, leg.controller_program_id, config.plan_speed_percent,
-        leg.phase_name, config);
+        leg.phase_name, config, force_spike_hand_guard);
     if (!monitor.ok) {
         return monitor;
     }
@@ -986,7 +1014,8 @@ common::Result<ProgramMonitorResult> ExecuteArmTrajectory(
     const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
     const ArmO6TrajectoryTaskConfig &config,
     const HardwareExecutionGateInput &gate_input,
-    const common::RobotJointLimits &joint_limits) {
+    const common::RobotJointLimits &joint_limits,
+    ForceSpikeHandGuard *force_spike_hand_guard) {
     if (StopWasRequested(config)) {
         return common::Result<ProgramMonitorResult>::failure(
             "stop requested before moving RM75 to the trajectory origin");
@@ -1028,7 +1057,7 @@ common::Result<ProgramMonitorResult> ExecuteArmTrajectory(
     }
     return MonitorControllerProgram(
         robot, config.controller_program_id, config.plan_speed_percent,
-        "forward trajectory", config);
+        "forward trajectory", config, force_spike_hand_guard);
 }
 
 common::Result<void> RecheckRobotBeforeGrip(
@@ -1148,8 +1177,14 @@ common::Result<ArmO6TrajectoryTaskReport> ArmO6TrajectoryTask::Run(
         return common::Result<ArmO6TrajectoryTaskReport>::failure(
             preparation.error_message);
     }
+    ForceSpikeHandGuard force_spike_hand_guard(
+        robot_, hand_, config.force_spike_hand_guard_config);
+    ForceSpikeHandGuard *force_spike_hand_guard_ptr =
+        config.open_hand_on_force_spike_during_replay ? &force_spike_hand_guard : nullptr;
     const auto monitor = ExecuteArmTrajectory(
-        robot_, trajectory, config, gate_input, preparation.value.robot_preflight.joint_limits);
+        robot_, trajectory, config, gate_input,
+        preparation.value.robot_preflight.joint_limits,
+        force_spike_hand_guard_ptr);
     if (!monitor.ok) {
         return FailAfterMotionAttempt(robot_, monitor.error_message);
     }
@@ -1166,6 +1201,10 @@ common::Result<ArmO6TrajectoryTaskReport> ArmO6TrajectoryTask::Run(
     report.controller_timing = monitor.value.timing;
     report.trajectory_point_count = trajectory.trajectory_point_count;
     report.observed_running_state = monitor.value.observed_running_state;
+    report.force_spike_open_command_sent =
+        monitor.value.force_spike_open_command_sent;
+    report.forward_force_spike_open_command_sent =
+        monitor.value.force_spike_open_command_sent;
     return common::Result<ArmO6TrajectoryTaskReport>::success(report);
 }
 
@@ -1187,6 +1226,10 @@ ArmO6TrajectoryTask::RunFromTrajectoryEnd(
         return common::Result<ArmO6TrajectoryTaskReport>::failure(
             saved.error_message);
     }
+    ForceSpikeHandGuard force_spike_hand_guard(
+        robot_, hand_, config.force_spike_hand_guard_config);
+    ForceSpikeHandGuard *force_spike_hand_guard_ptr =
+        config.open_hand_on_force_spike_during_replay ? &force_spike_hand_guard : nullptr;
 
     const ControllerProgramLegRequest return_leg{
         &return_trajectory, config.return_controller_program_id,
@@ -1194,7 +1237,8 @@ ArmO6TrajectoryTask::RunFromTrajectoryEnd(
     const auto return_monitor = ExecuteSavedControllerProgramLeg(
         robot_, return_leg, config, gate_input,
         preparation.value.robot_preflight.joint_limits,
-        TrajectoryEndpointCompletionMode::SettleWithLowSpeedMoveJ);
+        TrajectoryEndpointCompletionMode::SettleWithLowSpeedMoveJ,
+        force_spike_hand_guard_ptr);
     if (!return_monitor.ok) {
         return FailAfterMotionAttempt(robot_, return_monitor.error_message);
     }
@@ -1205,7 +1249,8 @@ ArmO6TrajectoryTask::RunFromTrajectoryEnd(
     const auto forward_monitor = ExecuteSavedControllerProgramLeg(
         robot_, forward_leg, config, gate_input,
         preparation.value.robot_preflight.joint_limits,
-        TrajectoryEndpointCompletionMode::StrictValidation);
+        TrajectoryEndpointCompletionMode::StrictValidation,
+        force_spike_hand_guard_ptr);
     if (!forward_monitor.ok) {
         return FailAfterMotionAttempt(robot_, forward_monitor.error_message);
     }
@@ -1230,6 +1275,13 @@ ArmO6TrajectoryTask::RunFromTrajectoryEnd(
         return_monitor.value.observed_running_state;
     report.observed_running_state =
         forward_monitor.value.observed_running_state;
+    report.return_force_spike_open_command_sent =
+        return_monitor.value.force_spike_open_command_sent;
+    report.forward_force_spike_open_command_sent =
+        forward_monitor.value.force_spike_open_command_sent;
+    report.force_spike_open_command_sent =
+        report.return_force_spike_open_command_sent ||
+        report.forward_force_spike_open_command_sent;
     return common::Result<ArmO6TrajectoryTaskReport>::success(report);
 }
 

RM/src/core/force_spike_hand_guard.cpp

+#include "rm_control/core/force_spike_hand_guard.h"
+
+#include <string>
+
+namespace rm_control::core {
+namespace {
+
+constexpr std::uint16_t kMaximumO6CommandValue = 255U;
+
+common::Result<void> ValidateO6JointPose(
+    const interfaces::O6JointPose &values,
+    const char *field_name) {
+    for (std::size_t index = 0U; index < values.size(); ++index) {
+        if (values[index] > kMaximumO6CommandValue) {
+            return common::Result<void>::failure(
+                std::string("O6 ") + field_name + " at joint " +
+                std::to_string(index) + " must be in range 0..255");
+        }
+    }
+    return common::Result<void>::success();
+}
+
+} // namespace
+
+interfaces::O6MotionCommand BuildDefaultO6FiveFingerOpenCommand() {
+    interfaces::O6MotionCommand command;
+    command.target_positions = {255U, 255U, 0U, 255U, 255U, 255U};
+    command.joint_speed_commands = {64U, 64U, 64U, 64U, 64U, 64U};
+    command.joint_torque_commands = {3U, 3U, 3U, 3U, 3U, 3U};
+    return command;
+}
+
+common::Result<void> ValidateForceSpikeHandGuardConfig(
+    const ForceSpikeHandGuardConfig &config) {
+    const auto detector = modules::force_safety::ValidateForceSpikeDetectorConfig(
+        config.detector);
+    const auto positions = ValidateO6JointPose(
+        config.open_hand_command.target_positions, "target position command");
+    const auto speeds = ValidateO6JointPose(
+        config.open_hand_command.joint_speed_commands, "speed command");
+    const auto torques = ValidateO6JointPose(
+        config.open_hand_command.joint_torque_commands, "torque command");
+    if (!detector.ok) {
+        return detector;
+    }
+    if (!positions.ok) {
+        return positions;
+    }
+    if (!speeds.ok) {
+        return speeds;
+    }
+    if (!torques.ok) {
+        return torques;
+    }
+    return common::Result<void>::success();
+}
+
+ForceSpikeHandGuard::ForceSpikeHandGuard(
+    interfaces::IRobotArm &robot,
+    interfaces::IDexterousHand &hand,
+    ForceSpikeHandGuardConfig config) noexcept
+    : robot_(robot), hand_(hand), config_(config) {}
+
+common::Result<ForceSpikeHandGuardStepReport>
+ForceSpikeHandGuard::CheckForceOnceAndOpenHandOnSpike() {
+    const auto config_check = ValidateForceSpikeHandGuardConfig(config_);
+    if (!config_check.ok) {
+        return common::Result<ForceSpikeHandGuardStepReport>::failure(
+            config_check.error_message);
+    }
+
+    const auto force = robot_.getForceData();
+    if (!force.ok) {
+        return common::Result<ForceSpikeHandGuardStepReport>::failure(
+            "force spike hand guard failed to read RM75 force data: " +
+            force.error_message);
+    }
+
+    const auto sample = modules::force_safety::SelectForceSignal(
+        force.value, config_.detector.signal);
+    const auto detection = modules::force_safety::UpdateForceSpikeDetector(
+        &detector_state_, sample, config_.detector);
+    if (!detection.ok) {
+        return common::Result<ForceSpikeHandGuardStepReport>::failure(
+            detection.error_message);
+    }
+
+    ForceSpikeHandGuardStepReport report;
+    report.detection = detection.value;
+    if (!report.detection.spike_detected) {
+        return common::Result<ForceSpikeHandGuardStepReport>::success(report);
+    }
+    if (config_.latch_after_successful_open_command && open_command_latched_) {
+        report.open_command_suppressed_by_latch = true;
+        return common::Result<ForceSpikeHandGuardStepReport>::success(report);
+    }
+
+    const auto opened = hand_.CommandMotion(config_.open_hand_command);
+    if (!opened.ok) {
+        return common::Result<ForceSpikeHandGuardStepReport>::failure(
+            "force spike hand guard failed to command O6 open hand: " +
+            opened.error_message);
+    }
+
+    report.open_command_sent = true;
+    if (config_.latch_after_successful_open_command) {
+        open_command_latched_ = true;
+    }
+    return common::Result<ForceSpikeHandGuardStepReport>::success(report);
+}
+
+void ForceSpikeHandGuard::ResetOpenCommandLatch() noexcept {
+    open_command_latched_ = false;
+}
+
+} // namespace rm_control::core

RM/src/modules/force_safety/force_spike_detector.cpp

+#include "rm_control/modules/force_safety/force_spike_detector.h"
+
+#include <cmath>
+#include <string>
+
+namespace rm_control::modules::force_safety {
+namespace {
+
+constexpr std::size_t kFirstTorqueAxis = 3U;
+
+bool IsFinitePositive(float value) {
+    return std::isfinite(value) && value > 0.0F;
+}
+
+bool ForceSampleIsFinite(const std::array<float, 6U> &sample) {
+    for (const float value : sample) {
+        if (!std::isfinite(value)) {
+            return false;
+        }
+    }
+    return true;
+}
+
+float LimitForAxis(std::size_t axis, const ForceSpikeDetectorConfig &config) {
+    return axis < kFirstTorqueAxis
+        ? config.maximum_force_delta_newton
+        : config.maximum_torque_delta_newton_meter;
+}
+
+} // namespace
+
+common::Result<void> ValidateForceSpikeDetectorConfig(
+    const ForceSpikeDetectorConfig &config) {
+    if (!IsFinitePositive(config.maximum_force_delta_newton)) {
+        return common::Result<void>::failure(
+            "force spike detector force delta must be finite and positive");
+    }
+    if (!IsFinitePositive(config.maximum_torque_delta_newton_meter)) {
+        return common::Result<void>::failure(
+            "force spike detector torque delta must be finite and positive");
+    }
+    return common::Result<void>::success();
+}
+
+std::array<float, 6U> SelectForceSignal(
+    const common::ForceData &force_data,
+    ForceDataSignal signal) {
+    switch (signal) {
+    case ForceDataSignal::Raw:
+        return force_data.raw;
+    case ForceDataSignal::Zero:
+        return force_data.zero;
+    case ForceDataSignal::WorkZero:
+        return force_data.work_zero;
+    case ForceDataSignal::ToolZero:
+        return force_data.tool_zero;
+    }
+    return force_data.work_zero;
+}
+
+common::Result<ForceSpikeDetectionReport> UpdateForceSpikeDetector(
+    ForceSpikeDetectorState *state,
+    const std::array<float, 6U> &current_sample,
+    const ForceSpikeDetectorConfig &config) {
+    if (state == nullptr) {
+        return common::Result<ForceSpikeDetectionReport>::failure(
+            "force spike detector state must not be null");
+    }
+    const auto config_check = ValidateForceSpikeDetectorConfig(config);
+    if (!config_check.ok) {
+        return common::Result<ForceSpikeDetectionReport>::failure(
+            config_check.error_message);
+    }
+    if (!ForceSampleIsFinite(current_sample)) {
+        return common::Result<ForceSpikeDetectionReport>::failure(
+            "force spike detector sample contains a non-finite value");
+    }
+
+    ForceSpikeDetectionReport report;
+    report.current_sample = current_sample;
+    if (!state->has_previous_sample) {
+        state->previous_sample = current_sample;
+        state->has_previous_sample = true;
+        report.baseline_initialized = true;
+        report.previous_sample = current_sample;
+        return common::Result<ForceSpikeDetectionReport>::success(report);
+    }
+
+    report.previous_sample = state->previous_sample;
+    for (std::size_t axis = 0U; axis < current_sample.size(); ++axis) {
+        const float delta = std::fabs(current_sample[axis] - state->previous_sample[axis]);
+        const float limit = LimitForAxis(axis, config);
+        if (delta >= limit) {
+            report.spike_detected = true;
+            report.trigger_axis = axis;
+            report.measured_delta = delta;
+            report.configured_limit = limit;
+            break;
+        }
+    }
+
+    state->previous_sample = current_sample;
+    return common::Result<ForceSpikeDetectionReport>::success(report);
+}
+
+} // namespace rm_control::modules::force_safety

RM/tests/mocks/mock_robot_arm.h

@@ -16,6 +16,9 @@ public:
     common::RobotSoftwareInfo software_info;
     common::ArmState arm_state;
     common::ForceData force_data;
+    common::Result<common::ForceData> force_data_result =
+        common::Result<common::ForceData>::success(force_data);
+    std::vector<common::ForceData> force_data_samples;
     common::RobotSafetyStatus safety_status;
     common::RobotJointLimits joint_limits;
     int vendor_save_return_code = 0;
@@ -25,6 +28,7 @@ public:
     int start_recording_failures_remaining = 0;
     int stop_recording_failures_remaining = 0;
     int current_state_calls = 0;
+    int force_data_calls = 0;
     int start_calls = 0;
     int stop_recording_calls = 0;
     int save_calls = 0;
@@ -116,6 +120,16 @@ public:
     }
 
     common::Result<common::ForceData> getForceData() override {
+        ++force_data_calls;
+        if (!force_data_result.ok) {
+            return force_data_result;
+        }
+        if (!force_data_samples.empty()) {
+            force_data = force_data_samples.front();
+            if (force_data_samples.size() > 1U) {
+                force_data_samples.erase(force_data_samples.begin());
+            }
+        }
         return common::Result<common::ForceData>::success(force_data);
     }
 

RM/tests/test_arm_o6_trajectory_task.cpp

@@ -5,6 +5,7 @@
 #include "mocks/mock_robot_arm.h"
 
 #include <chrono>
+#include <array>
 #include <cmath>
 #include <cstddef>
 #include <filesystem>
@@ -167,6 +168,13 @@ rm_control::interfaces::DexterousHandReadiness MakeReadyHand(
     return readiness;
 }
 
+rm_control::common::ForceData MakeWorkZeroForce(
+    const std::array<float, 6U> &sample) {
+    rm_control::common::ForceData data;
+    data.work_zero = sample;
+    return data;
+}
+
 class SequencedDexterousHand final : public rm_control::interfaces::IDexterousHand {
 public:
     std::vector<rm_control::common::Result<
@@ -177,6 +185,7 @@ public:
     int readiness_calls = 0;
     int command_calls = 0;
     rm_control::interfaces::O6MotionCommand last_motion_command{};
+    std::vector<rm_control::interfaces::O6MotionCommand> motion_commands;
 
     rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
     ReadReadiness() override {
@@ -197,6 +206,7 @@ public:
         const rm_control::interfaces::O6MotionCommand &command) override {
         ++command_calls;
         last_motion_command = command;
+        motion_commands.push_back(command);
         if (stop_requested_after_command != nullptr) {
             *stop_requested_after_command = true;
         }
@@ -262,6 +272,65 @@ void TestCompletesTrajectoryThenCommandsAndVerifiesGrip(TestContext *context) {
     }
 }
 
+void TestForceSpikeDuringForwardReplayCommandsOpenHandBeforeFinalGrip(
+    TestContext *context) {
+    rm_control::tests::MockRobotArm robot;
+    MakeRobotSafetyArraysExplicit(&robot);
+    robot.program_run_states = {
+        rm_control::common::ControllerProgramRunState{1, 1, 0, 5},
+        rm_control::common::ControllerProgramRunState{1, 1, 0, 5},
+        rm_control::common::ControllerProgramRunState{0, 1, 0, 5},
+    };
+    robot.force_data_samples = {
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({0.0F, 5.2F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({0.0F, 5.2F, 0.0F, 0.0F, 0.0F, 0.0F}),
+    };
+    SequencedDexterousHand hand;
+    const auto config = MakeTaskConfig();
+    const rm_control::interfaces::O6JointPose open_pose{};
+    hand.readiness_results = {
+        rm_control::common::Result<
+            rm_control::interfaces::DexterousHandReadiness>::success(
+                MakeReadyHand(open_pose)),
+        rm_control::common::Result<
+            rm_control::interfaces::DexterousHandReadiness>::success(
+                MakeReadyHand(open_pose)),
+        rm_control::common::Result<
+            rm_control::interfaces::DexterousHandReadiness>::success(
+                MakeReadyHand(config.grip_pose)),
+    };
+    rm_control::core::ArmO6TrajectoryTask task(robot, hand);
+
+    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
+
+    context->Expect(result.ok,
+                    "force spike during replay should not break a successful task");
+    context->Expect(robot.program_state_calls == 3,
+                    "test must exercise two running monitor polls before completion");
+    context->Expect(hand.command_calls == 2,
+                    "force spike should open O6 during replay before the final grip");
+    context->Expect(hand.motion_commands.size() == 2U,
+                    "test should capture both O6 commands in order");
+    if (hand.motion_commands.size() == 2U) {
+        const auto open_command =
+            rm_control::core::BuildDefaultO6FiveFingerOpenCommand();
+        context->Expect(hand.motion_commands[0].target_positions ==
+                            open_command.target_positions,
+                        "first O6 command during replay must be five-finger open");
+        context->Expect(hand.motion_commands[1].target_positions == config.grip_pose,
+                        "final O6 command should keep the existing post-trajectory grip");
+    }
+    if (result.ok) {
+        context->Expect(result.value.force_spike_open_command_sent &&
+                            result.value.forward_force_spike_open_command_sent,
+                        "success report should record the replay force spike open command");
+        context->Expect(!result.value.return_force_spike_open_command_sent,
+                        "single forward run must not mark a return-leg force spike");
+    }
+}
+
 void ConfigureSuccessfulRoundTripRobot(
     rm_control::tests::MockRobotArm *robot,
     const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
@@ -722,6 +791,32 @@ void TestRejectsO6MaximumOutsideProtocolRange(TestContext *context) {
         context, invalid_torque_maximum, "configured O6 torque maximum above 255");
 }
 
+void TestRejectsInvalidForceSpikeGuardConfigBeforeMotion(TestContext *context) {
+    auto invalid_force_spike_guard = MakeTaskConfig();
+    invalid_force_spike_guard.force_spike_hand_guard_config.detector
+        .maximum_force_delta_newton = 0.0F;
+    rm_control::tests::MockRobotArm robot;
+    MakeRobotSafetyArraysExplicit(&robot);
+    SequencedDexterousHand hand;
+    rm_control::core::ArmO6TrajectoryTask task(robot, hand);
+
+    const auto validation =
+        rm_control::core::ValidateArmO6TrajectoryTaskConfig(
+            invalid_force_spike_guard);
+    const auto result = task.Run(
+        MakeTrajectory(), invalid_force_spike_guard, MakeAllowedGate());
+
+    context->Expect(!validation.ok,
+                    "invalid force spike threshold must fail public validation");
+    context->Expect(!result.ok,
+                    "invalid force spike guard config must fail the task");
+    context->Expect(robot.move_joint_calls == 0 &&
+                        robot.execute_vendor_file_calls == 0,
+                    "invalid force spike guard must fail before RM75 motion");
+    context->Expect(hand.command_calls == 0,
+                    "invalid force spike guard must fail before O6 command");
+}
+
 void TestO6MotionWriteFailureSlowStops(TestContext *context) {
     rm_control::tests::MockRobotArm robot;
     MakeRobotSafetyArraysExplicit(&robot);
@@ -881,6 +976,7 @@ void TestControllerTimingReportMeasuresConfiguredPoll(TestContext *context) {
 int main() {
     TestContext context;
     TestCompletesTrajectoryThenCommandsAndVerifiesGrip(&context);
+    TestForceSpikeDuringForwardReplayCommandsOpenHandBeforeFinalGrip(&context);
     TestRoundTripReturnsThenReplaysBeforeGrip(&context);
     TestRoundTripSettlesSmallReturnEndpointErrorBeforeForwardReplay(&context);
     TestRoundTripRejectsCurrentStateAwayFromTrajectoryEnd(&context);
@@ -898,6 +994,7 @@ int main() {
     TestAcceptsO6CommandsAtConfiguredMaximum(&context);
     TestRejectsO6ConfiguredMaximumViolations(&context);
     TestRejectsO6MaximumOutsideProtocolRange(&context);
+    TestRejectsInvalidForceSpikeGuardConfigBeforeMotion(&context);
     TestO6MotionWriteFailureSlowStops(&context);
     TestO6GripReadbackTimeoutSlowStops(&context);
     TestStopRequestDuringProgramMonitorSlowStops(&context);

RM/tests/test_force_spike_detector.cpp

+#include "rm_control/modules/force_safety/force_spike_detector.h"
+
+#include <cmath>
+#include <iostream>
+#include <string>
+
+namespace {
+
+struct TestContext {
+    int failures = 0;
+
+    void Expect(bool condition, const std::string &message) {
+        if (!condition) {
+            ++failures;
+            std::cerr << "[FAIL] " << message << '\n';
+        }
+    }
+};
+
+rm_control::modules::force_safety::ForceSpikeDetectorConfig DetectorConfig() {
+    rm_control::modules::force_safety::ForceSpikeDetectorConfig config;
+    config.maximum_force_delta_newton = 5.0F;
+    config.maximum_torque_delta_newton_meter = 0.8F;
+    return config;
+}
+
+void TestFirstSampleInitializesBaselineWithoutSpike(TestContext *context) {
+    rm_control::modules::force_safety::ForceSpikeDetectorState state;
+    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
+        &state, {1.0F, 2.0F, 3.0F, 0.1F, 0.2F, 0.3F}, DetectorConfig());
+
+    context->Expect(result.ok, "first force sample should be accepted");
+    context->Expect(result.value.baseline_initialized,
+                    "first force sample should initialize the baseline");
+    context->Expect(!result.value.spike_detected,
+                    "first force sample must not trigger a spike");
+    context->Expect(state.has_previous_sample,
+                    "detector should remember the first sample");
+}
+
+void TestForceAxisDeltaAtThresholdTriggersSpike(TestContext *context) {
+    rm_control::modules::force_safety::ForceSpikeDetectorState state;
+    const auto config = DetectorConfig();
+    (void)rm_control::modules::force_safety::UpdateForceSpikeDetector(
+        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);
+
+    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
+        &state, {0.0F, 5.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);
+
+    context->Expect(result.ok, "force spike detector should accept finite sample");
+    context->Expect(result.value.spike_detected,
+                    "force delta equal to threshold should trigger");
+    context->Expect(result.value.trigger_axis == 1U,
+                    "force spike should report the triggering force axis");
+    context->Expect(result.value.configured_limit == config.maximum_force_delta_newton,
+                    "force spike should report the force threshold");
+}
+
+void TestTorqueAxisUsesTorqueThreshold(TestContext *context) {
+    rm_control::modules::force_safety::ForceSpikeDetectorState state;
+    const auto config = DetectorConfig();
+    (void)rm_control::modules::force_safety::UpdateForceSpikeDetector(
+        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);
+
+    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
+        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.9F, 0.0F}, config);
+
+    context->Expect(result.ok, "torque spike detector should accept finite sample");
+    context->Expect(result.value.spike_detected,
+                    "torque delta above torque threshold should trigger");
+    context->Expect(result.value.trigger_axis == 4U,
+                    "torque spike should report the triggering torque axis");
+    context->Expect(result.value.configured_limit ==
+                        config.maximum_torque_delta_newton_meter,
+                    "torque spike should report the torque threshold");
+}
+
+void TestBelowThresholdUpdatesBaselineWithoutSpike(TestContext *context) {
+    rm_control::modules::force_safety::ForceSpikeDetectorState state;
+    const auto config = DetectorConfig();
+    (void)rm_control::modules::force_safety::UpdateForceSpikeDetector(
+        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);
+
+    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
+        &state, {4.9F, 0.0F, 0.0F, 0.0F, 0.7F, 0.0F}, config);
+
+    context->Expect(result.ok, "below-threshold sample should be accepted");
+    context->Expect(!result.value.spike_detected,
+                    "below-threshold force and torque should not trigger");
+    context->Expect(state.previous_sample[0] == 4.9F &&
+                        state.previous_sample[4] == 0.7F,
+                    "detector should advance the baseline after every valid sample");
+}
+
+void TestInvalidConfigAndNonFiniteSampleFailClosed(TestContext *context) {
+    auto invalid_config = DetectorConfig();
+    invalid_config.maximum_force_delta_newton = 0.0F;
+    rm_control::modules::force_safety::ForceSpikeDetectorState state;
+    const auto invalid_config_result =
+        rm_control::modules::force_safety::UpdateForceSpikeDetector(
+            &state, {}, invalid_config);
+    context->Expect(!invalid_config_result.ok,
+                    "zero force threshold must be rejected");
+
+    const auto non_finite_sample =
+        rm_control::modules::force_safety::UpdateForceSpikeDetector(
+            &state,
+            {0.0F, std::nanf(""), 0.0F, 0.0F, 0.0F, 0.0F},
+            DetectorConfig());
+    context->Expect(!non_finite_sample.ok,
+                    "non-finite force sample must be rejected");
+}
+
+void TestSelectForceSignalKeepsConfiguredForceFrame(TestContext *context) {
+    rm_control::common::ForceData force_data;
+    force_data.raw = {1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F};
+    force_data.zero = {2.0F, 2.0F, 2.0F, 2.0F, 2.0F, 2.0F};
+    force_data.work_zero = {3.0F, 3.0F, 3.0F, 3.0F, 3.0F, 3.0F};
+    force_data.tool_zero = {4.0F, 4.0F, 4.0F, 4.0F, 4.0F, 4.0F};
+
+    const auto selected = rm_control::modules::force_safety::SelectForceSignal(
+        force_data, rm_control::modules::force_safety::ForceDataSignal::ToolZero);
+
+    context->Expect(selected[0] == 4.0F && selected[5] == 4.0F,
+                    "force detector should use the configured force data frame");
+}
+
+} // namespace
+
+int main() {
+    TestContext context;
+    TestFirstSampleInitializesBaselineWithoutSpike(&context);
+    TestForceAxisDeltaAtThresholdTriggersSpike(&context);
+    TestTorqueAxisUsesTorqueThreshold(&context);
+    TestBelowThresholdUpdatesBaselineWithoutSpike(&context);
+    TestInvalidConfigAndNonFiniteSampleFailClosed(&context);
+    TestSelectForceSignalKeepsConfiguredForceFrame(&context);
+
+    if (context.failures == 0) {
+        std::cout << "[PASS] rm_force_spike_detector_tests\n";
+        return 0;
+    }
+    std::cerr << context.failures << " test(s) failed\n";
+    return 1;
+}

RM/tests/test_force_spike_hand_guard.cpp

+#include "mocks/mock_dexterous_hand.h"
+#include "mocks/mock_robot_arm.h"
+
+#include "rm_control/core/force_spike_hand_guard.h"
+
+#include <iostream>
+#include <string>
+
+namespace {
+
+struct TestContext {
+    int failures = 0;
+
+    void Expect(bool condition, const std::string &message) {
+        if (!condition) {
+            ++failures;
+            std::cerr << "[FAIL] " << message << '\n';
+        }
+    }
+};
+
+rm_control::common::ForceData MakeWorkZeroForce(
+    const std::array<float, 6U> &sample) {
+    rm_control::common::ForceData data;
+    data.work_zero = sample;
+    return data;
+}
+
+rm_control::core::ForceSpikeHandGuardConfig GuardConfig() {
+    rm_control::core::ForceSpikeHandGuardConfig config;
+    config.detector.maximum_force_delta_newton = 5.0F;
+    config.detector.maximum_torque_delta_newton_meter = 0.8F;
+    config.open_hand_command =
+        rm_control::core::BuildDefaultO6FiveFingerOpenCommand();
+    return config;
+}
+
+void TestFirstSampleOnlyInitializesBaseline(TestContext *context) {
+    rm_control::tests::MockRobotArm robot;
+    robot.force_data_samples = {
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+    };
+    rm_control::tests::MockDexterousHand hand;
+    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());
+
+    const auto result = guard.CheckForceOnceAndOpenHandOnSpike();
+
+    context->Expect(result.ok, "baseline force sample should succeed");
+    context->Expect(result.value.detection.baseline_initialized,
+                    "first guard sample should initialize detector baseline");
+    context->Expect(!result.value.open_command_sent,
+                    "baseline initialization must not command the hand");
+    context->Expect(hand.command_calls == 0,
+                    "hand must not move before a force spike exists");
+}
+
+void TestForceSpikeCommandsDefaultOpenPoseImmediately(TestContext *context) {
+    rm_control::tests::MockRobotArm robot;
+    robot.force_data_samples = {
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({0.0F, 5.2F, 0.0F, 0.0F, 0.0F, 0.0F}),
+    };
+    rm_control::tests::MockDexterousHand hand;
+    const auto expected_open_command =
+        rm_control::core::BuildDefaultO6FiveFingerOpenCommand();
+    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());
+
+    const auto first = guard.CheckForceOnceAndOpenHandOnSpike();
+    const auto second = guard.CheckForceOnceAndOpenHandOnSpike();
+
+    context->Expect(first.ok && second.ok,
+                    "baseline and spike guard steps should succeed");
+    context->Expect(second.value.detection.spike_detected,
+                    "second sample should detect the force spike");
+    context->Expect(second.value.open_command_sent,
+                    "force spike should command O6 open in the same guard step");
+    context->Expect(hand.readiness_calls == 0,
+                    "immediate open command should not add a readiness round trip");
+    context->Expect(hand.command_calls == 1,
+                    "force spike should send exactly one hand command");
+    context->Expect(hand.last_motion_command.target_positions ==
+                        expected_open_command.target_positions,
+                    "force spike should send the configured five-finger open pose");
+    context->Expect(hand.last_motion_command.joint_speed_commands ==
+                        expected_open_command.joint_speed_commands,
+                    "force spike should send the configured open speed commands");
+    context->Expect(hand.last_motion_command.joint_torque_commands ==
+                        expected_open_command.joint_torque_commands,
+                    "force spike should send the configured open torque commands");
+}
+
+void TestLatchSuppressesRepeatedOpenUntilReset(TestContext *context) {
+    rm_control::tests::MockRobotArm robot;
+    robot.force_data_samples = {
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({6.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({6.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({6.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+    };
+    rm_control::tests::MockDexterousHand hand;
+    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());
+
+    (void)guard.CheckForceOnceAndOpenHandOnSpike();
+    const auto first_spike = guard.CheckForceOnceAndOpenHandOnSpike();
+    (void)guard.CheckForceOnceAndOpenHandOnSpike();
+    const auto latched_spike = guard.CheckForceOnceAndOpenHandOnSpike();
+    guard.ResetOpenCommandLatch();
+    (void)guard.CheckForceOnceAndOpenHandOnSpike();
+    const auto reset_spike = guard.CheckForceOnceAndOpenHandOnSpike();
+
+    context->Expect(first_spike.ok && first_spike.value.open_command_sent,
+                    "first spike should command open hand");
+    context->Expect(latched_spike.ok &&
+                        latched_spike.value.open_command_suppressed_by_latch,
+                    "latched guard should suppress repeated open command");
+    context->Expect(reset_spike.ok && reset_spike.value.open_command_sent,
+                    "reset latch should allow a later spike to command open again");
+    context->Expect(hand.command_calls == 2,
+                    "guard should send one command before reset and one after reset");
+}
+
+void TestForceReadFailurePreventsOpenCommand(TestContext *context) {
+    rm_control::tests::MockRobotArm robot;
+    robot.force_data_result =
+        rm_control::common::Result<rm_control::common::ForceData>::failure(
+            "mock force read failed");
+    rm_control::tests::MockDexterousHand hand;
+    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());
+
+    const auto result = guard.CheckForceOnceAndOpenHandOnSpike();
+
+    context->Expect(!result.ok, "force read failure should fail the guard step");
+    context->Expect(hand.command_calls == 0,
+                    "force read failure must not command the hand");
+}
+
+void TestInvalidConfigFailsBeforeReadingForce(TestContext *context) {
+    rm_control::tests::MockRobotArm robot;
+    rm_control::tests::MockDexterousHand hand;
+    auto config = GuardConfig();
+    config.detector.maximum_force_delta_newton = 0.0F;
+    rm_control::core::ForceSpikeHandGuard guard(robot, hand, config);
+
+    const auto result = guard.CheckForceOnceAndOpenHandOnSpike();
+
+    context->Expect(!result.ok, "invalid force threshold should fail the guard step");
+    context->Expect(robot.force_data_calls == 0,
+                    "invalid config must be rejected before reading hardware state");
+    context->Expect(hand.command_calls == 0,
+                    "invalid config must not command the hand");
+}
+
+void TestHandCommandFailureIsReturned(TestContext *context) {
+    rm_control::tests::MockRobotArm robot;
+    robot.force_data_samples = {
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
+        MakeWorkZeroForce({0.0F, 0.0F, 5.5F, 0.0F, 0.0F, 0.0F}),
+    };
+    rm_control::tests::MockDexterousHand hand;
+    hand.command_result =
+        rm_control::common::Result<void>::failure("mock hand command failed");
+    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());
+
+    const auto first = guard.CheckForceOnceAndOpenHandOnSpike();
+    const auto second = guard.CheckForceOnceAndOpenHandOnSpike();
+
+    context->Expect(first.ok, "baseline guard step should succeed");
+    context->Expect(!second.ok, "failed O6 open command should fail the guard step");
+    context->Expect(hand.command_calls == 1,
+                    "force spike should attempt exactly one O6 open command");
+}
+
+} // namespace
+
+int main() {
+    TestContext context;
+    TestFirstSampleOnlyInitializesBaseline(&context);
+    TestForceSpikeCommandsDefaultOpenPoseImmediately(&context);
+    TestLatchSuppressesRepeatedOpenUntilReset(&context);
+    TestForceReadFailurePreventsOpenCommand(&context);
+    TestInvalidConfigFailsBeforeReadingForce(&context);
+    TestHandCommandFailureIsReturned(&context);
+
+    if (context.failures == 0) {
+        std::cout << "[PASS] rm_force_spike_hand_guard_tests\n";
+        return 0;
+    }
+    std::cerr << context.failures << " test(s) failed\n";
+    return 1;
+}

RM/tests/test_trajectory_replay_force_spike_thumb_index.cpp

+#include "rm_control/core/arm_o6_trajectory_task.h"
+#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"
+
+#include "mocks/mock_robot_arm.h"
+
+#include <array>
+#include <chrono>
+#include <cstddef>
+#include <filesystem>
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <system_error>
+#include <vector>
+
+#ifndef RM_TEST_OUTPUT_DIR
+#define RM_TEST_OUTPUT_DIR "."
+#endif
+
+namespace {
+
+constexpr int kReplayLoopCount = 10;
+constexpr int kForceSpikeCycleIndex = 4;
+constexpr float kForceSpikeNewton = 6.0F;
+
+struct TestContext {
+    int failures = 0;
+
+    void Expect(bool condition, const std::string &message) {
+        if (!condition) {
+            ++failures;
+            std::cerr << "[FAIL] " << message << '\n';
+        }
+    }
+};
+
+const std::string &ValidTrajectoryJsonLines() {
+    static const std::string content =
+        "{\"point\":[0,0,0,0,0,0,0]}\n"
+        "{\"point\":[100,0,0,0,0,0,0]}\n"
+        "{\"point\":[200,0,0,0,0,0,0]}\n";
+    return content;
+}
+
+std::filesystem::path TestOutputRoot() {
+    static const std::filesystem::path output_root = [] {
+        const auto unique_suffix =
+            std::chrono::steady_clock::now().time_since_epoch().count();
+        const std::filesystem::path path =
+            std::filesystem::path(RM_TEST_OUTPUT_DIR) /
+            ("rm_trajectory_replay_force_spike_thumb_index_" +
+             std::to_string(unique_suffix));
+        std::error_code filesystem_error;
+        std::filesystem::create_directories(path, filesystem_error);
+        if (filesystem_error) {
+            std::cerr << "[FAIL] failed to create test output: "
+                      << filesystem_error.message() << '\n';
+        }
+        return path;
+    }();
+    return output_root;
+}
+
+rm_control::modules::trajectory::VendorDragTrajectoryParseResult
+CreateReadOnlyTrajectoryFixture() {
+    const std::filesystem::path path = TestOutputRoot() / "loop_replay.jsonl";
+    {
+        std::ofstream output(path, std::ios::binary | std::ios::trunc);
+        output << ValidTrajectoryJsonLines();
+    }
+    const auto parsed =
+        rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(path, 100);
+    if (!parsed.ok) {
+        std::cerr << "[FAIL] failed to parse loop replay fixture: "
+                  << parsed.error_message << '\n';
+        return {};
+    }
+    std::error_code filesystem_error;
+    const auto read_only_permissions = std::filesystem::perms::owner_read |
+        std::filesystem::perms::group_read | std::filesystem::perms::others_read;
+    std::filesystem::permissions(
+        path, read_only_permissions, std::filesystem::perm_options::replace,
+        filesystem_error);
+    if (filesystem_error) {
+        std::cerr << "[FAIL] failed to make loop replay fixture read-only: "
+                  << filesystem_error.message() << '\n';
+    }
+    return parsed.value;
+}
+
+rm_control::modules::trajectory::VendorDragTrajectoryParseResult MakeTrajectory() {
+    static const auto trajectory = CreateReadOnlyTrajectoryFixture();
+    return trajectory;
+}
+
+rm_control::interfaces::O6MotionCommand BuildThumbAndIndexOpenCommand() {
+    rm_control::interfaces::O6MotionCommand command;
+    command.target_positions = {255U, 255U, 0U, 55U, 55U, 55U};
+    command.joint_speed_commands = {64U, 64U, 64U, 64U, 64U, 64U};
+    command.joint_torque_commands = {3U, 3U, 3U, 3U, 3U, 3U};
+    return command;
+}
+
+rm_control::core::ArmO6TrajectoryTaskConfig MakeTaskConfig() {
+    rm_control::core::ArmO6TrajectoryTaskConfig config;
+    config.validation_limits.maximum_joint_jump_deg = 1.0;
+    config.validation_limits.maximum_joint_velocity_deg_per_second = 10.0;
+    config.validation_limits.maximum_joint_acceleration_deg_per_second_squared = 100.0;
+    config.origin_speed_percent = 5;
+    config.plan_speed_percent = 5;
+    config.controller_program_id = 1;
+    config.grip_pose = {140U, 100U, 55U, 55U, 55U, 55U};
+    config.grip_speed_commands = {64U, 64U, 64U, 64U, 64U, 64U};
+    config.grip_torque_commands = {80U, 80U, 80U, 80U, 80U, 80U};
+    config.maximum_o6_speed_command = 64U;
+    config.maximum_o6_torque_command = 80U;
+    config.maximum_origin_delta_deg = 5.0;
+    config.origin_position_tolerance_deg = 0.5;
+    config.program_status_poll_period = std::chrono::milliseconds(0);
+    config.program_execution_timeout = std::chrono::milliseconds(50);
+    config.hand_status_poll_period = std::chrono::milliseconds(0);
+    config.hand_motion_timeout = std::chrono::milliseconds(0);
+    config.open_hand_on_force_spike_during_replay = true;
+    config.force_spike_hand_guard_config.open_hand_command =
+        BuildThumbAndIndexOpenCommand();
+    return config;
+}
+
+rm_control::core::HardwareExecutionGateInput MakeAllowedGate() {
+    rm_control::core::HardwareExecutionGateInput gate;
+    gate.live_requested = true;
+    gate.hardware_execution_enabled = true;
+    gate.arm_connected = true;
+    gate.hand_connected = true;
+    gate.emergency_stop_verified = true;
+    gate.safety_limits_loaded = true;
+    gate.operator_confirmed = true;
+    gate.trajectory_validated = true;
+    gate.trajectory_sample_period_confirmed = true;
+    gate.vendor_raw_units_per_degree_confirmed = true;
+    gate.workspace_and_virtual_wall_verified = true;
+    gate.configured_max_speed_percent = 10;
+    return gate;
+}
+
+void MakeRobotSafetyArraysExplicit(rm_control::tests::MockRobotArm *robot) {
+    robot->safety_status.joint_enable_states.fill(1);
+    robot->safety_status.joint_error_codes.fill(0);
+    robot->safety_status.arm_error_codes.clear();
+    robot->safety_status.controller_error_code = 0;
+}
+
+rm_control::interfaces::DexterousHandReadiness MakeReadyHand(
+    const rm_control::interfaces::O6JointPose &positions) {
+    rm_control::interfaces::DexterousHandReadiness readiness;
+    readiness.ready = true;
+    readiness.device_id = 0x27U;
+    readiness.freedom = 6U;
+    readiness.direction = 'R';
+    readiness.joint_positions = positions;
+    return readiness;
+}
+
+rm_control::common::ForceData MakeWorkZeroForce(
+    const std::array<float, 6U> &sample) {
+    rm_control::common::ForceData data;
+    data.work_zero = sample;
+    return data;
+}
+
+class RecordingDexterousHand final : public rm_control::interfaces::IDexterousHand {
+public:
+    explicit RecordingDexterousHand(
+        rm_control::interfaces::O6JointPose initial_positions)
+        : current_positions_(initial_positions) {}
+
+    int readiness_calls = 0;
+    int command_calls = 0;
+    std::vector<rm_control::interfaces::O6MotionCommand> motion_commands;
+    rm_control::common::Result<void> command_result =
+        rm_control::common::Result<void>::success();
+
+    rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
+    ReadReadiness() override {
+        ++readiness_calls;
+        return rm_control::common::Result<
+            rm_control::interfaces::DexterousHandReadiness>::success(
+            MakeReadyHand(current_positions_));
+    }
+
+    rm_control::common::Result<void> CommandMotion(
+        const rm_control::interfaces::O6MotionCommand &command) override {
+        ++command_calls;
+        motion_commands.push_back(command);
+        if (!command_result.ok) {
+            return command_result;
+        }
+        current_positions_ = command.target_positions;
+        return rm_control::common::Result<void>::success();
+    }
+
+private:
+    rm_control::interfaces::O6JointPose current_positions_{};
+};
+
+void AppendForceSamplesForReplayCycle(
+    rm_control::tests::MockRobotArm *robot,
+    bool force_spike_during_monitoring) {
+    robot->force_data_samples.push_back(
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}));
+    robot->force_data_samples.push_back(
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}));
+    robot->force_data_samples.push_back(MakeWorkZeroForce(
+        {force_spike_during_monitoring ? kForceSpikeNewton : 0.0F,
+         0.0F,
+         0.0F,
+         0.0F,
+         0.0F,
+         0.0F}));
+    robot->force_data_samples.push_back(
+        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}));
+}
+
+void AppendControllerStatesForReplayCycle(
+    rm_control::tests::MockRobotArm *robot,
+    int controller_program_id) {
+    robot->program_run_states.push_back(
+        rm_control::common::ControllerProgramRunState{
+            1, controller_program_id, 0, 5});
+    robot->program_run_states.push_back(
+        rm_control::common::ControllerProgramRunState{
+            1, controller_program_id, 0, 5});
+    robot->program_run_states.push_back(
+        rm_control::common::ControllerProgramRunState{
+            0, controller_program_id, 0, 5});
+}
+
+void ConfigureRobotForTenReplayCycles(
+    rm_control::tests::MockRobotArm *robot,
+    const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
+    const rm_control::core::ArmO6TrajectoryTaskConfig &config) {
+    MakeRobotSafetyArraysExplicit(robot);
+    robot->program_run_states.clear();
+    robot->force_data_samples.clear();
+    robot->completed_program_joint_targets.clear();
+    for (int cycle = 0; cycle < kReplayLoopCount; ++cycle) {
+        AppendControllerStatesForReplayCycle(robot, config.controller_program_id);
+        AppendForceSamplesForReplayCycle(
+            robot, cycle == kForceSpikeCycleIndex);
+        robot->completed_program_joint_targets.push_back(
+            trajectory.samples.back().joints_deg);
+    }
+}
+
+void TestTenReplayCyclesOpenThumbAndIndexOnForceSpike(TestContext *context) {
+    const auto trajectory = MakeTrajectory();
+    const auto config = MakeTaskConfig();
+    rm_control::tests::MockRobotArm robot;
+    ConfigureRobotForTenReplayCycles(&robot, trajectory, config);
+    RecordingDexterousHand hand(config.grip_pose);
+    rm_control::core::ArmO6TrajectoryTask task(robot, hand);
+
+    for (int cycle = 0; cycle < kReplayLoopCount; ++cycle) {
+        const auto result = task.Run(trajectory, config, MakeAllowedGate());
+        context->Expect(result.ok, "each replay cycle should complete");
+        if (result.ok) {
+            context->Expect(
+                result.value.observed_running_state,
+                "each replay cycle should observe the controller running state");
+            context->Expect(
+                result.value.force_spike_open_command_sent ==
+                    (cycle == kForceSpikeCycleIndex),
+                "only the force spike cycle should report an O6 open command");
+        }
+    }
+
+    const auto thumb_index_open = BuildThumbAndIndexOpenCommand();
+    context->Expect(
+        robot.execute_vendor_file_calls == kReplayLoopCount,
+        "test should execute the trajectory file exactly ten times");
+    context->Expect(
+        robot.move_joint_calls == kReplayLoopCount,
+        "each replay cycle should still move to the trajectory origin through the task");
+    context->Expect(
+        robot.program_state_calls == kReplayLoopCount * 3,
+        "each replay cycle should poll running, running, then completed");
+    context->Expect(
+        robot.force_data_calls == kReplayLoopCount * 4,
+        "each replay cycle should read preflight, two monitor samples, and pre-grip force");
+    context->Expect(
+        hand.command_calls == kReplayLoopCount + 1,
+        "one force-spike open command plus ten final grip commands should be sent");
+    context->Expect(
+        hand.motion_commands.size() ==
+            static_cast<std::size_t>(kReplayLoopCount + 1),
+        "test should capture every O6 command in order");
+    if (hand.motion_commands.size() ==
+        static_cast<std::size_t>(kReplayLoopCount + 1)) {
+        const std::size_t open_command_index =
+            static_cast<std::size_t>(kForceSpikeCycleIndex);
+        context->Expect(
+            hand.motion_commands[open_command_index].target_positions ==
+                thumb_index_open.target_positions,
+            "force spike command must open thumb and index during replay");
+        context->Expect(
+            hand.motion_commands[open_command_index].joint_speed_commands ==
+                thumb_index_open.joint_speed_commands,
+            "force spike command should use the configured thumb/index speed");
+        context->Expect(
+            hand.motion_commands[open_command_index].joint_torque_commands ==
+                thumb_index_open.joint_torque_commands,
+            "force spike command should use the configured thumb/index torque");
+        context->Expect(
+            hand.motion_commands[open_command_index + 1U].target_positions ==
+                config.grip_pose,
+            "final grip command must still run after the spike-cycle trajectory completes");
+    }
+}
+
+} // namespace
+
+int main() {
+    TestContext context;
+    TestTenReplayCyclesOpenThumbAndIndexOnForceSpike(&context);
+
+    if (context.failures == 0) {
+        std::cout << "[PASS] rm_trajectory_replay_force_spike_thumb_index_tests\n";
+        return 0;
+    }
+    std::cerr << context.failures << " test(s) failed\n";
+    return 1;
+}

skills/linkerhand-sdk-quickread/SKILL.md

----
-name: linkerhand-sdk-quickread
-description: Use when working in the linkerhand-cpp-sdk repository and you need to quickly understand the project framework, locate the correct API layer, identify model-specific capabilities such as L6/L7/L10/L20/L25, or prepare for fast vibe-coding changes without rereading the whole repo.
----
-
-# LinkerHand SDK Quick Read
-
-Use this skill when the task is to understand, modify, document, or extend this repository efficiently.
-
-## Goal
-
-Build a fast and accurate mental model of the SDK before editing code or writing docs.
-
-## Quick workflow
-
-1. Read the framework entry points first:
-   - `README.md`
-   - `CMakeLists.txt`
-   - `include/Common.h`
-   - `include/LinkerHandApi.h`
-   - `include/HandFactory.h`
-2. Identify the target model header:
-   - `include/LinkerHandL6.h`
-   - `include/LinkerHandL7.h`
-   - `include/LinkerHandL10.h`
-   - `include/LinkerHandL20.h`
-   - `include/LinkerHandL25.h`
-   - `include/ModbusLinkerHandL10.h`
-3. Read `include/IHand.h` to see which methods are truly common and which ones can throw `UnsupportedFeatureException`.
-4. Read usage examples:
-   - `src/main.cpp`
-   - `examples/toolset_example.cpp`
-   - `examples/action_group_show_l10.cpp`
-5. Read docs only as needed:
-   - `docs/API-Reference.md`
-   - `docs/TROUBLESHOOTING.md`
-   - `docs/FAQ.md`
-   - `docs/L6-Application-Development-Manual.md`
-   - `docs/L6-API-Development-Guide.md`
-6. If you need a compact structure map, read `references/project-map.md`.
-
-## What to conclude early
-
-Before making claims, answer these points:
-
-- Is the user asking about the public API layer or a model-specific layer?
-- Is the capability exposed by `LinkerHandApi`, or only by a concrete hand class?
-- Is the behavior visible in headers/docs/examples, or hidden inside the prebuilt `.so`?
-- Is the task about SDK usage, repository structure, or extending the SDK itself?
-
-## Repository facts that matter
-
-- The main public entry point is `LinkerHandApi`.
-- The factory layer is `HandFactory`.
-- Cross-model behavior is defined by `IHand`.
-- Model-specific features live in model headers.
-- A large part of the implementation is shipped as prebuilt libraries under `lib/x86_64/` and `lib/aarch64/`.
-- Tests mostly cover utility and framework pieces, not all device behaviors.
-
-## Working rules
-
-- Prefer `rg --files` and `rg -n` to locate structure and APIs quickly.
-- For API claims, trust headers first, examples second, docs third.
-- If a behavior is inferred from factory logic or declarations rather than implementation, label it as an inference.
-- When documenting or changing L6 behavior, explicitly separate:
-  - generic API behavior
-  - `L6Hand`-only behavior
-- If a task concerns communication setup, inspect both `HandFactory.h` and the relevant docs before editing.
-
-## Common command pattern
-
-Use fast reads instead of broad scans:
-
-```bash
-rg --files
-rg -n "LinkerHandApi|HandFactory|IHand|L6Hand|fingerMove|getForce"
-sed -n '1,220p' include/LinkerHandApi.h
-```
-
-## Output expectation
-
-When summarizing the repo, keep the summary in this order:
-
-1. Public API entry
-2. Factory and abstraction layers
-3. Model-specific headers
-4. Examples and docs
-5. Build/test layout
-6. Any relevant limitation or ambiguity

skills/linkerhand-sdk-quickread/references/project-map.md

-# LinkerHand-CPP-SDK 项目框架速查
-
-## 1. 顶层结构
-
-| 路径 | 作用 |
-|---|---|
-| `include/` | 对外头文件、类型声明、各型号接口 |
-| `examples/` | 示例程序，最适合确认典型调用方式 |
-| `docs/` | 使用文档、FAQ、故障排查、API 参考 |
-| `tests/` | 工具类与框架级测试 |
-| `lib/x86_64/` | x86_64 预编译动态库 |
-| `lib/aarch64/` | aarch64 预编译动态库 |
-| `src/main.cpp` | 最小示例入口 |
-| `CMakeLists.txt` | 构建、示例、安装、测试入口 |
-
-## 2. 代码分层
-
-```text
-LinkerHandApi
-  -> HandFactory
-      -> IHand
-          -> L6Hand / L7Hand / L10Hand / L20Hand / L25Hand / ModbusL10Hand
-```
-
-## 3. 关键文件优先级
-
-### 第一批必读
-
-- `README.md`
-- `CMakeLists.txt`
-- `include/Common.h`
-- `include/LinkerHandApi.h`
-- `include/HandFactory.h`
-
-### 第二批按任务读
-
-- 型号问题：读对应 `include/LinkerHandL*.h`
-- 能力边界问题：读 `include/IHand.h`
-- 使用问题：读 `examples/toolset_example.cpp`
-- 构建/排错问题：读 `docs/TROUBLESHOOTING.md`
-
-## 4. L6 相关速记
-
-- 型号头文件：`include/LinkerHandL6.h`
-- 6 个关节位置值
-- 支持速度、扭矩、加速度、故障、温度、触觉矩阵、版本信息
-- `L6Hand` 还声明了 `reHome`、版本读取、参数保存等专有能力
-- `LinkerHandApi` 是更高层的统一封装，不会暴露所有型号专有方法
-
-## 5. 仓库约束
-
-- 很多核心实现不在源码中，而在 `lib/*/liblinkerhand_cpp_sdk.so*`
-- 因此“接口存在”与“内部实现细节可见”是两件事
-- 对行为判断，优先级建议：
-  1. 头文件声明
-  2. 示例代码
-  3. 文档说明
-  4. 推断
-
-## 6. 常用定位命令
-
-```bash
-rg --files
-rg -n "LinkerHandApi|HandFactory|IHand|L6Hand"
-rg -n "fingerMove|getState|getForce|setAcceleration|reHome"
-sed -n '1,220p' include/LinkerHandL6.h
-```
-
-## 7. 构建与测试
-
-构建：
-
-```bash
-mkdir build
-cd build
-cmake ..
-make
-```
-
-测试：
-
-```bash
-cmake .. -DBUILD_TESTING=ON
-ctest --output-on-failure
-```
-
-注意：测试依赖 `googletest`，由 `tests/CMakeLists.txt` 通过 `FetchContent` 拉取。

skills/rm75-drag-trajectory/SKILL.md

----
-name: rm75-drag-trajectory
-description: "Use when Codex needs to work with the local RM75 drag-teach trajectory workflow in /home/mashiro/project/linkerhand-cpp-sdk: six-dof force position+orientation drag teaching, synchronized CSV recording, vendor trajectory save attempts, CSV-to-C++ TCP waypoint conversion, RM/data/trajectories folder management, and safe build/dry-run steps. Trigger for requests about RM75 trajectory recording, drag teach files, C++ waypoint conversion, or maintaining the rm_control trajectory modules."
----
-
-# RM75 Drag Trajectory
-
-Use this skill for the RM75 drag-teach trajectory pipeline in the linkerhand checkout.
-
-## Workflow
-
-1. Read `references/workflow.md` before editing code or running commands.
-2. Prefer the `RM/` subproject for maintained C++ code.
-3. Keep the older `examples/realman_trajectory_*` programs as compatibility demos unless the user explicitly asks to remove or refactor them.
-4. Treat synchronized CSV as the primary artifact.
-5. Treat `rm_save_trajectory` vendor output as best-effort because the six-dof force drag API does not expose a trajectory-record flag.
-
-## Boundaries
-
-- Put RM SDK calls only in `RM/src/drivers/realman/`.
-- Put drag-teach workflow orchestration in `RM/src/core/`.
-- Put CSV, directory, and C++ waypoint conversion code in `RM/src/modules/trajectory/`.
-- Put human operating instructions in `docs/` or `RM/README.md`.
-- Do not run motion or drag-teach commands unless the user explicitly asks and confirms the safety setup.
-
-## Default Safety
-
-Build and validate first. For live drag teach, ensure the workspace is clear, payload is safe, and E-stop is reachable. For TCP replay work, generate C++ waypoint data first; do not jump directly from CSV to live `rm_movel`.

skills/rm75-drag-trajectory/agents/openai.yaml

-interface:
-  display_name: "RM75 Drag Trajectory"
-  short_description: "Record and convert RM75 drag trajectories"
-  default_prompt: "Use $rm75-drag-trajectory to record a safe RM75 drag-teach trajectory and convert it to C++ TCP waypoints."

skills/rm75-drag-trajectory/references/workflow.md

-# RM75 Drag Trajectory Workflow
-
-## Fixed Context
-
-- Repo: `/home/mashiro/project/linkerhand-cpp-sdk`
-- RM subproject: `/home/mashiro/project/linkerhand-cpp-sdk/RM`
-- SDK root: `/home/mashiro/RM_API2/C++`
-- Controller: `192.168.1.18:8080`
-- Robot: RM75, 7 DOF, integrated 6D force, controller generation 3
-
-## C++ Maintainer Map
-
-| Purpose | Path |
-| --- | --- |
-| Robot abstraction | `RM/include/rm_control/interfaces/robot_arm.h` |
-| RealMan SDK wrapper | `RM/include/rm_control/drivers/realman/realman_robot_arm.h`, `RM/src/drivers/realman/realman_robot_arm.cpp` |
-| Drag-teach orchestration | `RM/include/rm_control/core/drag_teach_recorder.h`, `RM/src/core/drag_teach_recorder.cpp` |
-| Trajectory storage and conversion | `RM/include/rm_control/modules/trajectory/trajectory_file_store.h`, `RM/src/modules/trajectory/trajectory_file_store.cpp` |
-| Live record CLI | `RM/src/drag_teach_record_main.cpp` |
-| Offline convert CLI | `RM/src/trajectory_convert_main.cpp` |
-
-## Data Folders
-
-Use this root:
-
-```text
-RM/data/trajectories/
-├── csv/
-├── vendor/
-├── converted/
-└── reports/
-```
-
-CSV is the source of truth. Vendor files are best-effort artifacts. Converted files hold generated C++ TCP waypoint tables.
-
-## Build And Test
-
-```bash
-cd /home/mashiro/project/linkerhand-cpp-sdk
-cmake -S RM -B RM/build -DCMAKE_BUILD_TYPE=Debug
-cmake --build RM/build -j
-ctest --test-dir RM/build --output-on-failure
-```
-
-These commands are safe: they do not move the robot.
-
-## Record Drag Teach
-
-Only run after explicit user confirmation and safety setup.
-
-```bash
-./RM/build/rm_drag_teach_record demo_pick_path 15 100
-```
-
-Default live behavior:
-
-- Connects to `192.168.1.18:8080`.
-- Starts six-dof force drag teach with position and orientation enabled.
-- Enables singular wall by default.
-- Samples current arm state every `100 ms`.
-- Writes CSV to `RM/data/trajectories/csv/demo_pick_path.csv`.
-- Attempts vendor save to `RM/data/trajectories/vendor/demo_pick_path_vendor.txt`.
-
-## Convert CSV To C++
-
-This is offline and safe.
-
-```bash
-./RM/build/rm_trajectory_convert RM/data/trajectories/csv/demo_pick_path.csv
-```
-
-Default output:
-
-```text
-RM/data/trajectories/converted/demo_pick_path_tcp_path.cpp
-```
-
-## Safety Rules
-
-- Do not run live drag teach or replay unless the user explicitly requests it.
-- First live run must be unloaded, low-speed/manual, with clear workspace and reachable E-stop.
-- Keep CSV as the primary replay/conversion input.
-- For TCP path reproduction, use CSV joint values as future IK references or validation data; do not assume TCP-only waypoints uniquely determine RM75 arm posture.