Feat: Arm teleop with Pinocchio - single & dual arm

dimos/control/blueprints.py

@@ -35,6 +35,11 @@
 from dimos.core.transport import LCMTransport
 from dimos.msgs.geometry_msgs import PoseStamped
 from dimos.msgs.sensor_msgs import JointState
+from dimos.teleop.quest.quest_types import QuestButtons
+from dimos.utils.data import LfsPath
+
+_PIPER_MODEL_PATH = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml")
+_XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf")
 
 # =============================================================================
 # Single Arm Blueprints
@@ -391,14 +396,6 @@
 # =============================================================================
 
 
-def _get_piper_model_path() -> str:
-    """Get path to Piper MJCF model for IK solver."""
-    from dimos.utils.data import get_data
-
-    piper_path = get_data("piper_description")
-    return str(piper_path / "mujoco_model" / "piper_no_gripper_description.xml")
-
-
 # Mock 6-DOF arm with CartesianIK
 coordinator_cartesian_ik_mock = control_coordinator(
     tick_rate=100.0,
@@ -418,7 +415,7 @@ def _get_piper_model_path() -> str:
             type="cartesian_ik",
             joint_names=[f"arm_joint{i + 1}" for i in range(6)],
             priority=10,
-            model_path=_get_piper_model_path(),
+            model_path=_PIPER_MODEL_PATH,
             ee_joint_id=6,
         ),
     ],
@@ -452,8 +449,137 @@ def _get_piper_model_path() -> str:
             type="cartesian_ik",
             joint_names=[f"arm_joint{i + 1}" for i in range(6)],
             priority=10,
-            model_path=_get_piper_model_path(),
+            model_path=_PIPER_MODEL_PATH,
+            ee_joint_id=6,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState),
+        ("cartesian_command", PoseStamped): LCMTransport(
+            "/coordinator/cartesian_command", PoseStamped
+        ),
+    }
+)
+
+
+# =============================================================================
+# Teleop IK Blueprints (VR teleoperation with internal Pinocchio IK)
+# =============================================================================
+
+# Single XArm6 with TeleopIK
+coordinator_teleop_xarm6 = control_coordinator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="coordinator",
+    hardware=[
+        HardwareComponent(
+            hardware_id="arm",
+            hardware_type=HardwareType.MANIPULATOR,
+            joints=make_joints("arm", 6),
+            adapter_type="xarm",
+            address="192.168.1.210",
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="teleop_xarm",
+            type="teleop_ik",
+            joint_names=[f"arm_joint{i + 1}" for i in range(6)],
+            priority=10,
+            model_path=_XARM6_MODEL_PATH,
+            ee_joint_id=6,
+            hand="right",
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState),
+        ("cartesian_command", PoseStamped): LCMTransport(
+            "/coordinator/cartesian_command", PoseStamped
+        ),
+        ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons),
+    }
+)
+
+# Single Piper with TeleopIK
+coordinator_teleop_piper = control_coordinator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="coordinator",
+    hardware=[
+        HardwareComponent(
+            hardware_id="arm",
+            hardware_type=HardwareType.MANIPULATOR,
+            joints=make_joints("arm", 6),
+            adapter_type="piper",
+            address="can0",
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="teleop_piper",
+            type="teleop_ik",
+            joint_names=[f"arm_joint{i + 1}" for i in range(6)],
+            priority=10,
+            model_path=_PIPER_MODEL_PATH,
+            ee_joint_id=6,
+            hand="left",
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState),
+        ("cartesian_command", PoseStamped): LCMTransport(
+            "/coordinator/cartesian_command", PoseStamped
+        ),
+        ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons),
+    }
+)
+
+# Dual arm teleop: XArm6 + Piper with TeleopIK
+coordinator_teleop_dual = control_coordinator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="coordinator",
+    hardware=[
+        HardwareComponent(
+            hardware_id="xarm_arm",
+            hardware_type=HardwareType.MANIPULATOR,
+            joints=make_joints("xarm_arm", 6),
+            adapter_type="xarm",
+            address="192.168.1.210",
+            auto_enable=True,
+        ),
+        HardwareComponent(
+            hardware_id="piper_arm",
+            hardware_type=HardwareType.MANIPULATOR,
+            joints=make_joints("piper_arm", 6),
+            adapter_type="piper",
+            address="can0",
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="teleop_xarm",
+            type="teleop_ik",
+            joint_names=[f"xarm_arm_joint{i + 1}" for i in range(6)],
+            priority=10,
+            model_path=_XARM6_MODEL_PATH,
+            ee_joint_id=6,
+            hand="left",
+        ),
+        TaskConfig(
+            name="teleop_piper",
+            type="teleop_ik",
+            joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)],
+            priority=10,
+            model_path=_PIPER_MODEL_PATH,
             ee_joint_id=6,
+            hand="right",
         ),
     ],
 ).transports(
@@ -462,6 +588,7 @@ def _get_piper_model_path() -> str:
         ("cartesian_command", PoseStamped): LCMTransport(
             "/coordinator/cartesian_command", PoseStamped
         ),
+        ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons),
     }
 )
 
@@ -500,6 +627,9 @@ def _get_piper_model_path() -> str:
     "coordinator_mock",
     "coordinator_piper",
     "coordinator_piper_xarm",
+    # Teleop IK
+    "coordinator_teleop_dual",
+    "coordinator_teleop_piper",
     "coordinator_teleop_xarm6",
     "coordinator_velocity_xarm6",
     "coordinator_xarm6",

dimos/control/coordinator.py

@@ -44,10 +44,12 @@
 from dimos.msgs.sensor_msgs import (
     JointState,  # noqa: TC001 - needed at runtime for Out[JointState]
 )
+from dimos.teleop.quest.quest_types import QuestButtons  # noqa: TC001 - needed for teleop buttons
 from dimos.utils.logging_config import setup_logger
 
 if TYPE_CHECKING:
     from collections.abc import Callable
+    from pathlib import Path
 
     from dimos.hardware.manipulators.spec import ManipulatorAdapter
 
@@ -65,20 +67,21 @@ class TaskConfig:
 
     Attributes:
         name: Task name (e.g., "traj_arm")
-        type: Task type ("trajectory", "servo", "velocity", "cartesian_ik")
+        type: Task type ("trajectory", "servo", "velocity", "cartesian_ik", "teleop_ik")
         joint_names: List of joint names this task controls
         priority: Task priority (higher wins arbitration)
-        model_path: Path to URDF/MJCF for IK solver (cartesian_ik only)
-        ee_joint_id: End-effector joint ID in model (cartesian_ik only)
+        model_path: Path to URDF/MJCF for IK solver (cartesian_ik/teleop_ik only)
+        ee_joint_id: End-effector joint ID in model (cartesian_ik/teleop_ik only)
     """
 
     name: str
     type: str = "trajectory"
     joint_names: list[str] = field(default_factory=lambda: [])
     priority: int = 10
-    # Cartesian IK specific
-    model_path: str | None = None
+    # Cartesian IK / Teleop IK specific
+    model_path: str | Path | None = None
     ee_joint_id: int = 6
+    hand: str = ""  # teleop_ik only: "left" or "right" controller
 
 
 @dataclass
@@ -145,6 +148,9 @@ class ControlCoordinator(Module[ControlCoordinatorConfig]):
     # Uses frame_id as task name for routing
     cartesian_command: In[PoseStamped]
 
+    # Input: Teleop buttons for engage/disengage signaling
+    buttons: In[QuestButtons]
+
     config: ControlCoordinatorConfig
     default_config = ControlCoordinatorConfig
 
@@ -168,6 +174,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
         # Subscription handles for streaming commands
         self._joint_command_unsub: Callable[[], None] | None = None
         self._cartesian_command_unsub: Callable[[], None] | None = None
+        self._buttons_unsub: Callable[[], None] | None = None
 
         logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz")
 
@@ -276,6 +283,23 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask:
                 ),
             )
 
+        elif task_type == "teleop_ik":
+            from dimos.control.tasks.teleop_task import TeleopIKTask, TeleopIKTaskConfig
+
+            if cfg.model_path is None:
+                raise ValueError(f"TeleopIKTask '{cfg.name}' requires model_path in TaskConfig")
+
+            return TeleopIKTask(
+                cfg.name,
+                TeleopIKTaskConfig(
+                    joint_names=cfg.joint_names,
+                    model_path=cfg.model_path,
+                    ee_joint_id=cfg.ee_joint_id,
+                    priority=cfg.priority,
+                    hand=cfg.hand,
+                ),
+            )
+
         else:
             raise ValueError(f"Unknown task type: {task_type}")
 
@@ -437,14 +461,14 @@ def _on_joint_command(self, msg: JointState) -> None:
                     continue
 
                 # Route to servo tasks (position control)
-                if hasattr(task, "set_target_by_name") and msg.position:
+                if msg.position:
                     positions_by_name = dict(zip(msg.name, msg.position, strict=False))
-                    task.set_target_by_name(positions_by_name, t_now)  # type: ignore[attr-defined]
+                    task.set_target_by_name(positions_by_name, t_now)
 
                 # Route to velocity tasks (velocity control)
-                elif hasattr(task, "set_velocities_by_name") and msg.velocity:
+                elif msg.velocity:
                     velocities_by_name = dict(zip(msg.name, msg.velocity, strict=False))
-                    task.set_velocities_by_name(velocities_by_name, t_now)  # type: ignore[attr-defined]
+                    task.set_velocities_by_name(velocities_by_name, t_now)
 
     def _on_cartesian_command(self, msg: PoseStamped) -> None:
         """Route incoming PoseStamped to CartesianIKTask by task name.
