feat: integrate gripper into coordinator tick loop (#1371)

* Feat: modified buttons to publish absolute trigger values

* Fix: control loop start must be in start

* feat: packing analog triggers iwith buttons in UInt32

* for xarm7

* Feat: added gripper as hardware_component

* Feat: gripper trigger decode

* Fix: pre-commit fixes

* comments update

* fix: bit conversion amth + comments

* fix: comments and errors

* fix comments + pre-commit errors

* fix: blueprint + mypy tests

* CI code cleanup

* Fix: no gripper as component, adding with joints

* feat: add literal for hand str

* fix: arm and gripper joint params

* CI code cleanup

* Misc: docstrings

Author: ruthwikdasyam

dimos/control/__init__.py

@@ -60,6 +60,7 @@
             "HardwareType",
             "JointName",
             "JointState",
+            "make_gripper_joints",
             "make_joints",
         ],
         "coordinator": [

dimos/control/blueprints.py

@@ -33,6 +33,7 @@
 from dimos.control.components import (
     HardwareComponent,
     HardwareType,
+    make_gripper_joints,
     make_joints,
     make_twist_base_joints,
 )
@@ -45,6 +46,7 @@
 
 _PIPER_MODEL_PATH = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml")
 _XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf")
+_XARM7_MODEL_PATH = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf")
 
 
 # =============================================================================
@@ -473,30 +475,34 @@
 # Teleop IK Blueprints (VR teleoperation with internal Pinocchio IK)
 # =============================================================================
 
-# Single XArm6 with TeleopIK
-coordinator_teleop_xarm6 = control_coordinator(
+# Single XArm7 with TeleopIK
+coordinator_teleop_xarm7 = 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),
+            joints=make_joints("arm", 7),
             adapter_type="xarm",
-            address="192.168.1.210",
+            address="192.168.2.235",
             auto_enable=True,
+            gripper_joints=make_gripper_joints("arm"),
         ),
     ],
     tasks=[
         TaskConfig(
             name="teleop_xarm",
             type="teleop_ik",
-            joint_names=[f"arm_joint{i + 1}" for i in range(6)],
+            joint_names=[f"arm_joint{i + 1}" for i in range(7)],
             priority=10,
-            model_path=_XARM6_MODEL_PATH,
-            ee_joint_id=6,
+            model_path=_XARM7_MODEL_PATH,
+            ee_joint_id=7,
             hand="right",
+            gripper_joint=make_gripper_joints("arm")[0],
+            gripper_open_pos=0.85,  # xArm gripper range
+            gripper_closed_pos=0.0,
         ),
     ],
 ).transports(
@@ -715,6 +721,7 @@
     "coordinator_teleop_dual",
     "coordinator_teleop_piper",
     "coordinator_teleop_xarm6",
+    "coordinator_teleop_xarm7",
     "coordinator_velocity_xarm6",
     "coordinator_xarm6",
     "coordinator_xarm7",

dimos/control/components.py

@@ -25,7 +25,6 @@
 class HardwareType(Enum):
     MANIPULATOR = "manipulator"
     BASE = "base"
-    GRIPPER = "gripper"
 
 
 @dataclass(frozen=True)
@@ -43,11 +42,12 @@ class HardwareComponent:
 
     Attributes:
         hardware_id: Unique identifier, also used as joint name prefix
-        hardware_type: Type of hardware (MANIPULATOR, BASE, GRIPPER)
+        hardware_type: Type of hardware (MANIPULATOR, BASE)
         joints: List of joint names (e.g., ["arm_joint1", "arm_joint2", ...])
         adapter_type: Adapter type ("mock", "xarm", "piper")
         address: Connection address - IP for TCP, port for CAN
         auto_enable: Whether to auto-enable servos
+        gripper_joints: Joints that use adapter gripper methods (separate from joints).
     """
 
     hardware_id: HardwareId
@@ -56,6 +56,24 @@ class HardwareComponent:
     adapter_type: str = "mock"
     address: str | None = None
     auto_enable: bool = True
+    gripper_joints: list[JointName] = field(default_factory=list)
+
+    @property
+    def all_joints(self) -> list[JointName]:
+        """All joints: arm joints + gripper joints."""
+        return self.joints + self.gripper_joints
+
+
+def make_gripper_joints(hardware_id: HardwareId) -> list[JointName]:
+    """Create gripper joint names for a hardware device.
+
+    Args:
+        hardware_id: The hardware identifier (e.g., "arm")
+
+    Returns:
+        List of joint names like ["arm_gripper"]
+    """
+    return [f"{hardware_id}_gripper"]
 
 
 def make_joints(hardware_id: HardwareId, dof: int) -> list[JointName]:
@@ -112,6 +130,7 @@ def make_twist_base_joints(
     "JointName",
     "JointState",
     "TaskName",
+    "make_gripper_joints",
     "make_joints",
     "make_twist_base_joints",
 ]

dimos/control/coordinator.py

@@ -29,7 +29,7 @@
 from pathlib import Path
 import threading
 import time
-from typing import TYPE_CHECKING, Any
+from typing import TYPE_CHECKING, Any, Literal
 
 from dimos.control.components import (
     TWIST_SUFFIX_MAP,
@@ -84,6 +84,10 @@ class TaskConfig:
         priority: Task priority (higher wins arbitration)
         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)
+        hand: "left" or "right" controller hand (teleop_ik only)
+        gripper_joint: Joint name for gripper virtual joint
+        gripper_open_pos: Gripper position at trigger 0.0
+        gripper_closed_pos: Gripper position at trigger 1.0
     """
 
     name: str
@@ -93,7 +97,11 @@ class TaskConfig:
     # 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
+    hand: Literal["left", "right"] | None = None  # teleop_ik only
+    # Teleop IK gripper specific
+    gripper_joint: str | None = None
+    gripper_open_pos: float = 0.0
+    gripper_closed_pos: float = 0.0
 
 
 @dataclass
@@ -327,6 +335,9 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask:
                     ee_joint_id=cfg.ee_joint_id,
                     priority=cfg.priority,
                     hand=cfg.hand,
+                    gripper_joint=cfg.gripper_joint,
+                    gripper_open_pos=cfg.gripper_open_pos,
+                    gripper_closed_pos=cfg.gripper_closed_pos,
                 ),
             )
 
