diff --git a/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py b/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py index 1fae10de3d..deed610f77 100644 --- a/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py +++ b/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py @@ -125,12 +125,6 @@ 2.0, # right arm ] -# Relaxed arms-down pose. The policy treats all 14 arm defaults as zero. -# Operators can override at runtime by publishing joint targets on the -# arms via the coordinator's joint_command transport. -ARM_DEFAULT_POSE: list[float] = [0.0] * 14 - - # Default joint angles for all 29 G1 joints. The policy treats these as # its zero-offset pose. _DEFAULT_POSITIONS_29 = [ diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index d3526dbc0d..1e52460ac0 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -33,7 +33,11 @@ make_twist_base_joints, ) from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.control.hardware_interface import ConnectedHardware, ConnectedTwistBase +from dimos.control.hardware_interface import ( + ConnectedHardware, + ConnectedTwistBase, + ConnectedWholeBody, +) from dimos.control.task import ( BaseControlTask, ControlMode, @@ -53,6 +57,7 @@ from dimos.control.tick_loop import TickLoop from dimos.core.stream import In from dimos.hardware.manipulators.spec import ManipulatorAdapter +from dimos.hardware.whole_body.spec import MotorState from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -237,6 +242,33 @@ def test_write_command(self, connected_hardware, mock_adapter): mock_adapter.write_joint_positions.assert_called() +class TestConnectedWholeBody: + def test_partial_commands_retain_last_targets_for_omitted_joints(self) -> None: + adapter = MagicMock() + adapter.has_motor_states.return_value = True + adapter.read_motor_states.return_value = [ + MotorState(q=0.1), + MotorState(q=0.2), + MotorState(q=0.3), + ] + adapter.write_motor_commands.return_value = True + hardware = ConnectedWholeBody( + adapter, + HardwareComponent( + hardware_id="robot", + hardware_type=HardwareType.WHOLE_BODY, + joints=["robot/leg", "robot/waist", "robot/arm"], + ), + ) + + assert hardware.write_command({"robot/arm": 0.8}, ControlMode.SERVO_POSITION) + assert hardware.write_command({"robot/leg": -0.4}, ControlMode.SERVO_POSITION) + + commands = adapter.write_motor_commands.call_args.args[0] + assert [command.q for command in commands] == [-0.4, 0.2, 0.8] + assert [command.kp for command in commands] == [40.0, 40.0, 40.0] + + @pytest.fixture def make_coordinator() -> Iterator[Callable[..., ControlCoordinator]]: """Factory for real coordinators, all stopped on teardown.""" @@ -261,27 +293,6 @@ class _EEFTwistCoordinator(ControlCoordinator): class TestControlCoordinatorLifecycle: - def test_dispatch_routes_ee_twist_only_to_matching_frame_id(self, make_coordinator): - coordinator = make_coordinator() - matching_task = RecordingTask("eef") - other_task = RecordingTask("other") - coordinator._tasks = {"eef": matching_task, "other": other_task} - coordinator._routes = { - "coordinator_ee_twist_command": [ - (matching_task, "on_ee_twist_command", Routing.BY_TASK_NAME), - (other_task, "on_ee_twist_command", Routing.BY_TASK_NAME), - ] - } - - for frame_id in ("eef", "missing", ""): - coordinator._dispatch( - "coordinator_ee_twist_command", - TwistStamped(frame_id=frame_id, linear=[0.1, 0.0, 0.0], angular=[0.0, 0.0, 0.0]), - ) - - assert len(matching_task.ee_twist_calls) == 1 - assert other_task.ee_twist_calls == [] - def test_start_subscribes_ee_twist_only_for_eef_twist_tasks(self, make_coordinator, mocker): mocker.patch("dimos.core.module.Module.start") mocker.patch("dimos.control.coordinator.TickLoop") diff --git a/dimos/core/coordination/blueprint_config/parser.py b/dimos/core/coordination/blueprint_config/parser.py index ce8338ec66..ebfcf51f80 100644 --- a/dimos/core/coordination/blueprint_config/parser.py +++ b/dimos/core/coordination/blueprint_config/parser.py @@ -75,6 +75,7 @@ plain, plain_mapping, snapshot_mapping, + validated_model_values, ) from dimos.core.coordination.blueprints import ( Blueprint, @@ -421,7 +422,7 @@ def _validate_modules( raise BlueprintConfigError( format_validation_error(module.atom.name, error) ) from error - dumped = model.model_dump(mode="python", exclude_unset=True) + dumped = validated_model_values(model, exclude_unset=True) dumped.pop("g", None) dumped.pop("instance_name", None) parsed[module.atom.name] = dumped @@ -463,7 +464,7 @@ def _validate_transports( ) from error if not models: continue - full = models[0].model_dump(mode="python") + full = validated_model_values(models[0]) if raw_overrides: parsed[transport.name] = extract_shape(full, raw_overrides) return parsed diff --git a/dimos/core/coordination/blueprint_config/test_parser.py b/dimos/core/coordination/blueprint_config/test_parser.py index b75ee52707..115cfb6197 100644 --- a/dimos/core/coordination/blueprint_config/test_parser.py +++ b/dimos/core/coordination/blueprint_config/test_parser.py @@ -13,6 +13,7 @@ # limitations under the License. from collections.abc import Callable +from dataclasses import dataclass from pathlib import Path from typing import Annotated, Any, Literal @@ -504,6 +505,26 @@ def test_blueprint_pinned_arbitrary_value_survives_filtering() -> None: assert isinstance(parsed.module_kwargs("arbitrarymodule")["scaling"], Anchor) +def test_blueprint_pinned_callable_dataclass_survives_validation() -> None: + @dataclass + class CallableFactory: + result: str + + def __call__(self, _value: Any) -> str: + return self.result + + factory = CallableFactory(result="rendered") + parsed = BlueprintConfigParser(ArbitraryModule.blueprint(handlers={"visual": factory})).parse( + environ={} + ) + + kwargs = parsed.module_kwargs("arbitrarymodule") + validated = ArbitraryConfig.model_validate(kwargs) + + assert isinstance(validated.handlers["visual"], CallableFactory) + assert validated.handlers["visual"](None) == "rendered" + + def test_format_help_uses_nested_parent_default_instance() -> None: class NestedRequiredConfig(BaseModel): value: int diff --git a/dimos/core/coordination/blueprint_config/values.py b/dimos/core/coordination/blueprint_config/values.py index 39cab50442..c7a6054064 100644 --- a/dimos/core/coordination/blueprint_config/values.py +++ b/dimos/core/coordination/blueprint_config/values.py @@ -55,6 +55,41 @@ def plain(value: Any) -> Any: return _copy_opaque(value) +def validated_model_values(model: BaseModel, *, exclude_unset: bool = False) -> dict[str, Any]: + """Export validated fields while preserving opaque Python values. + + Pydantic's ``model_dump`` expands dataclass instances, including callable + factories, into dictionaries. Module kwargs cross another validation + boundary in their worker process, where those dictionaries no longer + satisfy callable-typed fields. + """ + included = model.model_fields_set if exclude_unset else type(model).model_fields + return { + name: _validated_value(getattr(model, name), exclude_unset=exclude_unset) + for name in type(model).model_fields + if name in included + } + + +def _validated_value(value: Any, *, exclude_unset: bool) -> Any: + if isinstance(value, BaseModel): + return validated_model_values(value, exclude_unset=exclude_unset) + if isinstance(value, Mapping): + return { + _copy_opaque(key): _validated_value(item, exclude_unset=exclude_unset) + for key, item in value.items() + } + if isinstance(value, list): + return [_validated_value(item, exclude_unset=exclude_unset) for item in value] + if isinstance(value, tuple): + return tuple(_validated_value(item, exclude_unset=exclude_unset) for item in value) + if isinstance(value, set): + return {_validated_value(item, exclude_unset=exclude_unset) for item in value} + if isinstance(value, frozenset): + return frozenset(_validated_value(item, exclude_unset=exclude_unset) for item in value) + return _copy_opaque(value) + + def deep_merge(destination: dict[str, Any], incoming: Mapping[str, Any]) -> None: for key, value in incoming.items(): if key in destination and isinstance(destination[key], dict) and isinstance(value, Mapping): diff --git a/dimos/manipulation/planning/kinematics/pink_solver.py b/dimos/manipulation/planning/kinematics/pink_solver.py index 06152bcbc4..82a4926ebb 100644 --- a/dimos/manipulation/planning/kinematics/pink_solver.py +++ b/dimos/manipulation/planning/kinematics/pink_solver.py @@ -232,6 +232,7 @@ def _build_robot_context( ) model = pinocchio.buildModelFromUrdf(str(prepared_path)) + model = _reduce_to_controlled_joints(model, config, controlled_joints) data = model.createData() _assert_base_link_is_model_root(model, config.base_link) frame_id = _get_frame_id(model, frame_name) @@ -285,6 +286,29 @@ def _target_in_model_frame( return target_model +def _reduce_to_controlled_joints( + model: pinocchio.Model, + config: RobotModelConfig, + controlled_joints: Sequence[str] | None, +) -> pinocchio.Model: + """Lock joints outside the solve so IK cannot exploit uncommanded motion.""" + dimos_joint_names = tuple(controlled_joints or config.joint_names) + controlled_joint_ids = { + _get_joint_id(model, config.get_urdf_joint_name(joint_name)) + for joint_name in dimos_joint_names + } + locked_joint_ids = [ + joint_id for joint_id in range(1, len(model.joints)) if joint_id not in controlled_joint_ids + ] + if not locked_joint_ids: + return model + return pinocchio.buildReducedModel( + model, + locked_joint_ids, + np.asarray(pinocchio.neutral(model), dtype=np.float64), + ) + + def _build_joint_mapping( model: pinocchio.Model, config: RobotModelConfig, diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index f3e4c9efaf..5377bbb9c6 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -348,6 +348,27 @@ def _robot_config() -> RobotModelConfig: ) +def test_reduce_to_controlled_joints_locks_every_other_joint(mocker: MockerFixture) -> None: + modules = _install_fake_modules(mocker) + model = _FakeModel() + reduced = _FakeModel() + modules.pinocchio.neutral = lambda source: np.zeros(source.nq) + build_reduced_model = mocker.Mock(return_value=reduced) + modules.pinocchio.buildReducedModel = build_reduced_model + + result = pink_ik._reduce_to_controlled_joints( + model, + _robot_config(), + ["joint_a"], + ) + + assert result is reduced + args = build_reduced_model.call_args.args + assert args[0] is model + assert args[1] == [1, 3] + assert args[2] == pytest.approx([0.0, 0.0, 0.0]) + + def _streaming_ik(mocker: MockerFixture, converge: bool = True) -> _StreamingTestPinkIK: _install_fake_modules(mocker, converge=converge) return _StreamingTestPinkIK(PinkIKConfig(max_iterations=3)) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 953a5f0b47..383df6157e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -122,6 +122,7 @@ "unitree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_primitive_no_nav:unitree_g1_primitive_no_nav", "unitree-g1-shm": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1_shm:unitree_g1_shm", "unitree-g1-sim": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1_sim:unitree_g1_sim", + "unitree-g1-teleop": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_teleop:unitree_g1_teleop", "unitree-go2": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2", "unitree-go2-agentic": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic:unitree_go2_agentic", "unitree-go2-agentic-huggingface": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_huggingface:unitree_go2_agentic_huggingface", @@ -199,6 +200,7 @@ "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", "fast-lio2-recorder": "dimos.hardware.sensors.lidar.fastlio2.recorder.FastLio2Recorder", "front-camera": "dimos.teleop.hosted.blueprints.cloudflare.FrontCamera", + "g1-collection-recorder": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_teleop.G1CollectionRecorder", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase", "g1-high-level-dds-sdk": "dimos.robot.unitree.g1.effectors.high_level.dds_sdk.G1HighLevelDdsSdk", @@ -240,6 +242,7 @@ "mid360-realsense-recorder": "dimos.robot.assembly.mid360_realsense_30.Mid360RealsenseRecorder", "mid360-realsense-static-tf": "dimos.robot.assembly.mid360_realsense_30.Mid360RealsenseStaticTf", "mls-planner-native": "dimos.navigation.nav_3d.mls_planner.mls_planner_native.MLSPlannerNative", + "mobile-video-arm-teleop-module": "dimos.teleop.quest.quest_extensions.MobileVideoArmTeleopModule", "mock-b1-connection-module": "dimos.robot.unitree.b1.connection.MockB1ConnectionModule", "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA", "module-b": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleB", diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index f9604c57d6..5ad6442ddb 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -46,24 +46,27 @@ from __future__ import annotations +import math from pathlib import Path from typing import Any, cast from dimos.control.components import HardwareComponent, HardwareType -from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.coordinator import TaskConfig from dimos.control.tasks.g1_groot_wbc_task.g1_groot_wbc_task import ( - ARM_DEFAULT_POSE, G1_GROOT_KD, G1_GROOT_KP, g1_arms, g1_joints, g1_legs_waist, ) +from dimos.control.teleop_coordinator import TeleopControlCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.stream import Out from dimos.core.transport import LCMTransport from dimos.hardware.whole_body.spec import WholeBodyConfig +from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig +from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig from dimos.msgs.geometry_msgs.Twist import Twist @@ -80,6 +83,7 @@ g1_urdf_joint_state, g1_urdf_static_robot, ) +from dimos.robot.unitree.g1.teleop_ik import G1PinkPoseTargetSolver from dimos.simulation.scene_assets.spec import ScenePackage from dimos.utils.data import LfsPath from dimos.visualization.rerun.scene_package import scene_package_static_entities @@ -132,7 +136,7 @@ _G1_NAV_SAFE_RADIUS_MARGIN = 0.6 -class _G1GrootCoordinator(ControlCoordinator): +class _G1GrootCoordinator(TeleopControlCoordinator): g1_joints: Out[JointState] @@ -278,14 +282,6 @@ def _precomposed_g1_scene(package: ScenePackage) -> Path | None: _default_ramp_seconds = 0.0 _decimation: int | None = 1 _n_workers = 2 # sim: keep the default worker count - _arm_holder = TaskConfig( - name="servo_arms", - type="servo", - joint_names=g1_arms, - priority=10, - auto_start=True, - params={"default_positions": ARM_DEFAULT_POSE}, - ) _mapper = VoxelGridMapper.blueprint(emit_every=1) _nav_stack = autoconnect( _mapper, @@ -327,16 +323,6 @@ def _precomposed_g1_scene(package: ScenePackage) -> Path | None: _decimation = 2 # 100 Hz tick / 2 = 50 Hz policy (training + sim rate). # One process per heavy module; fewer workers starve the Rerun bridge. _n_workers = 10 - # Real hardware needs the arms held -- kd damping alone would let - # them sag toward singular configurations between trajectories. - _arm_holder = TaskConfig( - name="servo_arms", - type="servo", - joint_names=g1_arms, - priority=10, - auto_start=True, - params={"default_positions": ARM_DEFAULT_POSE}, - ) # Same nav middle as unitree-g1-nav-simple, fed by Point-LIO from the # MID-360, executed through the coordinator's twist_command. _nav_stack = autoconnect( @@ -392,6 +378,26 @@ def _g1_nav_path(path: NavPath) -> Any: _G1_ROOT = G1_RERUN_ROOT if global_config.simulation == "mujoco" else "world/odometry/g1" _G1_URDF_PATH = Path(__file__).resolve().parents[2] / "g1.urdf" +G1_TELEOP_TASK_NAME = "teleop_g1" +_G1_ARM_JOINT_NAME_MAPPING = { + joint_name: f"{joint_name.partition('/')[2]}_joint" for joint_name in g1_arms +} +_G1_TELEOP_MODEL = RobotModelConfig( + name="g1_arms", + model_path=_G1_URDF_PATH, + joint_names=list(_G1_ARM_JOINT_NAME_MAPPING.values()), + base_link="pelvis", + joint_name_mapping=_G1_ARM_JOINT_NAME_MAPPING, +) +_G1_TELEOP_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=0.25, +) # Nominal standing pelvis height; matches G1GrootWBCTask's height_cmd. _G1_NOMINAL_PELVIS_Z = 0.74 _g1_pelvis_mid360_cache: list[Any] = [] @@ -518,7 +524,26 @@ def _viewer() -> Any: "decimation": _decimation, }, ), - *([_arm_holder] if _arm_holder is not None else []), + # Shared bimanual Quest task with G1-only model and objective tuning. + TaskConfig( + name=G1_TELEOP_TASK_NAME, + type="teleop_ik", + joint_names=g1_arms, + priority=20, + params={ + "robot_model": _G1_TELEOP_MODEL, + "bindings": [ + {"hand": "left", "target_frame": "left_rubber_hand"}, + {"hand": "right", "target_frame": "right_rubber_hand"}, + ], + "solver_type": G1PinkPoseTargetSolver, + "pink": _G1_TELEOP_PINK, + "timeout": 0.5, + "max_command_tracking_error_deg": 10.0, + "max_joint_velocity_rad_s": math.radians(120.0), + "joint_command_filter_cutoff_hz": 5.0, + }, + ), ], ).transports( { diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_teleop.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_teleop.py new file mode 100644 index 0000000000..9f1ce7b42e --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_teleop.py @@ -0,0 +1,150 @@ +# Copyright 2025-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. + +"""Unitree G1 GR00T WBC + Quest teleop + episode recording. + +The full ``unitree-g1-groot-wbc`` stack (locomotion policy, nav, viewer, +``--simulation mujoco`` / ``--scene-package`` support) plus the Quest WebXR +retargeting module and the dimos.imitation data-collection stack. Put on the +headset, open ``https://:8443/teleop``, and: + + left stick walk forward/back (+ yaw in strafe mode) + right stick yaw (press = zero-Twist e-stop) + X + A hold to track both arms from a shared reference + B start / save an episode + Y discard the in-progress episode + +Controller poses route to the shared ``teleop_g1`` coordinator task declared +in the groot blueprint. Locomotion enters ``MovementManager.tele_cmd_vel`` so +operator input cancels navigation before reaching the GR00T WBC task. + +Recording runs continuously into a timestamped session DB under +``~/.local/state/dimos/recordings/``; B/Y only place episode markers +(EpisodeMonitorModule). Off-sim, a RealSense provides ``color_image`` — +recorded for training and pushed into the headset as the operator's view. +The groot MuJoCo sim publishes no color camera, so sim sessions record +joints/commands only (point DataPrep's sync anchor at joint state, or +enable a sim color camera, if you need images from sim). + +Export afterwards with ``dimos dataprep build`` — measured joint state, +the commanded wrist poses, and episode status are all in the DB, so +action semantics (next-state vs commanded) are a DataPrep config choice. + +Usage: + dimos --simulation mujoco --scene-package office run unitree-g1-teleop + dimos run unitree-g1-teleop # real hardware +""" + +from __future__ import annotations + +from datetime import datetime + +from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, STATE_DIR +from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.core.global_config import global_config +from dimos.core.stream import In +from dimos.core.transport import pSHMTransport +from dimos.imitation.collection.episode_monitor import EpisodeMonitorModule +from dimos.imitation.collection.recorder import CollectionRecorder +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.unitree.g1.blueprints.basic.unitree_g1_groot_wbc import ( + G1_TELEOP_TASK_NAME, + unitree_g1_groot_wbc, +) +from dimos.teleop.quest.quest_extensions import MobileVideoArmTeleopModule + + +class G1CollectionRecorder(CollectionRecorder): + """CollectionRecorder plus the operator's absolute controller poses. + + The shared teleop IK captures controller and robot references internally, + so joint commands do not appear on a stream. Recording both controller + streams preserves the operator input alongside measured joint state. + """ + + # Own process: sqlite/eMMC writes and the torch import must not share + # a GIL with control modules. + dedicated_worker = True + + left_cartesian_command: In[PoseStamped] + right_cartesian_command: In[PoseStamped] + + +def _session_db() -> str: + return str(STATE_DIR / "recordings" / f"session_g1_{datetime.now():%Y%m%d_%H%M%S}.db") + + +if not global_config.simulation: + from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera + + class DedicatedRealSenseCamera(RealSenseCamera): + """Own process: 15 fps frame copies must not share a GIL with the + coordinator's tick loop (measured arm latency when colocated).""" + + dedicated_worker = True + + +def _camera_if_real() -> tuple[Blueprint, ...]: + """Real RealSense only off-sim: the groot MuJoCo sim exposes no color + camera, and instantiating the module with no device would fail.""" + if global_config.simulation: + return () + return (DedicatedRealSenseCamera.blueprint(enable_pointcloud=False),) + + +unitree_g1_teleop = ( + autoconnect( + unitree_g1_groot_wbc, + MobileVideoArmTeleopModule.blueprint( + task_names={"left": G1_TELEOP_TASK_NAME, "right": G1_TELEOP_TASK_NAME} + ), + *_camera_if_real(), + EpisodeMonitorModule.blueprint(), # default button_map: toggle=B, discard=Y + G1CollectionRecorder.blueprint( + db_path=_session_db(), + # Command/status streams have no tf frame to anchor a pose; + # declaring them avoids a per-message no-pose warning at + # teleop rates. + poseless_streams=[ + "status", + "left_cartesian_command", + "right_cartesian_command", + "coordinator_joint_state", + ], + ), + ) + .remappings( + [ + (MobileVideoArmTeleopModule, "left_controller_output", "left_cartesian_command"), + (MobileVideoArmTeleopModule, "right_controller_output", "right_cartesian_command"), + (MobileVideoArmTeleopModule, "cmd_vel", "tele_cmd_vel"), + ] + ) + # Camera frames stay off the LCM bus: every consumer (quest module, + # recorder, viewer bridge) is on-box, and raw images multicast over LCM + # make each subscribing process pay receive+decode per frame — measured + # at ~31 MB/s and a starved coordinator tick loop on the Orin. SHM is + # zero-copy; an unconsumed stream costs only the producer's write. + .transports( + { + ("color_image", Image): pSHMTransport( + "/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + ("depth_image", Image): pSHMTransport( + "/depth_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + } + ) +) diff --git a/dimos/robot/unitree/g1/teleop_ik.py b/dimos/robot/unitree/g1/teleop_ik.py new file mode 100644 index 0000000000..2e2320feaa --- /dev/null +++ b/dimos/robot/unitree/g1/teleop_ik.py @@ -0,0 +1,68 @@ +# Copyright 2025-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. + +"""G1-specific Pink objective tuning for bimanual 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 + +_FRAME_POSITION_COST = 8.0 +_FRAME_ORIENTATION_COST = 2.0 +_POSTURE_WEIGHTS = np.tile( + np.array([4.0, 3.0, 0.1, 3.0, 1.0, 1.0, 0.1], dtype=np.float64), + 2, +) +_NOMINAL_POSTURE = np.zeros(14, dtype=np.float64) + + +class G1PinkPoseTargetSolver(PinkPoseTargetSolver): + """Shape G1 arm redundancy while retaining common solve and safety logic.""" + + 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("G1PinkPoseTargetSolver requires a positive posture cost") + posture_task.cost = self.config.posture_cost * _POSTURE_WEIGHTS + 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("G1PinkPoseTargetSolver requires a posture task") + if configuration.model.nq != len(_NOMINAL_POSTURE): + raise ValueError( + f"G1 nominal posture has {len(_NOMINAL_POSTURE)} joints, " + f"model has {configuration.model.nq}" + ) + posture_task.set_target(_NOMINAL_POSTURE) diff --git a/dimos/robot/unitree/g1/test_g1_teleop.py b/dimos/robot/unitree/g1/test_g1_teleop.py new file mode 100644 index 0000000000..f7011d7805 --- /dev/null +++ b/dimos/robot/unitree/g1/test_g1_teleop.py @@ -0,0 +1,138 @@ +# Copyright 2025-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. + +"""Construction and objective tests for shared G1 Quest teleoperation.""" + +from typing import Any + +import numpy as np +import pytest + +from dimos.control.coordinator import TaskConfig +from dimos.control.tasks.g1_groot_wbc_task.g1_groot_wbc_task import g1_arms +from dimos.control.tasks.pose_target_ik import PoseTargetIKTaskConfig +from dimos.control.teleop_coordinator import TeleopControlCoordinator +from dimos.core.coordination.blueprints import Blueprint +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.robot.unitree.g1.blueprints.basic.unitree_g1_groot_wbc import ( + _G1_ARM_JOINT_NAME_MAPPING, + _G1_TELEOP_MODEL, + _G1_TELEOP_PINK, + G1_TELEOP_TASK_NAME, + unitree_g1_groot_wbc, +) +from dimos.robot.unitree.g1.blueprints.basic.unitree_g1_teleop import ( + G1CollectionRecorder, + unitree_g1_teleop, +) +from dimos.robot.unitree.g1.teleop_ik import G1PinkPoseTargetSolver +from dimos.teleop.quest.quest_extensions import MobileVideoArmTeleopModule + + +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 _teleop_task() -> TaskConfig: + coordinator = next( + atom + for atom in unitree_g1_groot_wbc.blueprints + if issubclass(atom.module, TeleopControlCoordinator) + ) + return next(task for task in coordinator.kwargs["tasks"] if task.type == "teleop_ik") + + +def test_g1_blueprint_uses_shared_bimanual_teleop_task() -> None: + task = _teleop_task() + + assert task.name == G1_TELEOP_TASK_NAME + assert task.joint_names == g1_arms + assert task.priority == 20 + assert task.params["robot_model"] is _G1_TELEOP_MODEL + assert task.params["solver_type"] is G1PinkPoseTargetSolver + assert task.params["bindings"] == [ + {"hand": "left", "target_frame": "left_rubber_hand"}, + {"hand": "right", "target_frame": "right_rubber_hand"}, + ] + assert _G1_TELEOP_MODEL.base_link == "pelvis" + assert _G1_TELEOP_MODEL.joint_name_mapping == _G1_ARM_JOINT_NAME_MAPPING + assert task.params["max_joint_velocity_rad_s"] == pytest.approx(np.deg2rad(120.0)) + + +def test_g1_blueprint_has_no_static_arm_holder() -> None: + coordinator = next( + atom + for atom in unitree_g1_groot_wbc.blueprints + if issubclass(atom.module, TeleopControlCoordinator) + ) + + arm_tasks = [ + task for task in coordinator.kwargs["tasks"] if set(task.joint_names) & set(g1_arms) + ] + + assert [(task.name, task.type) for task in arm_tasks] == [(G1_TELEOP_TASK_NAME, "teleop_ik")] + + +def test_g1_teleop_wires_arm_velocity_and_recording_streams() -> None: + teleop_kwargs = _module_kwargs(unitree_g1_teleop, MobileVideoArmTeleopModule) + + assert teleop_kwargs["task_names"] == { + "left": G1_TELEOP_TASK_NAME, + "right": G1_TELEOP_TASK_NAME, + } + assert ( + unitree_g1_teleop.remapping_map[(MobileVideoArmTeleopModule.name, "left_controller_output")] + == "left_cartesian_command" + ) + assert ( + unitree_g1_teleop.remapping_map[ + (MobileVideoArmTeleopModule.name, "right_controller_output") + ] + == "right_cartesian_command" + ) + assert ( + unitree_g1_teleop.remapping_map[(MobileVideoArmTeleopModule.name, "cmd_vel")] + == "tele_cmd_vel" + ) + assert "left_cartesian_command" in G1CollectionRecorder.__annotations__ + assert "right_cartesian_command" in G1CollectionRecorder.__annotations__ + + +@pytest.mark.self_hosted +def test_g1_pink_solver_reduces_model_and_uses_g1_objective() -> None: + frames = ("left_rubber_hand", "right_rubber_hand") + config = PoseTargetIKTaskConfig( + joint_names=tuple(g1_arms), + robot_model=_G1_TELEOP_MODEL, + target_frames=frames, + pink=_G1_TELEOP_PINK, + ) + solver = G1PinkPoseTargetSolver(config) + seed = JointState(name=list(g1_arms), position=[0.0] * len(g1_arms)) + targets = solver.frame_poses(seed, frames) + + command = solver.step(targets, seed, 0.01) + + assert command is not None + assert command.name == g1_arms + context = next(iter(solver._control_contexts.values())) + assert context.robot.model.nq == len(g1_arms) + assert context.tasks is not None + for frame_name in frames: + frame_task = context.tasks[f"frame/{frame_name}"] + assert frame_task.position_cost == pytest.approx([8.0, 8.0, 8.0]) + assert frame_task.orientation_cost == pytest.approx([2.0, 2.0, 2.0]) + posture = context.tasks["posture/current"] + assert posture.cost == pytest.approx(np.tile([4.0, 3.0, 0.1, 3.0, 1.0, 1.0, 0.1], 2) * 0.01) + assert posture.target_q == pytest.approx(np.zeros(len(g1_arms))) diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index dc41018ffd..a481e03117 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -19,11 +19,12 @@ - HandTeleopModule: Pinch-to-toggle arm teleop using WebXR hand tracking - TwistTeleopModule: Outputs Twist instead of PoseStamped - VideoArmTeleopModule: ArmTeleopModule + JPEG frames pushed to the Quest over /ws + - MobileVideoArmTeleopModule: Video arm teleop + thumbstick base velocity - Go2TeleopModule: Thumbstick → Twist velocity for the Go2 + camera over /ws """ import asyncio -from typing import Any +from typing import Any, Literal from fastapi import WebSocket @@ -238,6 +239,90 @@ async def handle_color_image(self, msg: Image) -> None: _push_jpeg(self, msg, self.config.video_jpeg_quality) +class MobileVideoArmTeleopConfig(VideoArmTeleopConfig): + """Configuration for combined arm, video, and mobile-base teleoperation.""" + + linear_scale: float = 0.3 + yaw_scale: float = 0.3 + strafe_scale: float = 0.3 + right_stick_mode: Literal["yaw", "strafe"] = "yaw" + deadzone: float = 0.18 + + +class MobileVideoArmTeleopModule(VideoArmTeleopModule): + """Video arm teleop with thumbstick velocity for a mobile manipulator.""" + + dedicated_worker = True + + config: MobileVideoArmTeleopConfig + + cmd_vel: Out[Twist] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._cmd_vel_moving = False + self._right_stick_pressed = False + + def _on_joy_bytes(self, data: bytes) -> None: + super()._on_joy_bytes(data) + with self._lock: + left = self._controllers.get(Hand.LEFT) + right = self._controllers.get(Hand.RIGHT) + self._publish_cmd_vel(left, right) + + def _publish_cmd_vel( + self, + left: QuestControllerState | None, + right: QuestControllerState | None, + ) -> None: + """Publish operator motion and one definitive stop per stop transition.""" + + def deadzone(value: float) -> float: + return 0.0 if abs(value) < self.config.deadzone else value + + right_stick_pressed = right is not None and right.thumbstick_press + if right_stick_pressed: + if not self._right_stick_pressed: + self.cmd_vel.publish(Twist.zero()) + self._right_stick_pressed = True + self._cmd_vel_moving = False + return + self._right_stick_pressed = False + + left_x = deadzone(left.thumbstick.x if left is not None else 0.0) + left_y = deadzone(left.thumbstick.y if left is not None else 0.0) + right_x = deadzone(right.thumbstick.x if right is not None else 0.0) + + vx = -left_y * self.config.linear_scale + vy = 0.0 + yaw_rate = 0.0 + if self.config.right_stick_mode == "strafe": + vy = -right_x * self.config.strafe_scale + yaw_rate = -left_x * self.config.yaw_scale + else: + yaw_rate = -right_x * self.config.yaw_scale + + moving = any(value != 0.0 for value in (vx, vy, yaw_rate)) + if moving: + self.cmd_vel.publish( + Twist( + linear=Vector3(vx, vy, 0.0), + angular=Vector3(0.0, 0.0, yaw_rate), + ) + ) + elif self._cmd_vel_moving: + self.cmd_vel.publish(Twist.zero()) + self._cmd_vel_moving = moving + + @rpc + def stop(self) -> None: + try: + self.cmd_vel.publish(Twist.zero()) + except Exception: + logger.exception("Failed to publish stop Twist") + super().stop() + + class Go2TeleopConfig(QuestTeleopConfig): """Configuration for Go2TeleopModule.""" diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 389d25eca9..d9d2b89aea 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -223,8 +223,15 @@ def _resolve_hand(frame_id: str) -> Hand: raise ValueError(f"Unexpected frame_id: {frame_id!r}, expected 'left' or 'right'") def _on_pose_bytes(self, data: bytes) -> None: - """Decode LCM bytes into PoseStamped, transform to robot frame.""" + """Decode LCM bytes into PoseStamped, transform to robot frame. + + Poses that aren't controller poses (e.g. the "head" viewer pose the + web client also streams) are ignored here; subclasses that want them + override this method. Raising instead would kill the websocket. + """ msg = PoseStamped.lcm_decode(data) + if msg.frame_id not in ("left", "right"): + return hand = self._resolve_hand(msg.frame_id) robot_pose = webxr_to_robot(msg, is_left_controller=(hand == Hand.LEFT)) with self._lock: diff --git a/dimos/teleop/quest/test_quest_teleop_module.py b/dimos/teleop/quest/test_quest_teleop_module.py index 07600b8adf..23a61efe4d 100644 --- a/dimos/teleop/quest/test_quest_teleop_module.py +++ b/dimos/teleop/quest/test_quest_teleop_module.py @@ -17,9 +17,18 @@ import pytest from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.teleop.quest.quest_extensions import ArmTeleopModule, HandTeleopModule +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.teleop.quest.quest_extensions import ( + ArmTeleopModule, + HandTeleopModule, + MobileVideoArmTeleopModule, +) from dimos.teleop.quest.quest_teleop_module import QuestTeleopModule -from dimos.teleop.quest.quest_types import Hand, QuestControllerState +from dimos.teleop.quest.quest_types import ( + Hand, + QuestControllerState, + ThumbstickState, +) @pytest.fixture @@ -112,3 +121,75 @@ def test_hand_teleop_pinch_toggles_engagement(mocker) -> None: assert not publish.call_args.args[0].right_primary finally: module.stop() + + +def _controller( + *, + is_left: bool, + stick_x: float = 0.0, + stick_y: float = 0.0, + thumbstick_press: bool = False, +) -> QuestControllerState: + return QuestControllerState( + is_left=is_left, + thumbstick_press=thumbstick_press, + thumbstick=ThumbstickState(x=stick_x, y=stick_y), + ) + + +def test_mobile_arm_teleop_publishes_yaw_drive_and_one_neutral_stop(mocker) -> None: + module = MobileVideoArmTeleopModule() + try: + publish = mocker.patch.object(module.cmd_vel, "publish") + left = _controller(is_left=True, stick_y=-1.0) + right = _controller(is_left=False, stick_x=0.5) + + module._publish_cmd_vel(left, right) + moving = publish.call_args.args[0] + assert isinstance(moving, Twist) + assert moving.linear.x == pytest.approx(module.config.linear_scale) + assert moving.linear.y == 0.0 + assert moving.angular.z == pytest.approx(-0.5 * module.config.yaw_scale) + + idle_left = _controller(is_left=True) + idle_right = _controller(is_left=False) + module._publish_cmd_vel(idle_left, idle_right) + module._publish_cmd_vel(idle_left, idle_right) + + assert publish.call_count == 2 + assert publish.call_args.args[0] == Twist.zero() + finally: + module.stop() + + +def test_mobile_arm_teleop_strafe_mode_and_deadzone(mocker) -> None: + module = MobileVideoArmTeleopModule(right_stick_mode="strafe") + try: + publish = mocker.patch.object(module.cmd_vel, "publish") + left = _controller(is_left=True, stick_x=0.5, stick_y=0.1) + right = _controller(is_left=False, stick_x=-0.5) + + module._publish_cmd_vel(left, right) + + moving = publish.call_args.args[0] + assert moving.linear.x == 0.0 + assert moving.linear.y == pytest.approx(0.5 * module.config.strafe_scale) + assert moving.angular.z == pytest.approx(-0.5 * module.config.yaw_scale) + finally: + module.stop() + + +def test_mobile_arm_teleop_stick_press_publishes_one_stop_per_press(mocker) -> None: + module = MobileVideoArmTeleopModule() + try: + publish = mocker.patch.object(module.cmd_vel, "publish") + left = _controller(is_left=True, stick_y=-1.0) + pressed = _controller(is_left=False, thumbstick_press=True) + + module._publish_cmd_vel(left, pressed) + module._publish_cmd_vel(left, pressed) + + assert publish.call_count == 1 + assert publish.call_args.args[0] == Twist.zero() + finally: + module.stop() diff --git a/dimos/teleop/quest/web/static/teleop.js b/dimos/teleop/quest/web/static/teleop.js index 63bbcbe81d..c3cca4ce87 100644 --- a/dimos/teleop/quest/web/static/teleop.js +++ b/dimos/teleop/quest/web/static/teleop.js @@ -263,6 +263,13 @@ function processTracking(frame) { } lastSendTime = now; + // Humanoid retargeting needs the headset pose in the same reference space + // as both hands. Other teleop modules ignore the "head" frame. + const viewerPose = frame.getViewerPose(xrRefSpace); + if (viewerPose) { + sendPose('head', viewerPose); + } + // Process controller and hand input sources. for (const inputSource of frame.session.inputSources) { const handedness = inputSource.handedness; @@ -321,8 +328,13 @@ function processTracking(frame) { // [4] = X/A button // [5] = Y/B button // [6] = menu (if exposed) + // Pad to at least 7 entries: the Python side + // (QuestControllerState.from_joy) requires the full layout, + // but browsers only report the buttons the controller has + // (e.g. 6 when no menu/thumbrest is exposed). const buttons = []; - for (let i = 0; i < gamepad.buttons.length; i++) { + const buttonCount = Math.max(gamepad.buttons.length, 7); + for (let i = 0; i < buttonCount; i++) { buttons.push(gamepad.buttons[i]?.pressed ? 1 : 0); } diff --git a/pyproject.toml b/pyproject.toml index 426c524cdf..bea80c830d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -177,9 +177,6 @@ Changelog = "https://github.com/dimensionalOS/dimos/releases" [project.optional-dependencies] misc = [ - # Core requirements - "python-multipart>=0.0.27", - # Developer Specific "ipykernel", @@ -241,6 +238,7 @@ agents = [ web = [ "fastapi>=0.115.6", + "python-multipart>=0.0.27", "sse-starlette>=2.2.1", "uvicorn>=0.34.0", "jinja2>=3.1.6", diff --git a/uv.lock b/uv.lock index a605fa5739..872d08cb58 100644 --- a/uv.lock +++ b/uv.lock @@ -1808,6 +1808,7 @@ base = [ { name = "openai" }, { name = "openevals" }, { name = "pillow" }, + { name = "python-multipart" }, { name = "rerun-sdk" }, { name = "sounddevice" }, { name = "soundfile" }, @@ -1871,7 +1872,6 @@ misc = [ { name = "ipykernel" }, { name = "open-clip-torch" }, { name = "portal" }, - { name = "python-multipart" }, { name = "tensorboard" }, { name = "timm" }, { name = "torchreid" }, @@ -1926,6 +1926,7 @@ unitree = [ { name = "openai" }, { name = "openevals" }, { name = "pillow" }, + { name = "python-multipart" }, { name = "rerun-sdk" }, { name = "sounddevice" }, { name = "soundfile" }, @@ -1961,6 +1962,7 @@ unitree-dds = [ { name = "openai" }, { name = "openevals" }, { name = "pillow" }, + { name = "python-multipart" }, { name = "rerun-sdk" }, { name = "sounddevice" }, { name = "soundfile" }, @@ -1982,6 +1984,7 @@ web = [ { name = "fastapi" }, { name = "ffmpeg-python" }, { name = "jinja2" }, + { name = "python-multipart" }, { name = "soundfile" }, { name = "sse-starlette" }, { name = "uvicorn" }, @@ -2255,7 +2258,7 @@ requires-dist = [ { name = "pymavlink", marker = "extra == 'drone'" }, { name = "pyrealsense2-extended", marker = "sys_platform != 'darwin' and extra == 'manipulation'" }, { name = "python-dotenv" }, - { name = "python-multipart", marker = "extra == 'misc'", specifier = ">=0.0.27" }, + { name = "python-multipart", marker = "extra == 'web'", specifier = ">=0.0.27" }, { name = "pyturbojpeg", specifier = "==1.8.2" }, { name = "pyyaml", marker = "extra == 'manipulation'", specifier = ">=6.0" }, { name = "qpsolvers", extras = ["proxqp"], specifier = ">=4.12.0" },