@@ -464,11 +488,13 @@ def _on_cartesian_command(self, msg: PoseStamped) -> None:
                 logger.warning(f"Cartesian command for unknown task: {task_name}")
                 return
 
-            # Route to CartesianIKTask
-            if hasattr(task, "set_target_pose"):
-                task.set_target_pose(msg, t_now)  # type: ignore[attr-defined]
-            else:
-                logger.warning(f"Task {task_name} does not support set_target_pose")
+            task.on_cartesian_command(msg, t_now)
+
+    def _on_buttons(self, msg: QuestButtons) -> None:
+        """Forward button state to all tasks."""
+        with self._task_lock:
+            for task in self._tasks.values():
+                task.on_buttons(msg)
 
     @rpc
     def task_invoke(
@@ -561,35 +587,34 @@ def start(self) -> None:
         streaming_types = ("servo", "velocity")
         has_streaming = any(t.type in streaming_types for t in self.config.tasks)
         if has_streaming:
-            # Only subscribe if transport is configured
             try:
-                if self.joint_command.transport:
-                    self._joint_command_unsub = self.joint_command.subscribe(self._on_joint_command)
-                    logger.info("Subscribed to joint_command for streaming tasks")
-                else:
-                    logger.warning(
-                        "Streaming tasks configured but no transport set for joint_command. "
-                        "Use task_invoke RPC or set transport via blueprint."
-                    )
-            except Exception as e:
-                logger.warning(f"Could not subscribe to joint_command: {e}")
+                self._joint_command_unsub = self.joint_command.subscribe(self._on_joint_command)
+                logger.info("Subscribed to joint_command for streaming tasks")
+            except Exception:
+                logger.warning(
+                    "Streaming tasks configured but could not subscribe to joint_command. "
+                    "Use task_invoke RPC or set transport via blueprint."
+                )
 
         # Subscribe to cartesian commands if any cartesian_ik tasks configured
-        has_cartesian_ik = any(t.type == "cartesian_ik" for t in self.config.tasks)
+        has_cartesian_ik = any(t.type in ("cartesian_ik", "teleop_ik") for t in self.config.tasks)
         if has_cartesian_ik:
             try:
-                if self.cartesian_command.transport:
-                    self._cartesian_command_unsub = self.cartesian_command.subscribe(
-                        self._on_cartesian_command
-                    )
-                    logger.info("Subscribed to cartesian_command for CartesianIK tasks")
-                else:
-                    logger.warning(
-                        "CartesianIK tasks configured but no transport set for cartesian_command. "
-                        "Use task_invoke RPC or set transport via blueprint."
-                    )
-            except Exception as e:
-                logger.warning(f"Could not subscribe to cartesian_command: {e}")
+                self._cartesian_command_unsub = self.cartesian_command.subscribe(
+                    self._on_cartesian_command
+                )
+                logger.info("Subscribed to cartesian_command for CartesianIK/TeleopIK tasks")
+            except Exception:
+                logger.warning(
+                    "CartesianIK/TeleopIK tasks configured but could not subscribe to cartesian_command. "
+                    "Use task_invoke RPC or set transport via blueprint."
+                )
+
+        # Subscribe to buttons if any teleop_ik tasks configured (engage/disengage)
+        has_teleop_ik = any(t.type == "teleop_ik" for t in self.config.tasks)
+        if has_teleop_ik:
+            self._buttons_unsub = self.buttons.subscribe(self._on_buttons)
+            logger.info("Subscribed to buttons for engage/disengage")
 
         logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz")
 
@@ -605,6 +630,9 @@ def stop(self) -> None:
         if self._cartesian_command_unsub:
             self._cartesian_command_unsub()
             self._cartesian_command_unsub = None
+        if self._buttons_unsub:
+            self._buttons_unsub()
+            self._buttons_unsub = None
 
         if self._tick_loop:
             self._tick_loop.stop()