feat(g1): Quest teleop on shared dual-arm IK

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 = [

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")

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

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

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):

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,

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))

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",