From 9b538d6cbf395d3d425894b8d86f81bcb328cbbd Mon Sep 17 00:00:00 2001 From: Khalil Meftah Date: Tue, 28 Apr 2026 13:11:59 +0200 Subject: [PATCH] fix follower shakiness and space-bar trigger --- .../so100_to_so100_EE_deltas/teleoperate.py | 55 ++++++++++++++++--- 1 file changed, 47 insertions(+), 8 deletions(-) diff --git a/examples/so100_to_so100_EE_deltas/teleoperate.py b/examples/so100_to_so100_EE_deltas/teleoperate.py index 243c58191..c3834307f 100644 --- a/examples/so100_to_so100_EE_deltas/teleoperate.py +++ b/examples/so100_to_so100_EE_deltas/teleoperate.py @@ -44,7 +44,7 @@ from lerobot.robots.so_follower.robot_kinematic_processor import ( ) from lerobot.robots.so_follower.so_follower import SO101Follower from lerobot.teleoperators.so_leader.config_so_leader import SO101LeaderConfig -from lerobot.teleoperators.so_leader.so_leader import SO101Leader +from lerobot.teleoperators.so_leader.so101_leader_follower import SO101LeaderFollower from lerobot.utils.robot_utils import precise_sleep from lerobot.utils.rotation import Rotation @@ -170,6 +170,13 @@ class ForwardKinematicsJointsToEETargetAction(RobotActionProcessorStep): ] ) + # Deadband: zero out tiny deltas so encoder noise on the leader does not + # twitch the follower when the operator is holding the leader still. + # 5 % of step size = ~0.2 mm / ~0.25 deg with the current step sizes. + if float(np.linalg.norm(delta_pos)) < 0.05 and float(np.linalg.norm(delta_rvec)) < 0.05: + delta_pos = np.zeros_like(delta_pos) + delta_rvec = np.zeros_like(delta_rvec) + # Check if any of the normalized deltas exceed 1.0 max_normalized_pos = max( @@ -217,11 +224,17 @@ FPS = 20 # Initialize the robot and teleoperator config follower_config = SO101FollowerConfig(port="/dev/usb_follower_arm_a", id="follower_arm_a", use_degrees=True) -leader_config = SO101LeaderConfig(port="/dev/usb_leader_arm_a", id="leader_arm_a", use_degrees=True) +leader_config = SO101LeaderConfig( + port="/dev/usb_leader_arm_a", + id="leader_arm_a", + use_degrees=True, + leader_follower_mode=True, + use_gripper=True, +) # Initialize the robot and teleoperator follower = SO101Follower(follower_config) -leader = SO101Leader(leader_config) +leader = SO101LeaderFollower(leader_config) # NOTE: It is highly recommended to use the urdf in the SO-ARM100 repo: https://github.com/TheRobotStudio/SO-ARM100/blob/main/Simulation/SO101/so101_new_calib.urdf follower_kinematics_solver = RobotKinematics( @@ -273,7 +286,12 @@ ee_to_follower_joints = RobotProcessorPipeline[tuple[RobotAction, RobotObservati # end_effector_step_sizes={"x": 0.006, "y": 0.01, "z": 0.005}, end_effector_step_sizes=end_effector_step_sizes, motor_names=list(follower.bus.motors.keys()), - use_latched_reference=False, + # Latch the reference so the follower stops drifting when the leader + # is held still. With ``False`` the reference is recomputed every + # step from the previous IK solution, which combined with motor lag + # and encoder noise creates a positive feedback loop (follower keeps + # moving even when leader is stationary). + use_latched_reference=True, use_ik_solution=True, ), LogRobotAction(), @@ -297,7 +315,10 @@ ee_to_follower_joints = RobotProcessorPipeline[tuple[RobotAction, RobotObservati InverseKinematicsRLStep( kinematics=follower_kinematics_solver, motor_names=list(follower.bus.motors.keys()), - initial_guess_current_joints=False, + # Seed IK from the follower's measured joints (not the previous IK + # output) so any residual drift in the commanded pose does not feed + # back into the next IK solve. + initial_guess_current_joints=True, ), LogRobotAction(), ], @@ -315,22 +336,40 @@ start_time = time.perf_counter() reset_follower_position(follower, np.array(reset_pose)) reset_follower_position(leader, np.array(reset_pose)) precise_sleep(5.0 - (time.perf_counter() - start_time)) -# time.sleep(10) -leader.bus.sync_write("Torque_Enable", 0) # Init rerun viewer # init_rerun(session_name="so100_so100_EE_teleop") transition = None +prev_intervening = False print("Starting teleop loop...") +print(" - Press SPACE to toggle intervention (leader controls follower)") +print(" - Press 's' for success, ESC for failure, 'r' to re-record") + while True: - print("New loop iteration") t0 = time.perf_counter() # Get robot observation robot_obs = follower.get_observation() + # Haptic follow: when not intervening, push follower joints onto the + # leader so the leader tracks the follower (torque on, low PID gains). + # When intervening, this call is a no-op inside SO101LeaderFollower. + leader.send_action(robot_obs) + + # Re-latch the EE reference each time the user starts a new intervention + # so the follower does not jump from a stale reference pose. + if leader.is_intervening and not prev_intervening: + ee_to_follower_joints.reset() + transition = None + prev_intervening = leader.is_intervening + + if not leader.is_intervening: + # Idle: follower holds its current pose; do not command anything. + precise_sleep(max(1.0 / FPS - (time.perf_counter() - t0), 0.0)) + continue + # Get teleop observation leader_joints_obs = leader.get_action()