From 020fc12ead953f4e3efd6476edf869ac7005d322 Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Tue, 9 Dec 2025 14:05:36 +0100 Subject: [PATCH] tests on bimanual teleop --- examples/openarms/bilateral.py | 140 ++++ examples/openarms/gravity_compensation.py | 27 +- .../openarms/openarm_bimanual_pybullet.urdf | 618 ++++++++++++++++++ examples/openarms/test_r_arm.py | 76 +++ .../openarms/config_openarms_leader.py | 4 +- 5 files changed, 848 insertions(+), 17 deletions(-) create mode 100644 examples/openarms/bilateral.py create mode 100644 examples/openarms/openarm_bimanual_pybullet.urdf create mode 100644 examples/openarms/test_r_arm.py diff --git a/examples/openarms/bilateral.py b/examples/openarms/bilateral.py new file mode 100644 index 000000000..ae3283f88 --- /dev/null +++ b/examples/openarms/bilateral.py @@ -0,0 +1,140 @@ +import time +import numpy as np +import pinocchio as pin +from os.path import dirname + +from lerobot.teleoperators.openarms.openarms_leader import OpenArmsLeader +from lerobot.teleoperators.openarms.config_openarms_leader import OpenArmsLeaderConfig + + +same_direction = {"joint_4", "gripper"} + +idx = { + "joint_1": 0, + "joint_2": 1, + "joint_3": 2, + "joint_4": 3, + "joint_5": 4, + "joint_6": 5, + "joint_7": 6, + "gripper": 7, +} + +# joints to freeze +frozen = {"joint_6", "joint_7", "gripper"} +initial_pose = {} + + +def pos_deg(rob, obs): + out = {} + for side in ("left", "right"): + for m in getattr(rob, f"bus_{side}").motors: + k = f"{side}_{m}.pos" + if k in obs: + out[f"{side}_{m}"] = obs[k] + return out + + +def vel_rad(rob, obs): + out = {} + for side in ("left", "right"): + for m in getattr(rob, f"bus_{side}").motors: + k = f"{side}_{m}.vel" + out[f"{side}_{m}"] = np.deg2rad(obs.get(k, 0.0)) + return out + + +def main(): + cfg = OpenArmsLeaderConfig( + port_left="can0", + port_right="can1", + can_interface="socketcan", + id="openarms_bilateral", + manual_control=False, + ) + + rob = OpenArmsLeader(cfg) + rob.connect(calibrate=True) + + urdf = "/home/yope/Documents/lerobot_g1_integration/openarm_description/openarm_bimanual_pybullet.urdf" + rob.pin_robot = pin.RobotWrapper.BuildFromURDF(urdf, dirname(urdf)) + rob.pin_robot.data = rob.pin_robot.model.createData() + + dt = 0.005 + grav = 1.0 + fric = 0.3 + + # capture initial pose to freeze selected joints later + obs0 = rob.get_action() + for side in ("left", "right"): + for m in getattr(rob, f"bus_{side}").motors: + key = f"{side}_{m}.pos" + if key in obs0 and m in frozen: + initial_pose[f"{side}_{m}"] = obs0[key] + + try: + while True: + obs = rob.get_action() + + pdeg = pos_deg(rob, obs) + prad = {k: np.deg2rad(v) for k, v in pdeg.items()} + vrad = vel_rad(rob, obs) + + tau_g = rob._gravity_from_q(prad) + tau_f = rob._friction_from_velocity(vrad, friction_scale=fric) + + # bilateral midpoint calculation + cmd = {} + for m in rob.bus_right.motors: + kl = f"left_{m}.pos" + kr = f"right_{m}.pos" + if kl not in obs or kr not in obs: + continue + + ql = obs[kl] + qr = obs[kr] + + if m in same_direction: + qmid = 0.5 * (ql + qr) + else: + qmid = 0.5 * (ql - qr) + + # assign midpoint for both + cmd[f"left_{m}"] = qmid + cmd[f"right_{m}"] = qmid if m in same_direction else -qmid + + # override midpoint with frozen values + for key, val in initial_pose.items(): + cmd[key] = val + + # single mit control call + for side in ("left", "right"): + bus = getattr(rob, f"bus_{side}") + for m in bus.motors: + base_key = f"{side}_{m}" + kp = float(cfg.position_kp[idx[m]]) + kd = float(cfg.position_kd[idx[m]]) + torque = tau_g.get(base_key, 0.0) * grav + tau_f.get(base_key, 0.0) + pos_cmd = cmd.get(base_key, pdeg.get(base_key, 0.0)) + + bus._mit_control( + motor=m, + kp=kp, + kd=kd, + position_degrees=pos_cmd, + velocity_deg_per_sec=0.0, + torque=torque, + ) + + time.sleep(dt) + + except KeyboardInterrupt: + pass + + rob.bus_left.disable_torque() + rob.bus_right.disable_torque() + rob.disconnect() + + +if __name__ == "__main__": + main() diff --git a/examples/openarms/gravity_compensation.py b/examples/openarms/gravity_compensation.py index f6ef91d2a..b0e4d759e 100755 --- a/examples/openarms/gravity_compensation.py +++ b/examples/openarms/gravity_compensation.py @@ -3,28 +3,26 @@ import numpy as np import pinocchio as pin from os.path import join, dirname, exists, expanduser -from lerobot.robots.openarms.openarms_follower import OpenArmsFollower -from lerobot.robots.openarms.config_openarms_follower import OpenArmsFollowerConfig +from lerobot.teleoperators.openarms.openarms_leader import OpenArmsLeader +from lerobot.teleoperators.openarms.config_openarms_leader import OpenArmsLeaderConfig def main() -> None: - config = OpenArmsFollowerConfig( + config = OpenArmsLeaderConfig( port_left="can0", port_right="can1", can_interface="socketcan", - id="openarms_follower", - disable_torque_on_disconnect=True, - max_relative_target=5.0, + id="openarms_leader", + manual_control=False, # Enable torque control for gravity compensation ) print("Initializing robot...") - follower = OpenArmsFollower(config) + follower = OpenArmsLeader(config) follower.connect(calibrate=True) # Load URDF for Pinocchio dynamics - urdf_path = "/home/croissant/Documents/openarm_description/openarm_bimanual_pybullet.urdf" - + urdf_path = "/home/yope/Documents/lerobot_g1_integration/openarm_description/openarm_bimanual_pybullet.urdf" pin_robot = pin.RobotWrapper.BuildFromURDF(urdf_path, dirname(urdf_path)) pin_robot.data = pin_robot.model.createData() print(f"✓ Loaded Pinocchio model with {pin_robot.nq} DoFs") @@ -50,7 +48,7 @@ def main() -> None: loop_start = time.perf_counter() # Get current joint positions from robot - obs = follower.get_observation() + obs = follower.get_action() # Extract positions in degrees positions_deg = {} @@ -71,8 +69,7 @@ def main() -> None: # 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) @@ -88,10 +85,10 @@ def main() -> None: torque=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) @@ -106,7 +103,7 @@ def main() -> None: velocity_deg_per_sec=0.0, torque=torque ) - + # Measure loop time loop_end = time.perf_counter() loop_time = loop_end - loop_start diff --git a/examples/openarms/openarm_bimanual_pybullet.urdf b/examples/openarms/openarm_bimanual_pybullet.urdf new file mode 100644 index 000000000..f3e885a50 --- /dev/null +++ b/examples/openarms/openarm_bimanual_pybullet.urdf @@ -0,0 +1,618 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/openarms/test_r_arm.py b/examples/openarms/test_r_arm.py new file mode 100644 index 000000000..523211ef9 --- /dev/null +++ b/examples/openarms/test_r_arm.py @@ -0,0 +1,76 @@ +import time +import math +import numpy as np +from lerobot.robots.openarms.openarms_follower import OpenArmsFollower +from lerobot.robots.openarms.config_openarms_follower import OpenArmsFollowerConfig + + +def main(): + cfg = OpenArmsFollowerConfig( + port_left="can0", + port_right="can1", + can_interface="socketcan", + id="openarms_test", + manual_control=True, # direct position control + ) + + print('connecting...') + rob = OpenArmsFollower(cfg) + rob.connect(calibrate=True) + + # disable left torque fully — keep it still + rob.bus_left.disable_torque() + + # desired angular sweep = 1/4 of current joint range + sweep_deg = 20.0 # tweak if you want bigger movement + + # frequency of movement + hz = 100.0 + dt = 1.0 / hz + move_time = 1.0 # seconds per joint + + print('starting right–arm joint test…') + print('support the arm and keep clear') + + time.sleep(1.0) + + # iterate motors except gripper + for motor in rob.bus_right.motors: + if motor == 'gripper': + continue + + print(f'testing {motor} on right arm...') + start = time.time() + + # read current position as center + obs = rob.get_action() + key = f'right_{motor}.pos' + center = obs.get(key, 0.0) + + t = 0.0 + while time.time() - start < move_time: + offset = sweep_deg * math.sin(2 * math.pi * t) + pos_cmd = center + offset + + rob.bus_right._mit_control( + motor=motor, + kp=3.0, # some stiffness so it tracks well + kd=0.2, + position_degrees=pos_cmd, + velocity_deg_per_sec=0.0, + torque=0.0 + ) + + t += dt + time.sleep(dt) + + print(f'done {motor}') + + print('\nall right–arm joints tested') + print('disabling torque…') + rob.bus_right.disable_torque() + rob.disconnect() + + +if __name__ == '__main__': + main() diff --git a/src/lerobot/teleoperators/openarms/config_openarms_leader.py b/src/lerobot/teleoperators/openarms/config_openarms_leader.py index 094ecf0a9..2b1c20940 100644 --- a/src/lerobot/teleoperators/openarms/config_openarms_leader.py +++ b/src/lerobot/teleoperators/openarms/config_openarms_leader.py @@ -65,8 +65,8 @@ class OpenArmsLeaderConfig(TeleoperatorConfig): # MIT control parameters (used when manual_control=False for torque control) # List of 8 values: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7, gripper] - position_kp: list[float] = field(default_factory=lambda: [240.0, 240.0, 240.0, 240.0, 24.0, 31.0, 25.0, 16.0]) - position_kd: list[float] = field(default_factory=lambda: [3.0, 3.0, 3.0, 3.0, 0.2, 0.2, 0.2, 0.2]) + position_kp: list[float] = field(default_factory=lambda: [100.0, 100.0, 100.0, 48.0, 24.0, 31.0, 25.0, 16.0]) + position_kd: list[float] = field(default_factory=lambda: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) # Damping gains for stability when applying torque compensation (gravity/friction) # Used when kp=0 and only torque is applied