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

Update test_reachy2.py

Browse Source
This commit is contained in:
glannuzel
2025-08-26 16:04:45 +02:00
parent 56807e772f
commit 31117fcf94
1 changed files with 60 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/robots/test_reachy2.py
+60 -24
View File
@@ -28,27 +28,27 @@ from lerobot.robots.reachy2 import (
# {lerobot_keys: reachy2_sdk_keys} # {lerobot_keys: reachy2_sdk_keys}
REACHY2_JOINTS = { REACHY2_JOINTS = {
"head.neck.yaw": "neck_yaw.pos", "neck_yaw.pos": "head.neck.yaw",
"head.neck.pitch": "neck_pitch.pos", "neck_pitch.pos": "head.neck.pitch",
"head.neck.roll": "neck_roll.pos", "neck_roll.pos": "head.neck.roll",
"head.l_antenna": "l_antenna.pos", "l_antenna.pos": "head.l_antenna",
"head.r_antenna": "r_antenna.pos", "r_antenna.pos": "head.r_antenna",
"r_arm.shoulder.pitch": "r_shoulder_pitch.pos", "r_shoulder_pitch.pos": "r_arm.shoulder.pitch",
"r_arm.shoulder.roll": "r_shoulder_roll.pos", "r_shoulder_roll.pos": "r_arm.shoulder.roll",
"r_arm.elbow.yaw": "r_elbow_yaw.pos", "r_elbow_yaw.pos": "r_arm.elbow.yaw",
"r_arm.elbow.pitch": "r_elbow_pitch.pos", "r_elbow_pitch.pos": "r_arm.elbow.pitch",
"r_arm.wrist.roll": "r_wrist_roll.pos", "r_wrist_roll.pos": "r_arm.wrist.roll",
"r_arm.wrist.pitch": "r_wrist_pitch.pos", "r_wrist_pitch.pos": "r_arm.wrist.pitch",
"r_arm.wrist.yaw": "r_wrist_yaw.pos", "r_wrist_yaw.pos": "r_arm.wrist.yaw",
"r_arm.gripper": "r_gripper.pos", "r_gripper.pos": "r_arm.gripper",
"l_arm.shoulder.pitch": "l_shoulder_pitch.pos", "l_shoulder_pitch.pos": "l_arm.shoulder.pitch",
"l_arm.shoulder.roll": "l_shoulder_roll.pos", "l_shoulder_roll.pos": "l_arm.shoulder.roll",
"l_arm.elbow.yaw": "l_elbow_yaw.pos", "l_elbow_yaw.pos": "l_arm.elbow.yaw",
"l_arm.elbow.pitch": "l_elbow_pitch.pos", "l_elbow_pitch.pos": "l_arm.elbow.pitch",
"l_arm.wrist.roll": "l_wrist_roll.pos", "l_wrist_roll.pos": "l_arm.wrist.roll",
"l_arm.wrist.pitch": "l_wrist_pitch.pos", "l_wrist_pitch.pos": "l_arm.wrist.pitch",
"l_arm.wrist.yaw": "l_wrist_yaw.pos", "l_wrist_yaw.pos": "l_arm.wrist.yaw",
"l_arm.gripper": "l_gripper.pos", "l_gripper.pos": "l_arm.gripper",
} }
REACHY2_VEL = { REACHY2_VEL = {
@@ -62,6 +62,8 @@ PARAMS = [
{"with_mobile_base": False}, {"with_mobile_base": False},
{"with_mobile_base": False, "with_l_arm": False, "with_antennas": False}, {"with_mobile_base": False, "with_l_arm": False, "with_antennas": False},
{"with_r_arm": False, "with_neck": False, "with_antennas": False}, {"with_r_arm": False, "with_neck": False, "with_antennas": False},
{"use_external_commands": True},
{"use_external_commands": True, "with_mobile_base": False, "with_neck": False},
] ]
@@ -77,8 +79,8 @@ def _make_reachy2_sdk_mock():
# Mock joints with some dummy positions # Mock joints with some dummy positions
joints = { joints = {
k: MagicMock(present_position=round(0.1 + 0.01 * i, 2)) k: MagicMock(present_position=i, goal_position=i + 0.1)
for i, k in enumerate(REACHY2_JOINTS.keys()) for i, k in enumerate(REACHY2_JOINTS.values())
} }
r.joints = joints r.joints = joints
@@ -158,10 +160,44 @@ def test_get_observation(reachy2):
reachy2.connect() reachy2.connect()
obs = reachy2.get_observation() obs = reachy2.get_observation()
expected_keys = {f"{REACHY2_JOINTS[m]}" for m in reachy2.joints_dict.values()} expected_keys = {m for m in reachy2.joints_dict.keys()}
expected_keys.update( expected_keys.update(
f"{v}" for v in REACHY2_VEL.keys() if reachy2.config.with_mobile_base f"{v}" for v in REACHY2_VEL.keys() if reachy2.config.with_mobile_base
) )
expected_keys.update(reachy2.cameras.keys()) expected_keys.update(reachy2.cameras.keys())
assert set(obs.keys()) == expected_keys assert set(obs.keys()) == expected_keys
print(obs)
for motor in reachy2.joints_dict.keys():
assert (
obs[motor] == reachy2.reachy.joints[REACHY2_JOINTS[motor]].present_position
)
if reachy2.config.with_mobile_base:
for vel in REACHY2_VEL.keys():
assert obs[vel] == reachy2.reachy.mobile_base.odometry[REACHY2_VEL[vel]]
def test_send_action(reachy2):
reachy2.connect()
action = {k: i * 10.0 for i, k in enumerate(reachy2.joints_dict.keys())}
if reachy2.config.with_mobile_base:
action.update({k: i * 0.1 for i, k in enumerate(REACHY2_VEL.keys(), start=1)})
returned = reachy2.send_action(action)
assert returned == action
if reachy2.config.with_mobile_base:
goal_speed = [i * 0.1 for i, _ in enumerate(REACHY2_VEL.keys(), start=1)]
reachy2.reachy.mobile_base.set_goal_speed.assert_called_once_with(*goal_speed)
if reachy2.config.use_external_commands:
reachy2.reachy.send_goal_positions.assert_not_called()
if reachy2.config.with_mobile_base:
reachy2.reachy.mobile_base.send_speed_command.assert_not_called()
else:
reachy2.reachy.send_goal_positions.assert_called_once()
if reachy2.config.with_mobile_base:
reachy2.reachy.mobile_base.send_speed_command.assert_called_once()