B601: preserve the MIT target when entering safe-home

Summary

The native rebot_b601_follower currently disables and closes the motors directly in disconnect(). It would be useful to support an optional safe-home trajectory before torque is disabled, especially when a recording or teleoperation process is stopped with Ctrl+C.

For B601 arms running in MIT mode, the handoff into homing must preserve the last commanded position target. Initializing the homing trajectory from the latest measured position can briefly remove the PD torque supporting the arm against gravity and cause the arm to dip before returning home.

Hardware and control mode

• Robot: reBot B601-DM, dual-arm setup
• Arm control mode: MIT
• Gripper control mode: FORCE_POS
• Reproduced on both left and right arms

Observed behavior

During normal control, the arm can have a non-zero target-to-feedback error that creates the MIT position-loop torque needed to carry the arm. For example, one captured run had:

commanded shoulder_lift: -56.69 deg
feedback shoulder_lift:  about -59.5 deg

If safe-home reads -59.5 deg and immediately uses it as the new MIT target, the approximately 2.8-degree supporting error is reset to zero. With tau_ff=0, the arm briefly loses that supporting torque, moves under gravity, and is then pulled back by the subsequent homing trajectory.

Disabling the gripper pre-open step does not remove the dip. Using a single-stage rather than a two-stage homing trajectory does not remove it either. Timestamped joint traces show that the important transition is the MIT target change at the start of homing.

Suggested behavior

1. Store the final clipped arm target successfully sent by send_action().
2. At safe-home entry, read feedback for diagnostics and final-position checks, but use the last commanded arm target as the command-space start of the trajectory.
3. Keep sending that target while any gripper pre-open dwell runs.
4. Interpolate from the last commanded target to the configured home target.
5. Fall back to feedback only when no complete previous arm target is available.
6. Disable torque only after homing succeeds; preserve torque on failure for manual recovery.

Conceptually:

feedback = read_arm_positions()
trajectory_start = (
    last_commanded_arm_target
    if complete(last_commanded_arm_target)
    else feedback
)

send_mit_target(trajectory_start)  # preserve the existing supporting PD error
run_home_trajectory(trajectory_start, home_target)

Regression test

A unit test can send a complete arm action whose target differs from mocked feedback, enter safe-home, and assert that the first safe-home command equals the last commanded target rather than the feedback position.

This keeps the MIT command continuous at the control-to-homing boundary. The same logic does not need to affect a POS_VEL implementation.