admin/lerobot
1
0
Fork 0
mirror of https://github.com/huggingface/lerobot.git synced 2026-05-25 13:40:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

add full bimanual gravity comp

Browse Source
This commit is contained in:
croissant
2025-11-01 11:58:02 +01:00
parent 101fb02697
commit 5170862d23
4 changed files with 235 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/openarms/gravity_compensation_one_arm.py → examples/openarms/gravity_compensation.py
+20 -35
View File
@@ -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...")
examples/openarms/teleop_with_gravity_compensation.py
+56 -88
View File
@@ -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
src/lerobot/robots/openarms/openarms_follower.py
+53 -16
View File
@@ -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
src/lerobot/teleoperators/openarms/openarms_leader.py
+106
View File
@@ -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