small phase fix and documenting potential issues

Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
Maximellerbach
2026-05-05 11:59:16 +02:00
parent 375f23559e
commit 8d9f137f70
2 changed files with 17 additions and 0 deletions
+16
View File
@@ -194,6 +194,9 @@ def _teleop_smooth_move_to(
Requires the teleoperator to support feedback Requires the teleoperator to support feedback
(i.e. have non-empty ``feedback_features`` and implement ``disable_torque`` / ``enable_torque``). (i.e. have non-empty ``feedback_features`` and implement ``disable_torque`` / ``enable_torque``).
TODO(Maxime): This blocks up to ``duration_s`` seconds, during this time
the follower robot doesn't receive new actions, this could be an issue on LeKiwi.
""" """
teleop.enable_torque() teleop.enable_torque()
current = teleop.get_action() current = teleop.get_action()
@@ -751,6 +754,10 @@ class DAggerStrategy(RolloutStrategy):
Slide the follower to the teleop's current pose so the robot meets Slide the follower to the teleop's current pose so the robot meets
the operator's hand rather than jumping to it on the first frame. the operator's hand rather than jumping to it on the first frame.
CORRECTING -> PAUSED (actuated teleop):
Re-enable torque to hold position after correction.
This will be potentially useful if cancelling the correction recording
PAUSED -> AUTONOMOUS: PAUSED -> AUTONOMOUS:
Reset and resume the inference engine. Reset and resume the inference engine.
""" """
@@ -760,6 +767,11 @@ class DAggerStrategy(RolloutStrategy):
engine.pause() engine.pause()
if _teleop_supports_feedback(teleop) and prev_action is not None: if _teleop_supports_feedback(teleop) and prev_action is not None:
# TODO(Maxime): prev_action is in robot action key space (output of robot_action_processor).
# send_feedback expects teleop feedback key space. For homogeneous setups (e.g. SO-101
# leader + SO-101 follower) the keys are identical so this works. If the processor pipeline
# does non-trivial key renaming (e.g. a rename_map on action keys), the interpolation in
# _teleop_smooth_move_to silently no-ops and the arm doesn't move.
logger.info("Smooth handover: moving leader arm to follower position") logger.info("Smooth handover: moving leader arm to follower position")
_teleop_smooth_move_to(teleop, prev_action) _teleop_smooth_move_to(teleop, prev_action)
@@ -776,6 +788,10 @@ class DAggerStrategy(RolloutStrategy):
if _teleop_supports_feedback(teleop): if _teleop_supports_feedback(teleop):
teleop.disable_torque() teleop.disable_torque()
elif old_phase == DAggerPhase.CORRECTING and new_phase == DAggerPhase.PAUSED:
if _teleop_supports_feedback(teleop):
teleop.enable_torque()
elif new_phase == DAggerPhase.AUTONOMOUS: elif new_phase == DAggerPhase.AUTONOMOUS:
logger.info("Resuming autonomous mode - resetting engine and interpolator") logger.info("Resuming autonomous mode - resetting engine and interpolator")
interpolator.reset() interpolator.reset()
@@ -348,6 +348,7 @@ class OpenArmMini(Teleoperator):
if left_goals: if left_goals:
self.bus_left.sync_write("Goal_Position", left_goals) self.bus_left.sync_write("Goal_Position", left_goals)
@check_if_not_connected
def send_feedback(self, feedback: dict[str, float]) -> None: def send_feedback(self, feedback: dict[str, float]) -> None:
self.write_goal_positions(feedback) self.write_goal_positions(feedback)