From 40ef0939801ce019b358c4161f720c4ae252cabe Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 14 Aug 2026 14:17:34 -0700 Subject: [PATCH] feat(manipulation): add dual OpenYAM teleoperation --- data/.lfs/dual_openyam_abc_box_v2.tar.gz | 3 + dimos/hardware/test_adapter_registries.py | 1 + .../dual_openyam_damiao/_registry.py | 19 +++ .../whole_body/dual_openyam_damiao/adapter.py | 98 ++++++++++++ .../dual_openyam_damiao/test_adapter.py | 57 +++++++ dimos/robot/all_blueprints.py | 4 + .../dual_openyam/blueprints/basic.py | 70 ++++++++ .../dual_openyam/blueprints/teleop.py | 105 ++++++++++++ .../robot/manipulators/dual_openyam/config.py | 150 ++++++++++++++++++ .../robot/manipulators/dual_openyam/model.py | 31 ++++ .../manipulators/dual_openyam/teleop_ik.py | 70 ++++++++ .../dual_openyam/test_blueprints.py | 108 +++++++++++++ .../manipulators/dual_openyam/test_config.py | 74 +++++++++ .../dual_openyam/test_integration.py | 40 +++++ .../manipulators/dual_openyam/test_model.py | 126 +++++++++++++++ .../dual_openyam/test_teleop_ik.py | 96 +++++++++++ 16 files changed, 1052 insertions(+) create mode 100644 data/.lfs/dual_openyam_abc_box_v2.tar.gz create mode 100644 dimos/hardware/whole_body/dual_openyam_damiao/_registry.py create mode 100644 dimos/hardware/whole_body/dual_openyam_damiao/adapter.py create mode 100644 dimos/hardware/whole_body/dual_openyam_damiao/test_adapter.py create mode 100644 dimos/robot/manipulators/dual_openyam/blueprints/basic.py create mode 100644 dimos/robot/manipulators/dual_openyam/blueprints/teleop.py create mode 100644 dimos/robot/manipulators/dual_openyam/config.py create mode 100644 dimos/robot/manipulators/dual_openyam/model.py create mode 100644 dimos/robot/manipulators/dual_openyam/teleop_ik.py create mode 100644 dimos/robot/manipulators/dual_openyam/test_blueprints.py create mode 100644 dimos/robot/manipulators/dual_openyam/test_config.py create mode 100644 dimos/robot/manipulators/dual_openyam/test_integration.py create mode 100644 dimos/robot/manipulators/dual_openyam/test_model.py create mode 100644 dimos/robot/manipulators/dual_openyam/test_teleop_ik.py diff --git a/data/.lfs/dual_openyam_abc_box_v2.tar.gz b/data/.lfs/dual_openyam_abc_box_v2.tar.gz new file mode 100644 index 0000000000..af01705254 --- /dev/null +++ b/data/.lfs/dual_openyam_abc_box_v2.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f0c4802e4001a514054f01485828f7e69cd6bc97b6169b3a26e49a445e0b0539 +size 2488468 diff --git a/dimos/hardware/test_adapter_registries.py b/dimos/hardware/test_adapter_registries.py index 7061cee54c..d51d54a607 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -67,6 +67,7 @@ "unitree_go2", }, "whole_body": { + "dual_openyam_damiao", "mock_whole_body", "openarm_damiao", "openyam_damiao", diff --git a/dimos/hardware/whole_body/dual_openyam_damiao/_registry.py b/dimos/hardware/whole_body/dual_openyam_damiao/_registry.py new file mode 100644 index 0000000000..ce35bbee31 --- /dev/null +++ b/dimos/hardware/whole_body/dual_openyam_damiao/_registry.py @@ -0,0 +1,19 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "dual_openyam_damiao": ( + "dimos.hardware.whole_body.dual_openyam_damiao.adapter:DualOpenYamDamiaoAdapter" + ), +} diff --git a/dimos/hardware/whole_body/dual_openyam_damiao/adapter.py b/dimos/hardware/whole_body/dual_openyam_damiao/adapter.py new file mode 100644 index 0000000000..b178548200 --- /dev/null +++ b/dimos/hardware/whole_body/dual_openyam_damiao/adapter.py @@ -0,0 +1,98 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Dual OpenYAM bimanual physical topology for the Damiao whole-body adapter.""" + +from __future__ import annotations + +from pathlib import Path + +import can_motor_control +from can_motor_control import damiao + +from dimos.hardware.whole_body.damiao.adapter import DamiaoWholeBodyAdapter +from dimos.robot.manipulators.dual_openyam.config import DUAL_OPENYAM_URDF_ARM_JOINTS +from dimos.robot.manipulators.dual_openyam.model import DUAL_OPENYAM_MODEL_PATH + + +def _arm_motors(side: str) -> list[can_motor_control.MotorSpec]: + return [ + can_motor_control.MotorSpec(f"{side}_joint1", damiao.MotorType.DM4340, 0x01, 0x11), + can_motor_control.MotorSpec(f"{side}_joint2", damiao.MotorType.DM4340, 0x02, 0x12), + can_motor_control.MotorSpec(f"{side}_joint3", damiao.MotorType.DM4340, 0x03, 0x13), + can_motor_control.MotorSpec(f"{side}_joint4", damiao.MotorType.DM4310, 0x04, 0x14), + can_motor_control.MotorSpec(f"{side}_joint5", damiao.MotorType.DM4310, 0x05, 0x15), + can_motor_control.MotorSpec(f"{side}_joint6", damiao.MotorType.DM4310, 0x06, 0x16), + ] + + +def _gripper_motor(side: str) -> can_motor_control.MotorSpec: + return can_motor_control.MotorSpec( + f"{side}_gripper", + damiao.MotorType.DM4310, + 0x08, + 0x18, + ) + + +class DualOpenYamDamiaoAdapter(DamiaoWholeBodyAdapter): + """Two standard YAM follower arms and linear grippers, one bus per side.""" + + arm_joints = { + "left_arm": tuple(f"left_arm/joint{index}" for index in range(1, 7)), + "right_arm": tuple(f"right_arm/joint{index}" for index in range(1, 7)), + } + gripper_joints = { + "left_gripper": "left_arm/gripper", + "right_gripper": "right_arm/gripper", + } + bus_defaults = {"left": "", "right": ""} + kinematic_joint_names = tuple(DUAL_OPENYAM_URDF_ARM_JOINTS) + + @property + def kinematic_model_path(self) -> Path: + """Return the authoritative dual copy of the verified OpenYAM URDF.""" + return DUAL_OPENYAM_MODEL_PATH + + def _build_robot(self) -> can_motor_control.Robot: + return ( + can_motor_control.Robot.builder() + .add_bus( + "left", + can_motor_control.SocketCanBus(self.bus_address("left")), + damiao.DamiaoCodec(), + ) + .add_bus( + "right", + can_motor_control.SocketCanBus(self.bus_address("right")), + damiao.DamiaoCodec(), + ) + .add_arm("left_arm", bus="left", motors=_arm_motors("left")) + .add_arm("right_arm", bus="right", motors=_arm_motors("right")) + .add_gripper( + "left_gripper", + bus="left", + motor=_gripper_motor("left"), + opening_direction="decreasing_position", + default_current=0.15, + ) + .add_gripper( + "right_gripper", + bus="right", + motor=_gripper_motor("right"), + opening_direction="decreasing_position", + default_current=0.15, + ) + .build() + ) diff --git a/dimos/hardware/whole_body/dual_openyam_damiao/test_adapter.py b/dimos/hardware/whole_body/dual_openyam_damiao/test_adapter.py new file mode 100644 index 0000000000..3af3a79684 --- /dev/null +++ b/dimos/hardware/whole_body/dual_openyam_damiao/test_adapter.py @@ -0,0 +1,57 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from collections.abc import Iterator + +import can_motor_control +import pytest +from pytest_mock import MockerFixture + +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.dual_openyam_damiao.adapter import ( + DualOpenYamDamiaoAdapter, +) +from dimos.robot.manipulators.dual_openyam.config import DUAL_OPENYAM_JOINTS + + +@pytest.fixture +def adapter(mocker: MockerFixture) -> Iterator[DualOpenYamDamiaoAdapter]: + mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) + result = DualOpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig( + bus_addresses={"left": "can8", "right": "can9"}, + gravity_comp=False, + ) + ) + yield result + result.disconnect() + + +def test_adapter_connects_complete_dual_yam_topology( + adapter: DualOpenYamDamiaoAdapter, +) -> None: + assert adapter.connect() + + robot = adapter._robot + assert robot.bus_names() == ["left", "right"] + assert robot.group_names() == ["left_arm", "right_arm", "left_gripper", "right_gripper"] + assert len(robot["left_arm"]) == 6 + assert len(robot["right_arm"]) == 6 + assert list(adapter.joint_names) == DUAL_OPENYAM_JOINTS + assert adapter._pin_model.nq == 12 + assert adapter._pin_model.nv == 12 + assert tuple(str(name) for name in adapter._pin_model.names[1:]) == ( + *[f"left_joint{index}" for index in range(1, 7)], + *[f"right_joint{index}" for index in range(1, 7)], + ) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 383df6157e..731ef64b69 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -24,6 +24,7 @@ "coordinator-cartesian-ik-piper": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_cartesian_ik_piper", "coordinator-combined-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_combined_xarm6", "coordinator-dual-mock": "dimos.robot.manipulators.common.mock:coordinator_dual_mock", + "coordinator-dual-openyam": "dimos.robot.manipulators.dual_openyam.blueprints.basic:coordinator_dual_openyam", "coordinator-dual-xarm": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_dual_xarm", "coordinator-flowbase": "dimos.control.blueprints.mobile:coordinator_flowbase", "coordinator-flowbase-keyboard-teleop": "dimos.control.blueprints.mobile:coordinator_flowbase_keyboard_teleop", @@ -60,6 +61,7 @@ "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", + "dual-openyam-planner-coordinator": "dimos.robot.manipulators.dual_openyam.blueprints.basic:dual_openyam_planner_coordinator", "dual-xarm6-planner-coordinator": "dimos.robot.manipulators.xarm.blueprints.basic:dual_xarm6_planner_coordinator", "go2-zenoh-basic": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_basic", "go2-zenoh-htc": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_htc", @@ -98,6 +100,7 @@ "teleop-phone-go2-fleet": "dimos.teleop.phone.blueprints:teleop_phone_go2_fleet", "teleop-quest-a1z": "dimos.teleop.quest.blueprints:teleop_quest_a1z", "teleop-quest-dual": "dimos.teleop.quest.blueprints:teleop_quest_dual", + "teleop-quest-dual-openyam": "dimos.robot.manipulators.dual_openyam.blueprints.teleop:teleop_quest_dual_openyam", "teleop-quest-go2": "dimos.teleop.quest.blueprints:teleop_quest_go2", "teleop-quest-hand-xarm7": "dimos.teleop.quest.blueprints:teleop_quest_hand_xarm7", "teleop-quest-openarm": "dimos.robot.manipulators.openarm.blueprints.teleop:teleop_quest_openarm", @@ -193,6 +196,7 @@ "drone-camera-module": "dimos.robot.drone.camera_module.DroneCameraModule", "drone-connection-module": "dimos.robot.drone.connection_module.DroneConnectionModule", "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule", + "dual-open-yam-coordinator": "dimos.robot.manipulators.dual_openyam.blueprints.basic.DualOpenYamCoordinator", "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", "episode-monitor-module": "dimos.imitation.collection.episode_monitor.EpisodeMonitorModule", "eval-module": "dimos.evals.module.EvalModule", diff --git a/dimos/robot/manipulators/dual_openyam/blueprints/basic.py b/dimos/robot/manipulators/dual_openyam/blueprints/basic.py new file mode 100644 index 0000000000..be7d5d5ca7 --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/blueprints/basic.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Dual OpenYAM coordinator and planning blueprints.""" + +from dimos.control.coordinator import ControlCoordinatorConfig, TaskConfig +from dimos.control.tasks.trajectory_task.trajectory_task import JOINT_TRAJECTORY_TASK_NAME +from dimos.control.teleop_coordinator import TeleopControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.blueprints import planner +from dimos.robot.manipulators.dual_openyam.config import ( + DUAL_OPENYAM_ARM_JOINTS, + dual_openyam_hardware, + dual_openyam_model_config, +) + + +def dual_openyam_trajectory_task(*, priority: int = 20) -> TaskConfig: + return TaskConfig( + name=JOINT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=list(DUAL_OPENYAM_ARM_JOINTS), + priority=priority, + params={"start_position_tolerance": 0.05}, + ) + + +class DualOpenYamCoordinatorConfig(ControlCoordinatorConfig): + """Dual OpenYAM deployment configuration.""" + + left_can_port: str | None = None + right_can_port: str | None = None + + +class DualOpenYamCoordinator(TeleopControlCoordinator): + """Select mock or explicit dual-CAN hardware during coordinator setup.""" + + config: DualOpenYamCoordinatorConfig + + def _setup_from_config(self) -> None: + self.config.hardware = [ + dual_openyam_hardware( + left_can_port=self.config.left_can_port, + right_can_port=self.config.right_can_port, + ) + ] + super()._setup_from_config() + + +coordinator_dual_openyam = DualOpenYamCoordinator.blueprint( + tasks=[dual_openyam_trajectory_task()], +) + +dual_openyam_planner_coordinator = autoconnect( + planner(robots=[dual_openyam_model_config()]), + DualOpenYamCoordinator.blueprint( + tasks=[dual_openyam_trajectory_task()], + ), +) diff --git a/dimos/robot/manipulators/dual_openyam/blueprints/teleop.py b/dimos/robot/manipulators/dual_openyam/blueprints/teleop.py new file mode 100644 index 0000000000..a53319dbd5 --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/blueprints/teleop.py @@ -0,0 +1,105 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Coupled Quest teleoperation for the complete Dual OpenYAM entity.""" + +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig +from dimos.robot.manipulators.common.blueprints import teleop_ik_task +from dimos.robot.manipulators.dual_openyam.blueprints.basic import ( + DualOpenYamCoordinator, + dual_openyam_trajectory_task, +) +from dimos.robot.manipulators.dual_openyam.config import ( + DUAL_OPENYAM_ARM_JOINTS, + DUAL_OPENYAM_GRIPPER_JOINTS, + dual_openyam_hardware, + dual_openyam_model_config, +) +from dimos.robot.manipulators.dual_openyam.teleop_ik import ( + DualOpenYamPinkPoseTargetSolver, +) +from dimos.teleop.quest.quest_extensions import ArmTeleopModule + +DUAL_OPENYAM_QUEST_TASK_NAME = "teleop_dual_openyam" + +_dual_openyam_quest_pink = PinkKinematicsConfig( + dt=0.01, + position_cost=8.0, + orientation_cost=2.0, + posture_cost=0.01, + joint_limit_posture_margin=0.3, + lm_damping=0.01, + gain=1.0, +) +_dual_openyam_quest_hardware = dual_openyam_hardware() +_dual_openyam_quest_model = dual_openyam_model_config() +_dual_openyam_quest_task = teleop_ik_task( + _dual_openyam_quest_hardware, + robot_model=_dual_openyam_quest_model, + name=DUAL_OPENYAM_QUEST_TASK_NAME, + joint_names=DUAL_OPENYAM_ARM_JOINTS, + priority=10, + solver_type=DualOpenYamPinkPoseTargetSolver, + bindings=[ + { + "hand": "left", + "target_frame": "left_grasp_frame", + "gripper_joint": DUAL_OPENYAM_GRIPPER_JOINTS[0], + "gripper_open_position": 1.0, + "gripper_closed_position": 0.0, + }, + { + "hand": "right", + "target_frame": "right_grasp_frame", + "gripper_joint": DUAL_OPENYAM_GRIPPER_JOINTS[1], + "gripper_open_position": 1.0, + "gripper_closed_position": 0.0, + }, + ], + params={ + "pink": _dual_openyam_quest_pink, + "timeout": 0.5, + "max_command_tracking_error_deg": 10.0, + "max_joint_velocity_rad_s": 2.0, + "joint_command_filter_cutoff_hz": 30.0, + }, +) + +teleop_quest_dual_openyam = autoconnect( + ArmTeleopModule.blueprint( + task_names={ + "left": DUAL_OPENYAM_QUEST_TASK_NAME, + "right": DUAL_OPENYAM_QUEST_TASK_NAME, + } + ), + DualOpenYamCoordinator.blueprint( + instance_name="ControlCoordinator", + tasks=[ + _dual_openyam_quest_task, + dual_openyam_trajectory_task(priority=20), + ], + ), + ManipulationModule.blueprint( + robots=[_dual_openyam_quest_model], + kinematics=_dual_openyam_quest_pink, + visualization={"backend": "viser"}, + ), +).remappings( + [ + (ArmTeleopModule, "left_controller_output", "left_cartesian_command"), + (ArmTeleopModule, "right_controller_output", "right_cartesian_command"), + ] +) diff --git a/dimos/robot/manipulators/dual_openyam/config.py b/dimos/robot/manipulators/dual_openyam/config.py new file mode 100644 index 0000000000..d51846ac45 --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/config.py @@ -0,0 +1,150 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Arm-only Dual OpenYAM hardware and model configuration.""" + +from dimos.control.components import HardwareComponent, HardwareType +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.spec import WholeBodyConfig +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.robot.manipulators._modeling import base_pose +from dimos.robot.manipulators.dual_openyam.model import ( + DUAL_OPENYAM_MODEL_PATH, + DUAL_OPENYAM_PACKAGE_PATHS, +) + +DUAL_OPENYAM_DOF_PER_ARM = 6 +DUAL_OPENYAM_HARDWARE_ID = "dual_openyam" +DUAL_OPENYAM_ADAPTER_TYPE = "dual_openyam_damiao" +DUAL_OPENYAM_SIDES = ("left", "right") +DUAL_OPENYAM_LEFT_ARM_JOINTS = [ + f"left_arm/joint{index}" for index in range(1, DUAL_OPENYAM_DOF_PER_ARM + 1) +] +DUAL_OPENYAM_RIGHT_ARM_JOINTS = [ + f"right_arm/joint{index}" for index in range(1, DUAL_OPENYAM_DOF_PER_ARM + 1) +] +DUAL_OPENYAM_ARM_JOINTS = [ + *DUAL_OPENYAM_LEFT_ARM_JOINTS, + *DUAL_OPENYAM_RIGHT_ARM_JOINTS, +] +DUAL_OPENYAM_GRIPPER_JOINTS = ["left_arm/gripper", "right_arm/gripper"] +DUAL_OPENYAM_JOINTS = [*DUAL_OPENYAM_ARM_JOINTS, *DUAL_OPENYAM_GRIPPER_JOINTS] +DUAL_OPENYAM_HOME_PER_ARM = [0.0, 1.047, 1.047, 0.0, 0.0, 0.0] +DUAL_OPENYAM_HOME_JOINTS = [*DUAL_OPENYAM_HOME_PER_ARM, *DUAL_OPENYAM_HOME_PER_ARM] +DUAL_OPENYAM_URDF_ARM_JOINTS = [ + f"{side}_joint{index}" + for side in DUAL_OPENYAM_SIDES + for index in range(1, DUAL_OPENYAM_DOF_PER_ARM + 1) +] +_ARM_KP = (80.0, 80.0, 80.0, 10.0, 10.0, 10.0) +_ARM_KD = (5.0, 5.0, 5.0, 1.5, 1.5, 1.5) + + +def dual_openyam_arm_joints(side: str) -> list[str]: + if side not in DUAL_OPENYAM_SIDES: + raise ValueError(f"side must be 'left' or 'right', got {side!r}") + return [f"{side}_arm/joint{index}" for index in range(1, 7)] + + +def dual_openyam_urdf_joints(side: str) -> list[str]: + if side not in DUAL_OPENYAM_SIDES: + raise ValueError(f"side must be 'left' or 'right', got {side!r}") + return [f"{side}_joint{index}" for index in range(1, 7)] + + +def dual_openyam_hardware( + *, + left_can_port: str | None = None, + right_can_port: str | None = None, +) -> HardwareComponent: + """Select mock hardware or an explicitly configured dual-CAN adapter.""" + if left_can_port is None and right_can_port is None: + return dual_openyam_mock_hardware() + if left_can_port is None or right_can_port is None: + raise ValueError("Dual OpenYAM hardware requires both left and right CAN ports") + return _hardware_component( + DUAL_OPENYAM_ADAPTER_TYPE, + { + "runtime_config": DamiaoRuntimeConfig( + bus_addresses={"left": left_can_port, "right": right_can_port}, + gravity_comp=True, + ) + }, + ) + + +def dual_openyam_mock_hardware() -> HardwareComponent: + """Build an in-memory complete dual-arm actuator component.""" + return _hardware_component( + "mock_whole_body", + {"initial_positions": [*DUAL_OPENYAM_HOME_JOINTS, 0.0, 0.0]}, + ) + + +def _hardware_component( + adapter_type: str, + adapter_kwargs: dict[str, object], +) -> HardwareComponent: + return HardwareComponent( + hardware_id=DUAL_OPENYAM_HARDWARE_ID, + hardware_type=HardwareType.WHOLE_BODY, + joints=list(DUAL_OPENYAM_JOINTS), + adapter_type=adapter_type, + auto_enable=True, + adapter_kwargs=adapter_kwargs, + wb_config=WholeBodyConfig( + kp=(*_ARM_KP, *_ARM_KP, 0.0, 0.0), + kd=(*_ARM_KD, *_ARM_KD, 0.0, 0.0), + ), + ) + + +def dual_openyam_model_config() -> RobotModelConfig: + """Build the combined arm-only planning model.""" + mapping = dict( + zip( + DUAL_OPENYAM_ARM_JOINTS, + DUAL_OPENYAM_URDF_ARM_JOINTS, + strict=True, + ) + ) + return RobotModelConfig( + name=DUAL_OPENYAM_HARDWARE_ID, + model_path=DUAL_OPENYAM_MODEL_PATH, + base_pose=base_pose(), + joint_names=list(DUAL_OPENYAM_URDF_ARM_JOINTS), + base_link="dual_openyam_base", + planning_groups=[ + PlanningGroupDefinition( + name="left_manipulator", + joint_names=tuple(dual_openyam_urdf_joints("left")), + base_link="dual_openyam_base", + tip_link="left_grasp_frame", + ), + PlanningGroupDefinition( + name="right_manipulator", + joint_names=tuple(dual_openyam_urdf_joints("right")), + base_link="dual_openyam_base", + tip_link="right_grasp_frame", + ), + ], + package_paths=DUAL_OPENYAM_PACKAGE_PATHS, + auto_convert_meshes=True, + joint_name_mapping=mapping, + home_joints=list(DUAL_OPENYAM_HOME_JOINTS), + max_velocity=2.0, + max_acceleration=1.0, + tf_extra_links=[], + ) diff --git a/dimos/robot/manipulators/dual_openyam/model.py b/dimos/robot/manipulators/dual_openyam/model.py new file mode 100644 index 0000000000..2e6dbeaca9 --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/model.py @@ -0,0 +1,31 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Authoritative arm-only Dual OpenYAM model assets.""" + +from pathlib import Path + +from dimos.utils.data import LfsPath + +# LFS extraction caches by directory name. Version revised artifacts so they cannot +# silently reuse an obsolete extracted model. +DUAL_OPENYAM_ARTIFACT = "dual_openyam_abc_box_v2" +DUAL_OPENYAM_PACKAGE = LfsPath(DUAL_OPENYAM_ARTIFACT) +DUAL_OPENYAM_MODEL_PATH = DUAL_OPENYAM_PACKAGE / "dual_openyam.urdf" +DUAL_OPENYAM_PACKAGE_PATHS: dict[str, Path] = { + "dual_openyam_abc_box": DUAL_OPENYAM_PACKAGE, +} +DUAL_OPENYAM_SOURCE_PATH = DUAL_OPENYAM_PACKAGE / "SOURCE.md" +DUAL_OPENYAM_ABC_LICENSE_PATH = DUAL_OPENYAM_PACKAGE / "ABC_LICENSE" +DUAL_OPENYAM_I2RT_LICENSE_PATH = DUAL_OPENYAM_PACKAGE / "I2RT_YAM_LICENSE" diff --git a/dimos/robot/manipulators/dual_openyam/teleop_ik.py b/dimos/robot/manipulators/dual_openyam/teleop_ik.py new file mode 100644 index 0000000000..14931cdead --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/teleop_ik.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Dual OpenYAM-specific Pink objective tuning for Quest teleoperation.""" + +from __future__ import annotations + +from collections.abc import Mapping + +import numpy as np +import pink + +from dimos.control.tasks.pose_target_ik import PinkPoseTargetSolver +from dimos.robot.manipulators.dual_openyam.config import DUAL_OPENYAM_HOME_JOINTS + +_FRAME_POSITION_COST = 8.0 +_FRAME_ORIENTATION_COST = 2.0 +_POSTURE_WEIGHTS = np.ones(len(DUAL_OPENYAM_HOME_JOINTS), dtype=np.float64) +_NOMINAL_POSTURE = np.asarray(DUAL_OPENYAM_HOME_JOINTS, dtype=np.float64) + + +class DualOpenYamPinkPoseTargetSolver(PinkPoseTargetSolver): + """Keep both nonredundant OpenYAM arms near their canonical home posture.""" + + def _create_tasks( + self, + configuration: pink.Configuration, + target_frames: tuple[str, ...], + ) -> dict[str, pink.Task]: + tasks = super()._create_tasks(configuration, target_frames) + + for frame_name in target_frames: + frame_task = tasks[f"frame/{frame_name}"] + frame_task.set_position_cost(_FRAME_POSITION_COST) + frame_task.set_orientation_cost(_FRAME_ORIENTATION_COST) + + posture_task = tasks.get("posture/current") + if posture_task is None: + raise ValueError("DualOpenYamPinkPoseTargetSolver requires a positive posture cost") + posture_task.cost = self.config.posture_cost * _POSTURE_WEIGHTS + + # Each arm has six joints for a six-DoF frame objective, so there is no + # redundant direction in which to optimize a manipulability task. + return tasks + + def _update_current_posture_target( + self, + tasks: Mapping[str, pink.Task], + configuration: pink.Configuration, + ) -> None: + posture_task = tasks.get("posture/current") + if not isinstance(posture_task, pink.tasks.PostureTask): + raise ValueError("DualOpenYamPinkPoseTargetSolver requires a posture task") + if configuration.model.nq != len(_NOMINAL_POSTURE): + raise ValueError( + f"Dual OpenYAM nominal posture has {len(_NOMINAL_POSTURE)} joints, " + f"model has {configuration.model.nq}" + ) + posture_task.set_target(_NOMINAL_POSTURE) diff --git a/dimos/robot/manipulators/dual_openyam/test_blueprints.py b/dimos/robot/manipulators/dual_openyam/test_blueprints.py new file mode 100644 index 0000000000..8876301d47 --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/test_blueprints.py @@ -0,0 +1,108 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 + +from typing import Any, cast + +import pytest +from pytest_mock import MockerFixture + +from dimos.control.tasks.teleop_ik_task.teleop_ik_task import TeleopIKTask +from dimos.control.tick_loop import TickLoop +from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser +from dimos.core.coordination.blueprints import Blueprint +from dimos.hardware.whole_body.spec import WholeBodyAdapter +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.robot.manipulators.dual_openyam.blueprints.basic import ( + DualOpenYamCoordinator, +) +from dimos.robot.manipulators.dual_openyam.blueprints.teleop import ( + DUAL_OPENYAM_QUEST_TASK_NAME, + teleop_quest_dual_openyam, +) +from dimos.robot.manipulators.dual_openyam.config import ( + DUAL_OPENYAM_ARM_JOINTS, + DUAL_OPENYAM_GRIPPER_JOINTS, +) +from dimos.robot.manipulators.dual_openyam.teleop_ik import ( + DualOpenYamPinkPoseTargetSolver, +) +from dimos.teleop.quest.quest_types import Buttons + + +def _module_kwargs(blueprint: Blueprint, module_type: type) -> dict[str, Any]: + return next(atom.kwargs for atom in blueprint.blueprints if atom.module is module_type) + + +def test_quest_blueprint_selects_physical_hardware_from_both_can_ports() -> None: + parsed = BlueprintConfigParser(teleop_quest_dual_openyam).parse( + [ + "--left-can-port", + "follower_l", + "--right-can-port", + "follower_r", + "--manipulationmodule.visualization.host=0.0.0.0", + ], + environ={}, + ) + + coordinator = parsed.module_kwargs("ControlCoordinator") + assert coordinator["left_can_port"] == "follower_l" + assert coordinator["right_can_port"] == "follower_r" + assert parsed.module_kwargs("manipulationmodule")["visualization"]["host"] == "0.0.0.0" + + +def test_mock_quest_coordinator_commands_both_arms_and_grippers( + mocker: MockerFixture, +) -> None: + kwargs = _module_kwargs(teleop_quest_dual_openyam, DualOpenYamCoordinator) + mocker.patch.object(DualOpenYamPinkPoseTargetSolver, "_validate_frame_targets") + mocker.patch.object( + DualOpenYamPinkPoseTargetSolver, + "frame_poses", + return_value={ + "left_grasp_frame": PoseStamped(position=[0.5, 0.2, 0.4]), + "right_grasp_frame": PoseStamped(position=[0.5, -0.2, 0.4]), + }, + ) + mocker.patch.object( + DualOpenYamPinkPoseTargetSolver, + "step", + return_value=JointState(name=DUAL_OPENYAM_ARM_JOINTS, position=[0.01] * 12), + ) + mocker.patch.object(TickLoop, "start") + coordinator = DualOpenYamCoordinator(publish_joint_state=False, **kwargs) + + coordinator.start() + try: + task = cast("TeleopIKTask", coordinator._tasks[DUAL_OPENYAM_QUEST_TASK_NAME]) + assert set(task.claim().joints) == set( + DUAL_OPENYAM_ARM_JOINTS + DUAL_OPENYAM_GRIPPER_JOINTS + ) + buttons = Buttons() + buttons.left_primary = True + buttons.right_primary = True + buttons.pack_analog_triggers(left=0.25, right=0.75) + coordinator._dispatch("teleop_buttons", buttons) + coordinator._dispatch( + "left_cartesian_command", + PoseStamped(frame_id=DUAL_OPENYAM_QUEST_TASK_NAME, position=[1.0, 0.0, 0.0]), + ) + coordinator._dispatch( + "right_cartesian_command", + PoseStamped(frame_id=DUAL_OPENYAM_QUEST_TASK_NAME, position=[-1.0, 0.0, 0.0]), + ) + assert coordinator._tick_loop is not None + coordinator._tick_loop._tick() + + adapter = cast("WholeBodyAdapter", coordinator._hardware["dual_openyam"].adapter) + states = adapter.read_motor_states() + assert [state.q for state in states[:12]] == [0.01] * 12 + assert [state.q for state in states[12:]] == pytest.approx([0.75, 0.25], abs=0.01) + finally: + coordinator.stop() diff --git a/dimos/robot/manipulators/dual_openyam/test_config.py b/dimos/robot/manipulators/dual_openyam/test_config.py new file mode 100644 index 0000000000..45bdf66a84 --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/test_config.py @@ -0,0 +1,74 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest + +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.robot.manipulators.dual_openyam.config import ( + DUAL_OPENYAM_ADAPTER_TYPE, + DUAL_OPENYAM_ARM_JOINTS, + DUAL_OPENYAM_HOME_JOINTS, + DUAL_OPENYAM_JOINTS, + DUAL_OPENYAM_URDF_ARM_JOINTS, + dual_openyam_hardware, + dual_openyam_model_config, +) + + +def test_dual_openyam_model_has_canonical_groups_mapping_and_reference_posture() -> None: + config = dual_openyam_model_config() + + assert config.joint_names == DUAL_OPENYAM_URDF_ARM_JOINTS + assert config.get_coordinator_joint_names() == DUAL_OPENYAM_ARM_JOINTS + assert config.home_joints == DUAL_OPENYAM_HOME_JOINTS + assert config.max_velocity == pytest.approx(2.0) + assert [(group.name, group.tip_link) for group in config.planning_groups] == [ + ("left_manipulator", "left_grasp_frame"), + ("right_manipulator", "right_grasp_frame"), + ] + + +def test_dual_openyam_hardware_defaults_to_complete_mock() -> None: + hardware = dual_openyam_hardware() + + assert hardware.hardware_id == "dual_openyam" + assert hardware.adapter_type == "mock_whole_body" + assert hardware.joints == DUAL_OPENYAM_JOINTS + assert hardware.adapter_kwargs["initial_positions"] == [ + *DUAL_OPENYAM_HOME_JOINTS, + 0.0, + 0.0, + ] + + +def test_dual_openyam_hardware_uses_both_explicit_can_ports() -> None: + hardware = dual_openyam_hardware(left_can_port="can8", right_can_port="can9") + + assert hardware.adapter_type == DUAL_OPENYAM_ADAPTER_TYPE + runtime = hardware.adapter_kwargs["runtime_config"] + assert isinstance(runtime, DamiaoRuntimeConfig) + assert runtime.bus_addresses == {"left": "can8", "right": "can9"} + assert runtime.gravity_comp is True + + +@pytest.mark.parametrize( + ("left", "right"), + [("can8", None), (None, "can9")], +) +def test_dual_openyam_hardware_rejects_partial_can_configuration( + left: str | None, + right: str | None, +) -> None: + with pytest.raises(ValueError, match="requires both"): + dual_openyam_hardware(left_can_port=left, right_can_port=right) diff --git a/dimos/robot/manipulators/dual_openyam/test_integration.py b/dimos/robot/manipulators/dual_openyam/test_integration.py new file mode 100644 index 0000000000..de4119502f --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/test_integration.py @@ -0,0 +1,40 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 + +from pathlib import Path + +from yourdfpy import URDF # type: ignore[import-untyped] + +from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf +from dimos.robot.manipulators.dual_openyam.config import dual_openyam_model_config + + +def test_authoritative_arm_only_model_prepares_for_viser() -> None: + config = dual_openyam_model_config() + prepared = Path( + prepare_urdf( + config.model_path, + package_paths=config.package_paths, + convert_meshes=config.auto_convert_meshes, + ) + ) + model = URDF.load( + prepared, + build_scene_graph=True, + build_collision_scene_graph=True, + load_meshes=True, + load_collision_meshes=True, + ) + assert { + "left_grasp_frame", + "right_grasp_frame", + "left_tip_left", + "left_tip_right", + "right_tip_left", + "right_tip_right", + } <= set(model.link_map) diff --git a/dimos/robot/manipulators/dual_openyam/test_model.py b/dimos/robot/manipulators/dual_openyam/test_model.py new file mode 100644 index 0000000000..5fa9328769 --- /dev/null +++ b/dimos/robot/manipulators/dual_openyam/test_model.py @@ -0,0 +1,126 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import xml.etree.ElementTree as ET + +import pinocchio + +from dimos.robot.manipulators.dual_openyam.model import ( + DUAL_OPENYAM_ARTIFACT, + DUAL_OPENYAM_MODEL_PATH, + DUAL_OPENYAM_PACKAGE, + DUAL_OPENYAM_PACKAGE_PATHS, +) + + +def _tree() -> ET.Element: + return ET.parse(DUAL_OPENYAM_MODEL_PATH).getroot() + + +def _joint(root: ET.Element, name: str) -> ET.Element: + result = root.find(f"./joint[@name='{name}']") + assert result is not None + return result + + +def _origin_xyz(joint: ET.Element) -> tuple[float, float, float]: + origin = joint.find("origin") + assert origin is not None + return tuple(float(value) for value in origin.attrib["xyz"].split()) + + +def test_model_package_is_versioned_and_self_contained() -> None: + assert DUAL_OPENYAM_ARTIFACT == "dual_openyam_abc_box_v2" + assert Path(DUAL_OPENYAM_PACKAGE).name == DUAL_OPENYAM_ARTIFACT + assert DUAL_OPENYAM_MODEL_PATH.is_file() + assert DUAL_OPENYAM_PACKAGE_PATHS == { + "dual_openyam_abc_box": DUAL_OPENYAM_PACKAGE, + } + for mesh in _tree().findall("./link/visual/geometry/mesh"): + uri = mesh.attrib["filename"] + prefix = "package://dual_openyam_abc_box/" + assert uri.startswith(prefix) + assert (Path(DUAL_OPENYAM_PACKAGE) / uri.removeprefix(prefix)).is_file() + + +def test_model_has_one_geometry_free_root_and_only_preserves_arm_spacing() -> None: + root = _tree() + shared_base = root.find("./link[@name='dual_openyam_base']") + assert shared_base is not None + assert list(shared_base) == [] + + link_names = {link.attrib["name"] for link in root.findall("link")} + child_names = { + child.attrib["link"] + for joint in root.findall("joint") + if (child := joint.find("child")) is not None + } + assert link_names - child_names == {"dual_openyam_base"} + left = _origin_xyz(_joint(root, "left_arm_fixed_joint")) + right = _origin_xyz(_joint(root, "right_arm_fixed_joint")) + assert left == (0.0, 0.31, 0.0) + assert right == (0.0, -0.31, 0.0) + + +def test_both_arm_chains_are_symmetric_and_use_two_rad_s_limits() -> None: + root = _tree() + for index in range(1, 7): + left = _joint(root, f"left_joint{index}") + right = _joint(root, f"right_joint{index}") + assert left.attrib["type"] == right.attrib["type"] == "revolute" + for element_name in ("origin", "axis", "limit"): + left_element = left.find(element_name) + right_element = right.find(element_name) + assert left_element is not None and right_element is not None + assert left_element.attrib == right_element.attrib + limit = left.find("limit") + assert limit is not None + assert float(limit.attrib["velocity"]) == 2.0 + + +def test_model_exposes_twelve_arm_joints_and_fixed_finger_geometry() -> None: + model = pinocchio.buildModelFromUrdf(str(DUAL_OPENYAM_MODEL_PATH)) + expected = tuple(f"{side}_joint{index}" for side in ("left", "right") for index in range(1, 7)) + assert model.nq == 12 + assert model.nv == 12 + assert tuple(str(name) for name in model.names[1:]) == expected + + root = _tree() + for side in ("left", "right"): + for index in (7, 8): + joint = _joint(root, f"{side}_joint{index}") + assert joint.attrib["type"] == "fixed" + assert joint.find("axis") is None + assert joint.find("limit") is None + assert _joint(root, f"{side}_grasp_frame_joint").attrib["type"] == "fixed" + + +def test_model_contains_no_workbench_camera_or_environment_geometry() -> None: + root = _tree() + assert len(root.findall("link")) == 21 + assert len(root.findall("joint")) == 20 + xml = ET.tostring(root, encoding="unicode").lower() + for excluded in ( + "camera", + "table", + "cabinet", + "enclosure", + "gate", + "wall", + "bottle", + "bin", + " DualOpenYamPinkPoseTargetSolver: + task = _dual_openyam_quest_task + config = PoseTargetIKTaskConfig( + joint_names=tuple(task.joint_names), + robot_model=task.params["robot_model"], + target_frames=_TARGET_FRAMES, + pink=task.params["pink"], + max_joint_velocity_rad_s=task.params["max_joint_velocity_rad_s"], + joint_command_filter_cutoff_hz=task.params["joint_command_filter_cutoff_hz"], + ) + return DualOpenYamPinkPoseTargetSolver(config) + + +@pytest.mark.self_hosted +def test_solver_uses_nominal_posture_without_manipulability() -> None: + solver = _solver() + seed = JointState( + name=DUAL_OPENYAM_ARM_JOINTS, + position=DUAL_OPENYAM_HOME_JOINTS, + ) + targets = solver.frame_poses(seed, _TARGET_FRAMES) + + assert solver.step(targets, seed, 0.01) is not None + + context = next(iter(solver._control_contexts.values())) + assert context.tasks is not None + assert not any(name.startswith("manipulability/") for name in context.tasks) + assert context.tasks["posture/current"].target_q == pytest.approx(DUAL_OPENYAM_HOME_JOINTS) + + +@pytest.mark.self_hosted +def test_quest_solver_matches_a1z_target_tracking_speed() -> None: + solver = _solver() + state = JointState( + name=DUAL_OPENYAM_ARM_JOINTS, + position=DUAL_OPENYAM_HOME_JOINTS, + ) + initial = solver.frame_poses(state, _TARGET_FRAMES) + targets = { + frame_name: PoseStamped( + frame_id=pose.frame_id, + position=[pose.position.x, pose.position.y, pose.position.z + 0.1], + orientation=pose.orientation, + ) + for frame_name, pose in initial.items() + } + + for _ in range(25): + command = solver.step(targets, state, 0.01) + assert command is not None + state = command + + current = solver.frame_poses(state, _TARGET_FRAMES) + progress = np.mean( + [ + current[frame_name].position.z - initial[frame_name].position.z + for frame_name in _TARGET_FRAMES + ] + ) + assert progress >= 0.09