From d59505a735d4d8a2e2300906d3ce7b3f728f81b6 Mon Sep 17 00:00:00 2001 From: Baptiste Lubrano Lavadera <45080391+Mr-C4T@users.noreply.github.com> Date: Thu, 30 Jul 2026 13:38:14 +0200 Subject: [PATCH] feat(teleoperators): add DAgger/HIL smooth handover support for BiSOLeader (#4028) * fix: implement bimanual SO leader DAgger handover support - Add feedback_features property: enables DAgger's teleop_supports_feedback() check - Implement enable_torque()/disable_torque(): synchronized torque control for both arms - Implement send_feedback(): routes bimanual feedback to left/right arms with prefix stripping This fixes DAgger smooth handover for bimanual SO follower + SO leader setups: when pausing from policy to human intervention, both leader arms now move smoothly to the follower's current pose, avoiding discontinuities at the human takeover point. * Update hil_data_collection.mdx Signed-off-by: Baptiste Lubrano Lavadera <45080391+Mr-C4T@users.noreply.github.com> * Update bi_so_leader.py Signed-off-by: Baptiste Lubrano Lavadera <45080391+Mr-C4T@users.noreply.github.com> --------- Signed-off-by: Baptiste Lubrano Lavadera <45080391+Mr-C4T@users.noreply.github.com> Co-authored-by: Steven Palma --- docs/source/hil_data_collection.mdx | 1 + .../bi_so_leader/bi_so_leader.py | 51 +++++++++++++++++-- 2 files changed, 49 insertions(+), 3 deletions(-) diff --git a/docs/source/hil_data_collection.mdx b/docs/source/hil_data_collection.mdx index c7df0631e..b77b6a2d7 100644 --- a/docs/source/hil_data_collection.mdx +++ b/docs/source/hil_data_collection.mdx @@ -59,6 +59,7 @@ The `lerobot-rollout --strategy.type=dagger` mode requires **teleoperators with - `bi_openarm_mini` - Bimanual OpenArm Mini - `so_leader` - SO100 / SO101 leader arm +- `bi_so_leader` - Bimanual SO100 / SO101 leader arms > [!IMPORTANT] > The provided commands default to `bi_openarm_follower` + `bi_openarm_mini`. diff --git a/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py b/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py index 6a45b4d91..690ae7aa8 100644 --- a/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py +++ b/src/lerobot/teleoperators/bi_so_leader/bi_so_leader.py @@ -67,7 +67,15 @@ class BiSOLeader(BimanualMixin, Teleoperator): @cached_property def feedback_features(self) -> dict[str, type]: - return {} + # Bimanual teleop has feedback (can be actuated for handover). + # Return the same structure as action_features for consistency with left/right arms. + left_arm_features = self.left_arm.feedback_features + right_arm_features = self.right_arm.feedback_features + + return { + **{f"left_{k}": v for k, v in left_arm_features.items()}, + **{f"right_{k}": v for k, v in right_arm_features.items()}, + } def setup_motors(self) -> None: self.left_arm.setup_motors() @@ -87,6 +95,43 @@ class BiSOLeader(BimanualMixin, Teleoperator): return action_dict + def enable_torque(self) -> None: + """Enable torque on both leader arms for smooth handover.""" + self.left_arm.enable_torque() + self.right_arm.enable_torque() + + def disable_torque(self) -> None: + """Disable torque on both leader arms to allow human control.""" + self.left_arm.disable_torque() + self.right_arm.disable_torque() + + @check_if_not_connected def send_feedback(self, feedback: dict[str, float]) -> None: - # TODO: Implement force feedback - raise NotImplementedError + """Route bimanual feedback to left and right arms with proper prefix stripping. + + Receives feedback dict with keys like: left_shoulder_pan.pos, right_shoulder_pan.pos, ... + Splits and routes to each arm by removing the prefix. + + This enables DAgger smooth handover: when transitioning from policy control to human + intervention, both leader arms are commanded to the follower's current pose to avoid + discontinuities. + """ + # Split feedback by arm prefix + left_feedback = {} + right_feedback = {} + + for key, value in feedback.items(): + if key.startswith("left_"): + # Strip "left_" prefix and pass to left arm + stripped_key = key[5:] # len("left_") == 5 + left_feedback[stripped_key] = value + elif key.startswith("right_"): + # Strip "right_" prefix and pass to right arm + stripped_key = key[6:] # len("right_") == 6 + right_feedback[stripped_key] = value + + # Send to each arm + if left_feedback: + self.left_arm.send_feedback(left_feedback) + if right_feedback: + self.right_arm.send_feedback(right_feedback)