Add gamepad IK control support and IK safety check #2409
Conversation
Signed-off-by: Mimi Liao <[email protected]>
There was a problem hiding this comment.
Pull Request Overview
This PR adds gamepad teleoperation support for the SO100/SO101 robot arm, enabling real-time control via a gamepad controller. The implementation follows the existing phone teleoperation pattern and includes safety mechanisms to prevent abrupt robot movements.
- Introduces a new
MapGamepadActionToRobotActionprocessor to transform gamepad input deltas into robot end-effector targets - Adds safety checks to the inverse kinematics step that validate IK solutions and limit joint velocities
- Provides a complete example script demonstrating gamepad-based teleoperation with conservative safety parameters
Reviewed Changes
Copilot reviewed 3 out of 3 changed files in this pull request and generated 5 comments.
| File | Description |
|---|---|
| src/lerobot/teleoperators/gamepad/gamepad_processor.py | New processor that maps gamepad delta inputs (delta_x, delta_y, delta_z) to robot action targets with clipping for smoother movement |
| src/lerobot/robots/so100_follower/robot_kinematic_processor.py | Adds debug logging and two safety checks to inverse kinematics: validates IK accuracy against position error threshold and limits maximum joint step size |
| examples/gamepad_to_so100/teleoperate.py | Example teleoperation script that chains gamepad processor with kinematic processors, includes conservative safety bounds and error handling |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| dy = np.clip(dy, -0.5, 0.5) | ||
| dz = np.clip(dz, -0.5, 0.5) | ||
|
|
||
| print(f"dx: {dx}, dy: {dy}, dz: {dz}") |
There was a problem hiding this comment.
Debug print statement should be removed before merging to production. Consider using a logging framework (e.g., logging.debug()) instead for better control over verbosity levels.
| desired = np.eye(4, dtype=float) | ||
| desired[:3, :3] = ref[:3, :3] @ r_abs | ||
| desired[:3, 3] = ref[:3, 3] + delta_p | ||
| print(f"ee_reference_position_and_delta: ref: {ref[:3, 3]}, delta_p: {delta_p}") |
There was a problem hiding this comment.
Debug print statement should be removed before merging to production. Consider using a logging framework (e.g., logging.debug()) instead for better control over verbosity levels.
| action["ee.wy"] = float(twist[1]) | ||
| action["ee.wz"] = float(twist[2]) | ||
|
|
||
| print(f'eeboundsandsafety: ee.x: {action["ee.x"]}, ee.y: {action["ee.y"]}, ee.z: {action["ee.z"]}') |
There was a problem hiding this comment.
Debug print statement should be removed before merging to production. Consider using a logging framework (e.g., logging.debug()) instead for better control over verbosity levels.
| MAX_POSITION_ERROR = 0.05 # 5cm maximum position error | ||
| MAX_JOINT_STEP = 30.0 # 30 degrees maximum single step |
There was a problem hiding this comment.
Magic numbers should be defined as named constants at the class or module level for better maintainability. Consider moving MAX_POSITION_ERROR and MAX_JOINT_STEP to class attributes or constants defined at the top of the class/file.
| print(f"⚠️ IK FAILED: Position error {pos_error*1000:.1f}mm > {MAX_POSITION_ERROR*1000:.1f}mm threshold") | ||
| print(f" Desired: [{x:.3f}, {y:.3f}, {z:.3f}]") | ||
| print(f" Achieved: [{t_achieved[0,3]:.3f}, {t_achieved[1,3]:.3f}, {t_achieved[2,3]:.3f}]") | ||
| print(f" 🛑 SAFETY: Keeping current position to avoid damage!") | ||
| q_target = self.q_curr # SAFE: Don't move if IK failed | ||
|
|
||
| elif max_joint_delta > MAX_JOINT_STEP: | ||
| print(f"⚠️ LARGE MOVEMENT DETECTED: {max_joint_delta:.1f}° > {MAX_JOINT_STEP:.1f}° threshold") | ||
| print(f" 🛑 SAFETY: Capping joint movement to prevent violent shake!") | ||
| # Clip each joint to maximum step size | ||
| for i in range(len(self.motor_names)): | ||
| delta = q_target[i] - self.q_curr[i] | ||
| if abs(delta) > MAX_JOINT_STEP: | ||
| q_target[i] = self.q_curr[i] + np.sign(delta) * MAX_JOINT_STEP |
There was a problem hiding this comment.
Debug print statements should be removed before merging to production. Consider using a logging framework (e.g., logging.warning()) for safety-critical messages that need to persist, or logging.debug() for verbose diagnostic output.
| - "gripper_vel" (float): zero (gripper handled later; held by velocity=0) | ||
| """ | ||
|
|
||
| def action(self, action: RobotAction) -> RobotAction: |
There was a problem hiding this comment.
Hi, is this similar to MapDeltaActionToRobotActionStep?
2 participants
What this does
This PR solved below issues:
Gamepad teleoperation is not supported by
lerobot-teleoperate: The default command only supports leader-follower joint-to-joint mapping, lacking the IK pipeline required for gamepad delta control.IK solver lacks safety validation: When the target end-effector position is unreachable, the IK solver returns invalid joint configurations, causing the robot to shake violently and potentially break hardware.
How it was tested
How to checkout & try? (for the reviewer)
Examples: