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

Use odometry for vel with present_position

Browse Source
This commit is contained in:
glannuzel
2025-08-28 18:40:05 +02:00
parent 5264feecef
commit d023e33cd8
2 changed files with 23 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py
+6 -1
View File
@@ -142,11 +142,16 @@ class Reachy2Teleoperator(Teleoperator):
} }
else: else:
joint_action = {k: self.reachy.joints[v].goal_position for k, v in self.joints_dict.items()} joint_action = {k: self.reachy.joints[v].goal_position for k, v in self.joints_dict.items()}
if not self.config.with_mobile_base: if not self.config.with_mobile_base:
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms") logger.debug(f"{self} read action: {dt_ms:.1f}ms")
return joint_action return joint_action
vel_action = {k: self.reachy.mobile_base.last_cmd_vel[v] for k, v in REACHY2_VEL.items()}
if self.config.use_present_position:
vel_action = {k: self.reachy.mobile_base.odometry[v] for k, v in REACHY2_VEL.items()}
else:
vel_action = {k: self.reachy.mobile_base.last_cmd_vel[v] for k, v in REACHY2_VEL.items()}
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms") logger.debug(f"{self} read action: {dt_ms:.1f}ms")
return {**joint_action, **vel_action} return {**joint_action, **vel_action}
tests/teleoperators/test_reachy2_teleoperator.py
+17 -5
View File
@@ -87,9 +87,17 @@ def _make_reachy2_sdk_mock():
# Mock mobile base with some dummy odometry # Mock mobile base with some dummy odometry
r.mobile_base = MagicMock() r.mobile_base = MagicMock()
r.mobile_base.last_cmd_vel = { r.mobile_base.last_cmd_vel = {
"vx": 0.001, "vx": -0.2,
"vy": 0.002, "vy": 0.2,
"vtheta": 0.0, "vtheta": 11.0,
}
r.mobile_base.odometry = {
"x": 1.0,
"y": 2.0,
"theta": 20.0,
"vx": 0.1,
"vy": -0.1,
"vtheta": 8.0,
} }
r.connect = MagicMock(side_effect=_connect) r.connect = MagicMock(side_effect=_connect)
@@ -140,8 +148,12 @@ def test_get_action(reachy2):
else: else:
assert action[motor] == reachy2.reachy.joints[REACHY2_JOINTS[motor]].goal_position assert action[motor] == reachy2.reachy.joints[REACHY2_JOINTS[motor]].goal_position
if reachy2.config.with_mobile_base: if reachy2.config.with_mobile_base:
for vel in REACHY2_VEL.keys(): if reachy2.config.use_present_position:
assert action[vel] == reachy2.reachy.mobile_base.last_cmd_vel[REACHY2_VEL[vel]] for vel in REACHY2_VEL.keys():
assert action[vel] == reachy2.reachy.mobile_base.odometry[REACHY2_VEL[vel]]
else:
for vel in REACHY2_VEL.keys():
assert action[vel] == reachy2.reachy.mobile_base.last_cmd_vel[REACHY2_VEL[vel]]
def test_no_part_declared(): def test_no_part_declared():