@@ -345,6 +356,7 @@ def add_hardware(
     ) -> bool:
         """Register a hardware adapter with the coordinator."""
         is_base = component.hardware_type == HardwareType.BASE
+
         if is_base != isinstance(adapter, TwistBaseAdapter):
             raise TypeError(
                 f"Hardware type / adapter mismatch for '{component.hardware_id}': "

dimos/control/hardware_interface.py

@@ -65,7 +65,9 @@ def __init__(
 
         self._adapter = adapter
         self._component = component
-        self._joint_names = component.joints
+        self._arm_joint_names: list[JointName] = list(component.joints)
+        self._gripper_joints: list[JointName] = list(component.gripper_joints)
+        self._joint_names: list[JointName] = component.all_joints
 
         # Track last commanded values for hold-last behavior
         self._last_commanded: dict[str, float] = {}
@@ -114,15 +116,27 @@ def read_state(self) -> dict[JointName, JointState]:
         velocities = self._adapter.read_joint_velocities()
         efforts = self._adapter.read_joint_efforts()
 
-        return {
+        result: dict[JointName, JointState] = {
             name: JointState(
                 position=positions[i],
                 velocity=velocities[i],
                 effort=efforts[i],
             )
-            for i, name in enumerate(self._joint_names)
+            for i, name in enumerate(self._arm_joint_names)
         }
 
+        # Append gripper joint(s) via adapter gripper method
+        if self._gripper_joints:
+            gripper_pos = self._adapter.read_gripper_position()
+            for gj in self._gripper_joints:
+                result[gj] = JointState(
+                    position=gripper_pos if gripper_pos is not None else 0.0,
+                    velocity=0.0,
+                    effort=0.0,
+                )
+
+        return result
+
     def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
         """Write commands - allows partial joint sets, holds last for missing.
 
@@ -153,8 +167,8 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
                 )
                 self._warned_unknown_joints.add(joint_name)
 
-        # Build ordered list for adapter
-        ordered = self._build_ordered_command()
+        # Build ordered list for arm joints only
+        arm_ordered = [self._last_commanded[name] for name in self._arm_joint_names]
 
         # Switch control mode if needed
         if mode != self._current_mode:
@@ -163,25 +177,43 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
                 return False
             self._current_mode = mode
 
-        # Send to adapter
+        # Send arm joints to adapter
+        arm_ok: bool
         match mode:
             case ControlMode.POSITION | ControlMode.SERVO_POSITION:
-                return self._adapter.write_joint_positions(ordered)
+                arm_ok = self._adapter.write_joint_positions(arm_ordered)
             case ControlMode.VELOCITY:
-                return self._adapter.write_joint_velocities(ordered)
+                arm_ok = self._adapter.write_joint_velocities(arm_ordered)
             case ControlMode.TORQUE:
                 logger.warning(f"Hardware {self.hardware_id} does not support torque mode")
-                return False
+                arm_ok = False
             case _:
-                return False
+                arm_ok = False
+
+        # Send gripper joints via adapter gripper method
+        gripper_ok = True
+        for gj in self._gripper_joints:
+            if gj in self._last_commanded:
+                gripper_ok = (
+                    self._adapter.write_gripper_position(self._last_commanded[gj]) and gripper_ok
+                )
+
+        return arm_ok and gripper_ok
 
     def _initialize_last_commanded(self) -> None:
         """Initialize last_commanded with current hardware positions."""
         for _ in range(10):
             try:
                 current = self._adapter.read_joint_positions()
-                for i, name in enumerate(self._joint_names):
+                for i, name in enumerate(self._arm_joint_names):
                     self._last_commanded[name] = current[i]
+
+                # Initialize gripper joint(s) from adapter
+                if self._gripper_joints:
+                    gripper_pos = self._adapter.read_gripper_position()
+                    for gj in self._gripper_joints:
+                        self._last_commanded[gj] = gripper_pos if gripper_pos is not None else 0.0
+
                 self._initialized = True
                 return
             except Exception: