add full bimanual gravity comp

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="can0",
-        port_left="can1",
+        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")
-    
-    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("  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,
+                    kp=0.0,  # No position control
-                    kd=0.0,
+                    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"{current_hz:.1f} Hz ({avg_time*1000:.1f} ms)")
-                    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
                     loop_times = []
                     last_print_time = loop_end
             
-            # Small sleep to avoid overwhelming the CAN bus
+            time.sleep(0.005)
-            time.sleep(0.01)  # 100 Hz
             
     except KeyboardInterrupt:
         print("\n\nStopping gravity compensation...")

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)
+Leader arms (both LEFT and RIGHT): Gravity compensation (weightless, easy to move)
-Follower RIGHT arm: Mirrors leader movements
+Follower arms (both LEFT and RIGHT): Mirror leader movements
-Both LEFT arms: Free to move (disabled)
 
-The urdf file tested with this script is found in:
+Uses the URDF file from the lerobot repository.
-https://github.com/michel-aractingi/openarm_description/blob/main/openarm_bimanual_pybullet.urdf
 """
 
 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="can0",
-        port_left="can1",
+        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="can2",
-        port_left="can3",
+        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
+    # URDF is automatically loaded in the leader constructor
-    if not os.path.exists(URDF_PATH):
+    if leader.pin_robot is None:
-        raise FileNotFoundError(f"URDF file not found at {URDF_PATH}")
+        raise RuntimeError("URDF model not loaded on leader. Gravity compensation not available.")
-    pin_robot = pin.RobotWrapper.BuildFromURDF(URDF_PATH, os.path.dirname(URDF_PATH))
-    pin_robot.data = pin_robot.model.createData()
-    leader.pin_robot = pin_robot
 
-    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
+    # All joints (both arms)
-    right_joints = [
+    all_joints = []
-        "right_joint_1",
+    for motor in leader.bus_right.motors:
-        "right_joint_2",
+        all_joints.append(f"right_{motor}")
-        "right_joint_3",
+    for motor in leader.bus_left.motors:
-        "right_joint_4",
+        all_joints.append(f"left_{motor}")
-        "right_joint_5",
-        "right_joint_6",
-        "right_joint_7",
-        "right_gripper",
-    ]
 
     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

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
+            # Load URDF - try external path first (with meshes), then repository
-            # TODO: Add OpenArms URDF file to repository
+            import os
-            self.pin_robot = pin.RobotWrapper.BuildFromURDF("urdf/openarms.urdf", "urdf")
+            from os.path import expanduser, dirname
-            logger.info("Loaded OpenArms URDF for dynamics computation")
+            
+            # 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
+        # Left arm motors (first in URDF) - joints 1-7
-        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
         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])
+        # Left arm torques (joints 1-7)
-            idx += 1
         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
     

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
+
+