fix: settle RM75 return endpoint before replay

RM/src/core/arm_o6_trajectory_task.cpp

@@ -59,6 +59,11 @@ struct ControllerProgramLegRequest {
     const char *phase_name = nullptr;
 };
 
+enum class TrajectoryEndpointCompletionMode {
+    StrictValidation,
+    SettleWithLowSpeedMoveJ,
+};
+
 common::Result<std::uintmax_t> InspectReadOnlyRegularSourceFile(
     const std::filesystem::path &path) {
     std::error_code filesystem_error;
@@ -743,6 +748,65 @@ common::Result<void> ValidateRobotAtTrajectoryEndpoint(
     return common::Result<void>::success();
 }
 
+common::Result<common::ArmState> ReadAndValidateRobotStateForEndpoint(
+    interfaces::IRobotArm &robot,
+    const common::RobotJointLimits &joint_limits,
+    const ArmO6TrajectoryTaskConfig &config,
+    const std::string &context) {
+    const auto safety = robot.getRobotSafetyStatus();
+    const auto state = robot.getCurrentArmState();
+    const auto force = robot.getForceData();
+    if (!safety.ok || !state.ok || !force.ok) {
+        const std::string error = !safety.ok ? safety.error_message
+            : !state.ok ? state.error_message : force.error_message;
+        return common::Result<common::ArmState>::failure(context + " query failed: " + error);
+    }
+    const auto safety_check = ValidateRobotSafetyStatus(safety.value);
+    const auto state_check = ValidateArmState(state.value, joint_limits);
+    const auto force_check = ValidateForceData(force.value, config);
+    if (!safety_check.ok || !state_check.ok || !force_check.ok) {
+        const auto &failed = !safety_check.ok ? safety_check
+            : !state_check.ok ? state_check : force_check;
+        return common::Result<common::ArmState>::failure(failed.error_message);
+    }
+    return common::Result<common::ArmState>::success(state.value);
+}
+
+common::Result<void> SettleRobotAtTrajectoryEndpointWithinOriginLimit(
+    interfaces::IRobotArm &robot,
+    const common::RobotJointLimits &joint_limits,
+    const std::vector<float> &expected_joints_deg,
+    const ArmO6TrajectoryTaskConfig &config,
+    const std::string &context) {
+    const auto state = ReadAndValidateRobotStateForEndpoint(
+        robot, joint_limits, config, context);
+    if (!state.ok) {
+        return common::Result<void>::failure(state.error_message);
+    }
+    const auto strict_endpoint = ValidateOriginDifference(
+        state.value, expected_joints_deg, config.origin_position_tolerance_deg, context);
+    if (strict_endpoint.ok) {
+        return common::Result<void>::success();
+    }
+    const auto correction_limit = ValidateOriginDifference(
+        state.value, expected_joints_deg, config.maximum_origin_delta_deg,
+        context + " low-speed correction exceeds safe origin move limit");
+    if (!correction_limit.ok) {
+        return common::Result<void>::failure(strict_endpoint.error_message);
+    }
+    if (StopWasRequested(config)) {
+        return common::Result<void>::failure(
+            "stop requested before low-speed endpoint correction");
+    }
+    const auto move = robot.moveJ(
+        expected_joints_deg, config.origin_speed_percent, 0, 0, false);
+    if (!move.ok) {
+        return common::Result<void>::failure(
+            context + " low-speed correction failed: " + move.error_message);
+    }
+    return WaitForTrajectoryOrigin(robot, joint_limits, expected_joints_deg, config);
+}
+
 common::Result<RoundTripTaskPreparation> PrepareRoundTripTask(
     interfaces::IRobotArm &robot,
     interfaces::IDexterousHand &hand,
@@ -860,7 +924,8 @@ common::Result<ProgramMonitorResult> ExecuteSavedControllerProgramLeg(
     const ControllerProgramLegRequest &leg,
     const ArmO6TrajectoryTaskConfig &config,
     const HardwareExecutionGateInput &gate_input,
-    const common::RobotJointLimits &joint_limits) {
+    const common::RobotJointLimits &joint_limits,
+    TrajectoryEndpointCompletionMode endpoint_completion_mode) {
     if (leg.trajectory == nullptr || leg.phase_name == nullptr) {
         return common::Result<ProgramMonitorResult>::failure(
             "internal round-trip controller program leg is invalid");
@@ -899,9 +964,16 @@ common::Result<ProgramMonitorResult> ExecuteSavedControllerProgramLeg(
     if (!monitor.ok) {
         return monitor;
     }
-    const auto end_state = ValidateRobotAtTrajectoryEndpoint(
-        robot, joint_limits, leg.trajectory->samples.back().joints_deg,
-        config, std::string(leg.phase_name) + " end point mismatch");
+    const std::string end_context =
+        std::string(leg.phase_name) + " end point mismatch";
+    const auto end_state =
+        endpoint_completion_mode == TrajectoryEndpointCompletionMode::StrictValidation
+            ? ValidateRobotAtTrajectoryEndpoint(
+                  robot, joint_limits, leg.trajectory->samples.back().joints_deg,
+                  config, end_context)
+            : SettleRobotAtTrajectoryEndpointWithinOriginLimit(
+                  robot, joint_limits, leg.trajectory->samples.back().joints_deg,
+                  config, end_context);
     if (!end_state.ok) {
         return common::Result<ProgramMonitorResult>::failure(
             end_state.error_message);
@@ -1121,7 +1193,8 @@ ArmO6TrajectoryTask::RunFromTrajectoryEnd(
         "return trajectory"};
     const auto return_monitor = ExecuteSavedControllerProgramLeg(
         robot_, return_leg, config, gate_input,
-        preparation.value.robot_preflight.joint_limits);
+        preparation.value.robot_preflight.joint_limits,
+        TrajectoryEndpointCompletionMode::SettleWithLowSpeedMoveJ);
     if (!return_monitor.ok) {
         return FailAfterMotionAttempt(robot_, return_monitor.error_message);
     }
@@ -1131,7 +1204,8 @@ ArmO6TrajectoryTask::RunFromTrajectoryEnd(
         "forward trajectory"};
     const auto forward_monitor = ExecuteSavedControllerProgramLeg(
         robot_, forward_leg, config, gate_input,
-        preparation.value.robot_preflight.joint_limits);
+        preparation.value.robot_preflight.joint_limits,
+        TrajectoryEndpointCompletionMode::StrictValidation);
     if (!forward_monitor.ok) {
         return FailAfterMotionAttempt(robot_, forward_monitor.error_message);
     }

RM/tests/test_arm_o6_trajectory_task.cpp

@@ -355,6 +355,41 @@ void TestRoundTripReturnsThenReplaysBeforeGrip(TestContext *context) {
     }
 }
 
+void TestRoundTripSettlesSmallReturnEndpointErrorBeforeForwardReplay(
+    TestContext *context) {
+    const auto forward_trajectory = MakeTrajectory();
+    const auto return_trajectory = MakeReturnTrajectory();
+    rm_control::tests::MockRobotArm robot;
+    ConfigureSuccessfulRoundTripRobot(
+        &robot, return_trajectory, forward_trajectory);
+    auto drifted_forward_origin = forward_trajectory.samples.front().joints_deg;
+    drifted_forward_origin[2] += 0.08F;
+    robot.completed_program_joint_targets = {
+        drifted_forward_origin,
+        forward_trajectory.samples.back().joints_deg,
+    };
+    SequencedDexterousHand hand;
+    const auto config = MakeRoundTripTaskConfig();
+    ConfigureSuccessfulRoundTripHand(&hand, config);
+    rm_control::core::ArmO6TrajectoryTask task(robot, hand);
+
+    const auto result = task.RunFromTrajectoryEnd(
+        return_trajectory, forward_trajectory, config, MakeAllowedGate());
+
+    context->Expect(result.ok,
+                    "small return endpoint error should be settled before forward replay");
+    context->Expect(robot.move_joint_calls == 1,
+                    "round trip should use one low-speed correction moveJ");
+    context->Expect(!robot.last_move_joint_blocking,
+                    "correction moveJ must be nonblocking for stop observability");
+    context->Expect(robot.arm_state.joints_deg == forward_trajectory.samples.back().joints_deg,
+                    "successful forward replay should still finish at the forward end point");
+    context->Expect(robot.run_controller_program_calls == 2,
+                    "settled round trip must still run both controller programs");
+    context->Expect(hand.command_calls == 1,
+                    "O6 grip must remain after the corrected forward trajectory");
+}
+
 void TestRoundTripRejectsCurrentStateAwayFromTrajectoryEnd(TestContext *context) {
     const auto forward_trajectory = MakeTrajectory();
     const auto return_trajectory = MakeReturnTrajectory();
@@ -847,6 +882,7 @@ int main() {
     TestContext context;
     TestCompletesTrajectoryThenCommandsAndVerifiesGrip(&context);
     TestRoundTripReturnsThenReplaysBeforeGrip(&context);
+    TestRoundTripSettlesSmallReturnEndpointErrorBeforeForwardReplay(&context);
     TestRoundTripRejectsCurrentStateAwayFromTrajectoryEnd(&context);
     TestRoundTripForwardSaveFailurePreventsMotion(&context);
     TestRoundTripPausedReturnProgramStopsBeforeForward(&context);