diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py index c85fd452ca..1076be533d 100644 --- a/dimos/hardware/whole_body/damiao/adapter.py +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -78,6 +78,11 @@ def __init__( unknown_buses = config.bus_devices.keys() - set(self.bus_names) if unknown_buses: raise ValueError(f"unknown CAN bus overrides: {sorted(unknown_buses)}") + unknown_grippers = set(config.passive_grippers) - set(self.gripper_joints) + if unknown_grippers: + raise ValueError(f"unknown passive grippers: {sorted(unknown_grippers)}") + if len(config.passive_grippers) != len(set(config.passive_grippers)): + raise ValueError("passive_grippers contains duplicate names") if len(self.bus_names) != len(set(self.bus_names)): raise ValueError("Damiao topology contains duplicate logical bus names") @@ -245,6 +250,13 @@ def activate(self) -> bool: for arm in self._arms.values(): arm.set_mode("mit") self._robot.enable() + for name in self._runtime_config.passive_grippers: + gripper = self._grippers[name] + gripper.set_mode("mit") + gripper.enable() + gripper.mit_control(0.0, 0.0, float(gripper.motor.position), 0.0, 0.0) + if self._runtime_config.passive_grippers: + self._robot.tick(self._runtime_config.tick_deadline_us) self._active = True self.read_motor_states() return True @@ -340,6 +352,8 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: commands[arm_count:], strict=True, ): + if name in self._runtime_config.passive_grippers: + continue if not np.isfinite(command.q) or not 0.0 <= command.q <= 1.0: raise ValueError(f"gripper {name!r} opening must be in [0, 1]") @@ -368,7 +382,11 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: for name in self.gripper_joints: opening = commands[offset].q - self._grippers[name].set_opening(opening) + if name in self._runtime_config.passive_grippers: + gripper = self._grippers[name] + gripper.mit_control(0.0, 0.0, float(gripper.motor.position), 0.0, 0.0) + else: + self._grippers[name].set_opening(opening) offset += 1 self._robot.tick(self._runtime_config.tick_deadline_us) diff --git a/dimos/hardware/whole_body/damiao/config.py b/dimos/hardware/whole_body/damiao/config.py index d9b95092e2..ce0c3829bf 100644 --- a/dimos/hardware/whole_body/damiao/config.py +++ b/dimos/hardware/whole_body/damiao/config.py @@ -31,4 +31,5 @@ class DamiaoRuntimeConfig: bus_devices: dict[_NonEmptyString, _NonEmptyString] = Field(default_factory=dict) gravity_comp: bool = Field(default=True, strict=True) + passive_grippers: tuple[_NonEmptyString, ...] = () tick_deadline_us: int = Field(default=1_000, ge=1, strict=True) diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index 8345fdde63..9711b2faa5 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -65,14 +65,31 @@ def mit_control(self, commands: np.ndarray) -> None: class FakeGripper: def __init__(self, opening: float) -> None: self.opening = opening + self.motor = Mock(position=opening) self.command_error: Exception | None = None self.commands: list[float] = [] + self.modes: list[str] = [] + self.mit_commands: list[tuple[float, float, float, float, float]] = [] + self.enable_count = 0 + self.disable_count = 0 def set_opening(self, opening: float) -> None: if self.command_error is not None: raise self.command_error self.commands.append(opening) + def disable(self) -> None: + self.disable_count += 1 + + def enable(self) -> None: + self.enable_count += 1 + + def set_mode(self, mode: str) -> None: + self.modes.append(mode) + + def mit_control(self, kp: float, kd: float, q: float, dq: float, tau: float) -> None: + self.mit_commands.append((kp, kd, q, dq, tau)) + class FakeTransport: def __init__(self) -> None: @@ -296,6 +313,14 @@ def test_init_unknown_bus_override_raises_value_error(dual_robot: FakeRobot) -> ) +def test_init_unknown_passive_gripper_raises_value_error(dual_robot: FakeRobot) -> None: + with pytest.raises(ValueError, match="unknown passive grippers"): + DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig(passive_grippers=("missing",)), + ) + + def test_init_duplicate_logical_bus_names_raises_value_error(dual_robot: FakeRobot) -> None: class DuplicateBusAdapter(DualAdapter): bus_names = ("left", "left") @@ -360,12 +385,14 @@ def test_init_rehydrates_serialized_runtime_config(dual_robot: FakeRobot) -> Non runtime_config={ "bus_devices": {"left": "can8"}, "gravity_comp": False, + "passive_grippers": ["left_gripper"], "tick_deadline_us": 2_000, }, ) assert adapter._runtime_config.bus_devices == {"left": "can8"} assert adapter._runtime_config.gravity_comp is False + assert adapter._runtime_config.passive_grippers == ("left_gripper",) assert adapter._runtime_config.tick_deadline_us == 2_000 @@ -570,6 +597,32 @@ def test_activate_enable_failure_disables_robot( assert dual_robot.disable_count == 1 +def test_activate_enables_zero_impedance_for_configured_passive_gripper( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory( + dual_robot, + runtime_config=DamiaoRuntimeConfig( + gravity_comp=False, + passive_grippers=("left_gripper",), + ), + ) + assert adapter.connect() + + assert adapter.activate() + + left_gripper = cast("FakeGripper", dual_robot["left_gripper"]) + right_gripper = cast("FakeGripper", dual_robot["right_gripper"]) + assert left_gripper.disable_count == 0 + assert left_gripper.modes == ["mit"] + assert left_gripper.enable_count == 1 + assert left_gripper.mit_commands == [(0.0, 0.0, 0.5, 0.0, 0.0)] + assert right_gripper.modes == [] + assert right_gripper.enable_count == 0 + assert right_gripper.mit_commands == [] + + def test_deactivate_connected_adapter_disables_robot( active_dual_adapter: DualAdapter, dual_robot: FakeRobot, @@ -756,6 +809,32 @@ def test_write_motor_commands_grippers_routes_normalized_openings( assert cast("FakeGripper", dual_robot["right_gripper"]).commands == [0.75] +def test_write_motor_commands_keeps_passive_gripper_at_zero_impedance( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory( + dual_robot, + runtime_config=DamiaoRuntimeConfig( + gravity_comp=False, + passive_grippers=("left_gripper",), + ), + ) + assert adapter.connect() + assert adapter.activate() + commands = [MotorCommand(q=0.0)] * 4 + [MotorCommand(q=2.0), MotorCommand(q=0.75)] + + assert adapter.write_motor_commands(commands) + + left_gripper = cast("FakeGripper", dual_robot["left_gripper"]) + assert left_gripper.commands == [] + assert left_gripper.mit_commands == [ + (0.0, 0.0, 0.5, 0.0, 0.0), + (0.0, 0.0, 0.5, 0.0, 0.0), + ] + assert cast("FakeGripper", dual_robot["right_gripper"]).commands == [0.75] + + def test_write_motor_commands_combined_command_ticks_once( active_dual_adapter: DualAdapter, dual_robot: FakeRobot, diff --git a/dimos/imitation/README.md b/dimos/imitation/README.md index 48b0ebcbda..a4da33e479 100644 --- a/dimos/imitation/README.md +++ b/dimos/imitation/README.md @@ -84,3 +84,16 @@ Use `openyam-lerobot-quest-rollout` for optional Quest takeover. The Blueprint uses the existing single-arm, single-camera LeRobot contract. Configure devices through standard module options. Python clients discover `RolloutControlSpec` and explicitly call preflight/start/stop; disconnecting is not a stop request. + +## OpenYAM hand-guided collection + +```bash +dimos --can-port follower_l run openyam-teach-collection \ + --recorder.recording recordings/teach-001 \ + --episodes.task "pick up the cube" +``` + +Guide the arm and gripper by hand. This Blueprint uses gravity compensation, +zero position stiffness, joint damping, and a passive gripper. State and action +both project the measured joint positions. Use `EpisodeControlSpec` to start, +save, or discard episodes; support the arm before stopping the runtime. diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index c8aecb3cdf..a45a8ef732 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -101,6 +101,7 @@ "openyam-lerobot-rollout": "dimos.robot.manipulators.openyam.blueprints.learning_rollout:openyam_lerobot_rollout", "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator", "openyam-quest-collection": "dimos.robot.manipulators.openyam.blueprints.learning_quest_collection:openyam_quest_collection", + "openyam-teach-collection": "dimos.robot.manipulators.openyam.blueprints.learning_collection:openyam_teach_collection", "pointlio-rust": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:pointlio_rust", "pointlio-rust-replay": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:pointlio_rust_replay", "r1pro-coordinator": "dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_coordinator:r1pro_coordinator", diff --git a/dimos/robot/manipulators/openyam/blueprints/learning_collection.py b/dimos/robot/manipulators/openyam/blueprints/learning_collection.py new file mode 100644 index 0000000000..10e4f1b50c --- /dev/null +++ b/dimos/robot/manipulators/openyam/blueprints/learning_collection.py @@ -0,0 +1,82 @@ +# 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. + +"""Gravity-compensated OpenYAM collection, configured through dimos run.""" + +from dataclasses import replace + +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.hardware.sensors.camera.module import CameraModule +from dimos.hardware.sensors.camera.webcam import WebcamConfig +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.spec import WholeBodyConfig +from dimos.imitation.collection.episode_monitor import EpisodeMonitorModule +from dimos.imitation.collection.native_recorder import collection_recorder +from dimos.robot.manipulators.openyam.collection import OPENYAM_TEACH_COLLECTION +from dimos.robot.manipulators.openyam.config import OPENYAM_JOINTS, openyam_hardware + +OPENYAM_TEACH_DAMPING = (2.0, 2.0, 2.0, 0.5, 0.5, 0.5, 0.0) + + +def _teach_robot() -> Blueprint: + hardware = openyam_hardware() + if hardware.adapter_type == "openyam_damiao": + runtime_config = hardware.adapter_kwargs["runtime_config"] + if not isinstance(runtime_config, DamiaoRuntimeConfig): + raise TypeError("OpenYAM Damiao hardware requires DamiaoRuntimeConfig") + hardware = replace( + hardware, + adapter_kwargs={ + **hardware.adapter_kwargs, + "runtime_config": replace(runtime_config, passive_grippers=("gripper",)), + }, + ) + hardware = replace( + hardware, + wb_config=WholeBodyConfig(kp=(0.0,) * len(OPENYAM_JOINTS), kd=OPENYAM_TEACH_DAMPING), + ) + return ControlCoordinator.blueprint( + instance_name="ControlCoordinator", + hardware=[hardware], + tasks=[ + TaskConfig( + name="teach_openyam", + type="trajectory", + joint_names=list(OPENYAM_JOINTS), + priority=10, + params={"hold_position_when_idle": True}, + ) + ], + ) + + +openyam_teach_collection = autoconnect( + _teach_robot(), + CameraModule.blueprint( + instance_name="wrist", + hardware=WebcamConfig( + camera_index=0, width=640, height=480, fps=30, frame_id_prefix="wrist_image" + ), + frame_id="wrist_camera_link", + ), + collection_recorder(profile=OPENYAM_TEACH_COLLECTION), + EpisodeMonitorModule.blueprint(instance_name="episodes"), +).remappings( + [ + ("wrist", "color_image", "wrist_image"), + ("wrist", "camera_info", "wrist_camera_info"), + ("wrist", "tf", "wrist_tf"), + ] +) diff --git a/dimos/robot/manipulators/openyam/blueprints/test_learning_collection.py b/dimos/robot/manipulators/openyam/blueprints/test_learning_collection.py new file mode 100644 index 0000000000..ef5dcbbf57 --- /dev/null +++ b/dimos/robot/manipulators/openyam/blueprints/test_learning_collection.py @@ -0,0 +1,83 @@ +# 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.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser +from dimos.hardware.sensors.camera.module import CameraModule +from dimos.imitation.collection.episode_monitor import EpisodeMonitorModule +from dimos.robot.manipulators.openyam.blueprints.learning_collection import ( + openyam_teach_collection, +) +from dimos.robot.manipulators.openyam.blueprints.learning_quest_collection import ( + openyam_quest_collection, +) +from dimos.robot.manipulators.openyam.config import OPENYAM_JOINTS + + +@pytest.mark.parametrize("blueprint", [openyam_teach_collection, openyam_quest_collection]) +def test_collection_uses_normal_module_configuration(blueprint, tmp_path): + parsed = BlueprintConfigParser(blueprint).parse( + [ + "--recorder.recording", + str(tmp_path / "session"), + "--episodes.task", + "pick up the block", + "--wrist.hardware.camera-index", + "/dev/video3", + ], + environ={}, + ) + recorder = next(atom for atom in blueprint.active_blueprints if atom.name == "recorder") + assert recorder.kwargs["recording_schema"].robot_type == "openyam" + assert parsed.module_configs["recorder"]["recording"] == tmp_path / "session" + assert parsed.module_configs["wrist"]["hardware"]["camera_index"] == "/dev/video3" + + +def test_teach_collection_is_a_minimal_native_stack(): + modules = [atom.module for atom in openyam_teach_collection.active_blueprints] + assert modules[:2] == [ControlCoordinator, CameraModule] + assert modules[-1] is EpisodeMonitorModule + assert len(modules) == 4 + + +def test_openyam_teach_collection_uses_gravity_compensation_and_zero_stiffness( + tmp_path, +) -> None: + blueprint = openyam_teach_collection + coordinator = next( + atom for atom in blueprint.active_blueprints if atom.module is ControlCoordinator + ) + hardware = coordinator.kwargs["hardware"][0] + assert hardware.joints == OPENYAM_JOINTS + assert hardware.wb_config is not None + assert hardware.wb_config.kp == (0.0,) * len(OPENYAM_JOINTS) + assert hardware.wb_config.kd == (2.0, 2.0, 2.0, 0.5, 0.5, 0.5, 0.0) + if hardware.adapter_type == "openyam_damiao": + assert hardware.adapter_kwargs["runtime_config"].gravity_comp is True + assert hardware.adapter_kwargs["runtime_config"].passive_grippers == ("gripper",) + + tasks = coordinator.kwargs["tasks"] + assert [ + (task.name, task.type, task.joint_names, task.priority, task.params) for task in tasks + ] == [ + ( + "teach_openyam", + "trajectory", + OPENYAM_JOINTS, + 10, + {"hold_position_when_idle": True}, + ), + ] diff --git a/dimos/robot/manipulators/openyam/collection.py b/dimos/robot/manipulators/openyam/collection.py index 0ff972b22e..a45e4c72bd 100644 --- a/dimos/robot/manipulators/openyam/collection.py +++ b/dimos/robot/manipulators/openyam/collection.py @@ -64,3 +64,4 @@ def _profile( OPENYAM_QUEST_COLLECTION = _profile( "openyam-quest", "applied_joint_position_command", action_source_kind="joint_position_updates" ) +OPENYAM_TEACH_COLLECTION = _profile("openyam-teach", "coordinator_joint_state")