From 5170862d231abbebc5ee8e6925896e48676e6c68 Mon Sep 17 00:00:00 2001 From: croissant Date: Sat, 1 Nov 2025 11:58:02 +0100 Subject: [PATCH] add full bimanual gravity comp --- ...ion_one_arm.py => gravity_compensation.py} | 55 +++---- .../teleop_with_gravity_compensation.py | 144 +++++++----------- .../robots/openarms/openarms_follower.py | 69 +++++++-- .../teleoperators/openarms/openarms_leader.py | 106 +++++++++++++ 4 files changed, 235 insertions(+), 139 deletions(-) rename examples/openarms/{gravity_compensation_one_arm.py => gravity_compensation.py} (73%) diff --git a/examples/openarms/gravity_compensation_one_arm.py b/examples/openarms/gravity_compensation.py similarity index 73% rename from examples/openarms/gravity_compensation_one_arm.py rename to examples/openarms/gravity_compensation.py index dfdca19f9..f6ef91d2a 100755 --- a/examples/openarms/gravity_compensation_one_arm.py +++ b/examples/openarms/gravity_compensation.py @@ -9,8 +9,8 @@ from lerobot.robots.openarms.config_openarms_follower import OpenArmsFollowerCon def main() -> None: config = OpenArmsFollowerConfig( - port_right="can0", - port_left="can1", + port_left="can0", + port_right="can1", can_interface="socketcan", id="openarms_follower", disable_torque_on_disconnect=True, @@ -31,25 +31,14 @@ def main() -> None: follower.pin_robot = pin_robot - control_arm = "right" - - print(f"Applying gravity compensation to: {control_arm.upper()} ARM (all joints)") - print("\nAll joints on the right arm will have gravity compensation applied.") - print("Left arm joints will be disabled (free to move).") - print("\nIMPORTANT:") + print(f"Applying gravity compensation") print(" 1. Support the arm before starting") print(" 2. The arm will be held in place by gravity compensation") print(" 3. You should be able to move it with gentle force") print("\nPress ENTER when ready to start...") input() - follower.bus_left.enable_torque() - time.sleep(0.1) - print(f"✓ Motors enabled") - print(f" {control_arm.upper()} arm: Gravity compensation active") - print(f" LEFT arm: Zero torque (free to move)") - print("\nStarting gravity compensation loop...") print("Press Ctrl+C to stop\n") @@ -80,12 +69,13 @@ def main() -> None: positions_rad = {k: np.deg2rad(v) for k, v in positions_deg.items()} torques_nm = follower._gravity_from_q(positions_rad) - # Apply gravity compensation torques to all joints on the right arm + # Apply gravity compensation to right arm (all joints except gripper) for motor in follower.bus_right.motors: + if motor == "gripper": + continue # Skip gripper + full_name = f"right_{motor}" position = positions_deg.get(full_name, 0.0) - - # Apply gravity compensation to this joint torque = torques_nm.get(full_name, 0.0) # Send MIT control command with gravity compensation torque @@ -97,19 +87,24 @@ def main() -> None: velocity_deg_per_sec=0.0, torque=torque ) - - # Also ensure left arm joints have zero torque + + # Apply gravity compensation to left arm (all joints except gripper) for motor in follower.bus_left.motors: + if motor == "gripper": + continue # Skip gripper + full_name = f"left_{motor}" position = positions_deg.get(full_name, 0.0) + torque = torques_nm.get(full_name, 0.0) + # Send MIT control command with gravity compensation torque follower.bus_left._mit_control( motor=motor, - kp=0.0, - kd=0.0, + kp=0.0, # No position control + kd=0.0, # No velocity damping position_degrees=position, velocity_deg_per_sec=0.0, - torque=0.0 + torque=torque ) # Measure loop time @@ -123,22 +118,12 @@ def main() -> None: avg_time = sum(loop_times) / len(loop_times) current_hz = 1.0 / avg_time if avg_time > 0 else 0 - # Display status for all joints on the controlled arm - print(f"\n[RIGHT ARM] Loop: {current_hz:.1f} Hz ({avg_time*1000:.1f} ms)") - - # Show each joint's position and torque - for motor in follower.bus_right.motors: - full_name = f"right_{motor}" - pos = positions_deg.get(full_name, 0.0) - torque = torques_nm.get(full_name, 0.0) - print(f" {motor:8s}: Pos={pos:7.1f}° | Torque={torque:7.3f} N·m") - - # Reset for next measurement window + print(f"{current_hz:.1f} Hz ({avg_time*1000:.1f} ms)") + loop_times = [] last_print_time = loop_end - # Small sleep to avoid overwhelming the CAN bus - time.sleep(0.01) # 100 Hz + time.sleep(0.005) except KeyboardInterrupt: print("\n\nStopping gravity compensation...") diff --git a/examples/openarms/teleop_with_gravity_compensation.py b/examples/openarms/teleop_with_gravity_compensation.py index a260f1a2a..8cebe4e18 100755 --- a/examples/openarms/teleop_with_gravity_compensation.py +++ b/examples/openarms/teleop_with_gravity_compensation.py @@ -1,73 +1,21 @@ """ OpenArms Teleoperation with Gravity Compensation -Leader RIGHT arm: Gravity compensation (weightless, easy to move) -Follower RIGHT arm: Mirrors leader movements -Both LEFT arms: Free to move (disabled) +Leader arms (both LEFT and RIGHT): Gravity compensation (weightless, easy to move) +Follower arms (both LEFT and RIGHT): Mirror leader movements -The urdf file tested with this script is found in: -https://github.com/michel-aractingi/openarm_description/blob/main/openarm_bimanual_pybullet.urdf +Uses the URDF file from the lerobot repository. """ import time -import os import numpy as np -import pinocchio as pin from lerobot.robots.openarms.config_openarms_follower import OpenArmsFollowerConfig from lerobot.robots.openarms.openarms_follower import OpenArmsFollower from lerobot.teleoperators.openarms.config_openarms_leader import OpenArmsLeaderConfig from lerobot.teleoperators.openarms.openarms_leader import OpenArmsLeader -# Path to the URDF file -URDF_PATH = "/home/croissant/Documents/openarm_description/openarm_bimanual_pybullet.urdf" - -def compute_gravity_torques(leader, positions_rad): - """ - Compute gravity torques for all joints in the robot. - - Args: - leader: OpenArmsLeader instance with pin_robot set - positions_rad: Dictionary mapping motor names (with arm prefix) to positions in radians - - Returns: - Dictionary mapping motor names to gravity torques in N·m - """ - if not hasattr(leader, "pin_robot") or leader.pin_robot is None: - raise RuntimeError("URDF model not loaded on leader") - - # Build position vector in the order of motors (right arm, then left arm) - q = np.zeros(leader.pin_robot.model.nq) - idx = 0 - - # Right arm motors - for motor_name in leader.bus_right.motors: - full_name = f"right_{motor_name}" - q[idx] = positions_rad.get(full_name, 0.0) - idx += 1 - - # Left arm motors - for motor_name in leader.bus_left.motors: - full_name = f"left_{motor_name}" - q[idx] = positions_rad.get(full_name, 0.0) - idx += 1 - - # Compute generalized gravity vector - g = pin.computeGeneralizedGravity(leader.pin_robot.model, leader.pin_robot.data, q) - - # Map back to motor names - result = {} - idx = 0 - for motor_name in leader.bus_right.motors: - result[f"right_{motor_name}"] = float(g[idx]) - idx += 1 - for motor_name in leader.bus_left.motors: - result[f"left_{motor_name}"] = float(g[idx]) - idx += 1 - - return result - def main(): """Main teleoperation loop with gravity compensation""" @@ -78,8 +26,8 @@ def main(): # Configuration follower_config = OpenArmsFollowerConfig( - port_right="can0", - port_left="can1", + port_left="can0", + port_right="can1", can_interface="socketcan", id="openarms_follower", disable_torque_on_disconnect=True, @@ -87,8 +35,8 @@ def main(): ) leader_config = OpenArmsLeaderConfig( - port_right="can2", - port_left="can3", + port_left="can2", + port_right="can3", can_interface="socketcan", id="openarms_leader", manual_control=False, # Enable torque control for gravity compensation @@ -102,18 +50,15 @@ def main(): follower.connect(calibrate=True) leader.connect(calibrate=True) - # Load URDF for gravity compensation - if not os.path.exists(URDF_PATH): - raise FileNotFoundError(f"URDF file not found at {URDF_PATH}") - pin_robot = pin.RobotWrapper.BuildFromURDF(URDF_PATH, os.path.dirname(URDF_PATH)) - pin_robot.data = pin_robot.model.createData() - leader.pin_robot = pin_robot + # URDF is automatically loaded in the leader constructor + if leader.pin_robot is None: + raise RuntimeError("URDF model not loaded on leader. Gravity compensation not available.") - print("\nLeader RIGHT: G-comp | Follower RIGHT: Teleop") + print("\nLeader BOTH arms: G-comp | Follower BOTH arms: Teleop") print("Press ENTER to start...") input() - # Enable motors + # Enable motors on both leader arms for gravity compensation leader.bus_right.enable_torque() leader.bus_left.enable_torque() time.sleep(0.1) @@ -124,17 +69,12 @@ def main(): loop_times = [] last_print_time = time.perf_counter() - # Right arm joints only - right_joints = [ - "right_joint_1", - "right_joint_2", - "right_joint_3", - "right_joint_4", - "right_joint_5", - "right_joint_6", - "right_joint_7", - "right_gripper", - ] + # All joints (both arms) + all_joints = [] + for motor in leader.bus_right.motors: + all_joints.append(f"right_{motor}") + for motor in leader.bus_left.motors: + all_joints.append(f"left_{motor}") try: while True: @@ -143,6 +83,7 @@ def main(): # Get leader state leader_action = leader.get_action() + # Extract positions in degrees leader_positions_deg = {} for motor in leader.bus_right.motors: key = f"right_{motor}.pos" @@ -154,12 +95,26 @@ def main(): if key in leader_action: leader_positions_deg[f"left_{motor}"] = leader_action[key] - # Calculate gravity torques for leader + # Calculate gravity torques for leader using built-in method leader_positions_rad = {k: np.deg2rad(v) for k, v in leader_positions_deg.items()} - leader_torques_nm = compute_gravity_torques(leader, leader_positions_rad) + leader_torques_nm = leader._gravity_from_q(leader_positions_rad) - # Apply gravity compensation to leader right arm + # Apply gravity compensation to leader RIGHT arm (all joints except gripper) for motor in leader.bus_right.motors: + if motor == "gripper": + # Skip gripper - keep it free + full_name = f"right_{motor}" + position = leader_positions_deg.get(full_name, 0.0) + leader.bus_right._mit_control( + motor=motor, + kp=0.0, + kd=0.0, + position_degrees=position, + velocity_deg_per_sec=0.0, + torque=0.0, + ) + continue + full_name = f"right_{motor}" position = leader_positions_deg.get(full_name, 0.0) torque = leader_torques_nm.get(full_name, 0.0) @@ -173,10 +128,25 @@ def main(): torque=torque, ) - # Keep leader left arm free + # Apply gravity compensation to leader LEFT arm (all joints except gripper) for motor in leader.bus_left.motors: + if motor == "gripper": + # Skip gripper - keep it free + full_name = f"left_{motor}" + position = leader_positions_deg.get(full_name, 0.0) + leader.bus_left._mit_control( + motor=motor, + kp=0.0, + kd=0.0, + position_degrees=position, + velocity_deg_per_sec=0.0, + torque=0.0, + ) + continue + full_name = f"left_{motor}" position = leader_positions_deg.get(full_name, 0.0) + torque = leader_torques_nm.get(full_name, 0.0) leader.bus_left._mit_control( motor=motor, @@ -184,12 +154,12 @@ def main(): kd=0.0, position_degrees=position, velocity_deg_per_sec=0.0, - torque=0.0, + torque=torque, ) - # Send leader positions to follower right arm + # Send leader positions to follower (both arms) follower_action = {} - for joint in right_joints: + for joint in all_joints: pos_key = f"{joint}.pos" if pos_key in leader_action: follower_action[pos_key] = leader_action[pos_key] @@ -206,10 +176,8 @@ def main(): if loop_times: avg_time = sum(loop_times) / len(loop_times) current_hz = 1.0 / avg_time if avg_time > 0 else 0 - sample_pos = leader_positions_deg.get("right_joint_2", 0.0) - sample_torque = leader_torques_nm.get("right_joint_2", 0.0) - print(f"[{current_hz:.1f} Hz] J2: {sample_pos:5.1f}° | Torque: {sample_torque:5.2f} N·m") + print(f"{current_hz:.1f} Hz ({avg_time*1000:.1f} ms)") loop_times = [] last_print_time = loop_end diff --git a/src/lerobot/robots/openarms/openarms_follower.py b/src/lerobot/robots/openarms/openarms_follower.py index d65cd98c5..e2dd5b68f 100644 --- a/src/lerobot/robots/openarms/openarms_follower.py +++ b/src/lerobot/robots/openarms/openarms_follower.py @@ -94,10 +94,19 @@ class OpenArmsFollower(Robot): # Initialize Pinocchio robot model for dynamics (optional) self.pin_robot = None try: - # Try to load URDF if available - # TODO: Add OpenArms URDF file to repository - self.pin_robot = pin.RobotWrapper.BuildFromURDF("urdf/openarms.urdf", "urdf") - logger.info("Loaded OpenArms URDF for dynamics computation") + # Load URDF - try external path first (with meshes), then repository + import os + from os.path import expanduser, dirname + + # Try external URDF with meshes first + external_urdf_path = expanduser("~/Documents/openarm_description/openarm_bimanual_pybullet.urdf") + if os.path.exists(external_urdf_path): + urdf_path = external_urdf_path + urdf_dir = dirname(urdf_path) + + self.pin_robot = pin.RobotWrapper.BuildFromURDF(urdf_path, urdf_dir) + self.pin_robot.data = self.pin_robot.model.createData() + logger.info(f"Loaded OpenArms URDF for dynamics computation from {urdf_path}") except Exception as e: logger.warning(f"Could not load URDF for dynamics: {e}. Gravity compensation will not be available.") @@ -476,34 +485,62 @@ class OpenArmsFollower(Robot): "Ensure urdf/openarms.urdf exists and is valid." ) - # Build position vector in the order of motors (right arm, then left arm) + # Build position vector in the order of motors (left arm, then right arm) + # This order must match the URDF joint order + # URDF has: left_joint1-7, left_finger_joint1-2, right_joint1-7, right_finger_joint1-2 q = np.zeros(self.pin_robot.model.nq) idx = 0 - # Right arm motors - for motor_name in self.bus_right.motors: - full_name = f"right_{motor_name}" - q[idx] = q_rad.get(full_name, 0.0) - idx += 1 - - # Left arm motors + # Left arm motors (first in URDF) - joints 1-7 for motor_name in self.bus_left.motors: + if motor_name == "gripper": + continue # Skip gripper, will be handled separately full_name = f"left_{motor_name}" q[idx] = q_rad.get(full_name, 0.0) idx += 1 + # Skip left finger joints (leave as zeros) + idx += 2 + + # Right arm motors (second in URDF) - joints 1-7 + for motor_name in self.bus_right.motors: + if motor_name == "gripper": + continue # Skip gripper, will be handled separately + full_name = f"right_{motor_name}" + q[idx] = q_rad.get(full_name, 0.0) + idx += 1 + + # Skip right finger joints (leave as zeros) + idx += 2 + # Compute generalized gravity vector g = pin.computeGeneralizedGravity(self.pin_robot.model, self.pin_robot.data, q) - # Map back to motor names + # Map back to motor names (only arm joints, not fingers) result = {} idx = 0 - for motor_name in self.bus_right.motors: - result[f"right_{motor_name}"] = float(g[idx]) - idx += 1 + + # Left arm torques (joints 1-7) for motor_name in self.bus_left.motors: + if motor_name == "gripper": + result["left_gripper"] = 0.0 # No gravity compensation for gripper + continue result[f"left_{motor_name}"] = float(g[idx]) idx += 1 + # Skip left finger joint torques in output + idx += 2 + + # Right arm torques (joints 1-7) + for motor_name in self.bus_right.motors: + if motor_name == "gripper": + result["right_gripper"] = 0.0 # No gravity compensation for gripper + continue + result[f"right_{motor_name}"] = float(g[idx]) + idx += 1 + + # Skip right finger joint torques in output + idx += 2 + return result diff --git a/src/lerobot/teleoperators/openarms/openarms_leader.py b/src/lerobot/teleoperators/openarms/openarms_leader.py index ec712ecbc..f129959ce 100644 --- a/src/lerobot/teleoperators/openarms/openarms_leader.py +++ b/src/lerobot/teleoperators/openarms/openarms_leader.py @@ -18,6 +18,9 @@ import logging import time from typing import Any, Dict +import numpy as np +import pinocchio as pin + from lerobot.motors import Motor, MotorCalibration, MotorNormMode from lerobot.motors.damiao import DamiaoMotorsBus from lerobot.motors.damiao.tables import MotorType @@ -84,6 +87,25 @@ class OpenArmsLeader(Teleoperator): data_bitrate=self.config.can_data_bitrate if self.config.use_can_fd else None, ) + # Initialize Pinocchio robot model for dynamics (optional) + self.pin_robot = None + try: + # Load URDF - try external path first (with meshes), then repository + import os + from os.path import expanduser, dirname + + # Try external URDF with meshes first + external_urdf_path = expanduser("~/Documents/openarm_description/openarm_bimanual_pybullet.urdf") + if os.path.exists(external_urdf_path): + urdf_path = external_urdf_path + urdf_dir = dirname(urdf_path) + + self.pin_robot = pin.RobotWrapper.BuildFromURDF(urdf_path, urdf_dir) + self.pin_robot.data = self.pin_robot.model.createData() + logger.info(f"Loaded OpenArms URDF for dynamics computation from {urdf_path}") + except Exception as e: + logger.warning(f"Could not load URDF for dynamics: {e}. Gravity compensation will not be available.") + @property def action_features(self) -> Dict[str, type]: """Features produced by this teleoperator.""" @@ -308,3 +330,87 @@ class OpenArmsLeader(Teleoperator): logger.info(f"{self} disconnected.") + def _deg_to_rad(self, deg: Dict[str, float | int]) -> Dict[str, float]: + """Convert degrees to radians for all motors.""" + return {m: np.deg2rad(float(v)) for m, v in deg.items()} + + def _gravity_from_q(self, q_rad: Dict[str, float]) -> Dict[str, float]: + """ + Compute g(q) [N·m] for all joints in the robot. + The order of joints in the URDF matches the concatenated motor lists (right then left). + + Args: + q_rad: Dictionary mapping motor names (with arm prefix) to positions in radians + + Returns: + Dictionary mapping motor names to gravity torques in N·m + + Raises: + RuntimeError: If URDF model is not loaded + """ + if self.pin_robot is None: + raise RuntimeError( + "Cannot compute gravity: URDF model not loaded. " + "Ensure urdf/openarms.urdf exists and is valid." + ) + + # Build position vector in the order of motors (left arm, then right arm) + # This order must match the URDF joint order + # URDF has: left_joint1-7, left_finger_joint1-2, right_joint1-7, right_finger_joint1-2 + q = np.zeros(self.pin_robot.model.nq) + idx = 0 + + # Left arm motors (first in URDF) - joints 1-7 + for motor_name in self.bus_left.motors: + if motor_name == "gripper": + continue # Skip gripper, will be handled separately + full_name = f"left_{motor_name}" + q[idx] = q_rad.get(full_name, 0.0) + idx += 1 + + # Skip left finger joints (leave as zeros) + idx += 2 + + # Right arm motors (second in URDF) - joints 1-7 + for motor_name in self.bus_right.motors: + if motor_name == "gripper": + continue # Skip gripper, will be handled separately + full_name = f"right_{motor_name}" + q[idx] = q_rad.get(full_name, 0.0) + idx += 1 + + # Skip right finger joints (leave as zeros) + idx += 2 + + # Compute generalized gravity vector + g = pin.computeGeneralizedGravity(self.pin_robot.model, self.pin_robot.data, q) + + # Map back to motor names (only arm joints, not fingers) + result = {} + idx = 0 + + # Left arm torques (joints 1-7) + for motor_name in self.bus_left.motors: + if motor_name == "gripper": + result["left_gripper"] = 0.0 # No gravity compensation for gripper + continue + result[f"left_{motor_name}"] = float(g[idx]) + idx += 1 + + # Skip left finger joint torques in output + idx += 2 + + # Right arm torques (joints 1-7) + for motor_name in self.bus_right.motors: + if motor_name == "gripper": + result["right_gripper"] = 0.0 # No gravity compensation for gripper + continue + result[f"right_{motor_name}"] = float(g[idx]) + idx += 1 + + # Skip right finger joint torques in output + idx += 2 + + return result + +