diff --git a/dimos/control/__init__.py b/dimos/control/__init__.py index 23ac02836b..639f0ba38a 100644 --- a/dimos/control/__init__.py +++ b/dimos/control/__init__.py @@ -60,6 +60,7 @@ "HardwareType", "JointName", "JointState", + "make_gripper_joints", "make_joints", ], "coordinator": [ diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 5c33928e79..0384c69160 100644 --- a/dimos/control/blueprints.py +++ b/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,8 +475,8 @@ # 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", @@ -482,21 +484,25 @@ 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", diff --git a/dimos/control/components.py b/dimos/control/components.py index 8157a288d2..a32b395b83 100644 --- a/dimos/control/components.py +++ b/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", ] diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 6cd96d0000..21d4c9d06c 100644 --- a/dimos/control/coordinator.py +++ b/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}': " diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 4e5d1d634c..b4f84a2507 100644 --- a/dimos/control/hardware_interface.py +++ b/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: diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index d1a90eb6d0..ce63dc4006 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -27,7 +27,7 @@ from dataclasses import dataclass import threading -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Any, Literal import numpy as np import pinocchio # type: ignore[import-untyped] @@ -69,6 +69,9 @@ class TeleopIKTaskConfig: timeout: If no command received for this many seconds, go inactive (0 = never) max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) hand: "left" or "right" — which controller's primary button to listen to + gripper_joint: Optional joint name for the gripper (e.g. "arm_gripper"). + gripper_open_pos: Gripper position (adapter units) at trigger value 0.0 (no press). + gripper_closed_pos: Gripper position (adapter units) at trigger value 1.0 (full press). """ joint_names: list[str] @@ -77,7 +80,10 @@ class TeleopIKTaskConfig: priority: int = 10 timeout: float = 0.5 max_joint_delta_deg: float = 5.0 # ~500°/s at 100Hz - hand: str = "" + hand: Literal["left", "right"] | None = None + gripper_joint: str | None = None + gripper_open_pos: float = 0.0 + gripper_closed_pos: float = 0.0 class TeleopIKTask(BaseControlTask): @@ -101,6 +107,7 @@ class TeleopIKTask(BaseControlTask): ... ee_joint_id=6, ... priority=10, ... timeout=0.5, + ... hand="right", ... ), ... ) >>> coordinator.add_task(task) @@ -121,6 +128,8 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: raise ValueError(f"TeleopIKTask '{name}' requires at least one joint") if not config.model_path: raise ValueError(f"TeleopIKTask '{name}' requires model_path for IK solver") + if config.hand not in ("left", "right"): + raise ValueError(f"TeleopIKTask '{name}' requires hand='left' or 'right'") self._name = name self._config = config @@ -148,6 +157,8 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: self._initial_ee_pose: pinocchio.SE3 | None = None self._prev_primary: bool = False + self._gripper_target: float = config.gripper_open_pos + logger.info( f"TeleopIKTask {name} initialized with model: {config.model_path}, " f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}" @@ -160,8 +171,11 @@ def name(self) -> str: def claim(self) -> ResourceClaim: """Declare resource requirements.""" + joints = self._joint_names + if self._config.gripper_joint: + joints = joints | frozenset([self._config.gripper_joint]) return ResourceClaim( - joints=self._joint_names, + joints=joints, priority=self._config.priority, mode=ControlMode.SERVO_POSITION, ) @@ -245,9 +259,18 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: ) return None + joint_names = list(self._joint_names_list) positions = q_solution.flatten().tolist() + + # Append gripper joint if configured — routed to ConnectedHardware by tick loop + if self._config.gripper_joint: + with self._lock: + gripper_pos = self._gripper_target + joint_names.append(self._config.gripper_joint) + positions.append(gripper_pos) + return JointCommandOutput( - joint_names=self._joint_names_list, + joint_names=joint_names, positions=positions, mode=ControlMode.SERVO_POSITION, ) @@ -277,31 +300,25 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: # ========================================================================= def on_buttons(self, msg: Buttons) -> bool: - """Press-and-hold engage: hold primary button to track, release to stop. - - Checks only the button matching self._config.hand (left_primary or right_primary). - If hand is not set, listens to both. - """ - hand = self._config.hand - if hand == "left": - primary = msg.left_primary - elif hand == "right": - primary = msg.right_primary - else: - primary = msg.left_primary or msg.right_primary + """Press-and-hold engage: hold primary button to track, release to stop.""" + is_left = self._config.hand == "left" + primary = msg.left_primary if is_left else msg.right_primary if primary and not self._prev_primary: - # Rising edge: reset initial pose so compute() recaptures logger.info(f"TeleopIKTask {self._name}: engage") with self._lock: self._initial_ee_pose = None elif not primary and self._prev_primary: - # Falling edge: stop tracking logger.info(f"TeleopIKTask {self._name}: disengage") with self._lock: self._target_pose = None self._initial_ee_pose = None self._prev_primary = primary + + if self._config.gripper_joint: + trigger = msg.left_trigger_analog if is_left else msg.right_trigger_analog + self.on_gripper_trigger(trigger) + return True def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: @@ -313,6 +330,22 @@ def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: return True + def on_gripper_trigger(self, value: float, _t_now: float = 0.0) -> bool: + """Map analog trigger (0-1) to gripper position""" + if not self._config.gripper_joint: + return False + + clamped = max(0.0, min(1.0, value)) + pos = ( + self._config.gripper_open_pos + + (self._config.gripper_closed_pos - self._config.gripper_open_pos) * clamped + ) + + with self._lock: + self._gripper_target = pos + + return True + def start(self) -> None: """Activate the task (start accepting and outputting commands).""" with self._lock: diff --git a/dimos/hardware/manipulators/xarm/adapter.py b/dimos/hardware/manipulators/xarm/adapter.py index dd9f764031..80cc8edb38 100644 --- a/dimos/hardware/manipulators/xarm/adapter.py +++ b/dimos/hardware/manipulators/xarm/adapter.py @@ -62,6 +62,7 @@ def __init__(self, address: str, dof: int = 6, **_: object) -> None: self._dof = dof self._arm: XArmAPI | None = None self._control_mode: ControlMode = ControlMode.POSITION + self._gripper_enabled: bool = False # ========================================================================= # Connection @@ -351,9 +352,11 @@ def write_gripper_position(self, position: float) -> bool: if not self._arm: return False - self._arm.set_gripper_enable(True) + if not self._gripper_enabled: + self._arm.set_gripper_enable(True) + self._gripper_enabled = True pos_mm = position * M_TO_MM - code: int = self._arm.set_gripper_position(pos_mm, wait=True) + code: int = self._arm.set_gripper_position(pos_mm, wait=False) return code == 0 # ========================================================================= diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 0fb09fb696..f3d63efc29 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -20,7 +20,7 @@ "arm-teleop-dual": "dimos.teleop.quest.blueprints:arm_teleop_dual", "arm-teleop-piper": "dimos.teleop.quest.blueprints:arm_teleop_piper", "arm-teleop-visualizing": "dimos.teleop.quest.blueprints:arm_teleop_visualizing", - "arm-teleop-xarm6": "dimos.teleop.quest.blueprints:arm_teleop_xarm6", + "arm-teleop-xarm7": "dimos.teleop.quest.blueprints:arm_teleop_xarm7", "coordinator-basic": "dimos.control.blueprints:coordinator_basic", "coordinator-cartesian-ik-mock": "dimos.control.blueprints:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints:coordinator_cartesian_ik_piper", @@ -35,6 +35,7 @@ "coordinator-teleop-dual": "dimos.control.blueprints:coordinator_teleop_dual", "coordinator-teleop-piper": "dimos.control.blueprints:coordinator_teleop_piper", "coordinator-teleop-xarm6": "dimos.control.blueprints:coordinator_teleop_xarm6", + "coordinator-teleop-xarm7": "dimos.control.blueprints:coordinator_teleop_xarm7", "coordinator-velocity-xarm6": "dimos.control.blueprints:coordinator_velocity_xarm6", "coordinator-xarm6": "dimos.control.blueprints:coordinator_xarm6", "coordinator-xarm7": "dimos.control.blueprints:coordinator_xarm7", diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index c46bf657ff..5672a2bea0 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -18,7 +18,7 @@ from dimos.control.blueprints import ( coordinator_teleop_dual, coordinator_teleop_piper, - coordinator_teleop_xarm6, + coordinator_teleop_xarm7, ) from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport @@ -57,12 +57,12 @@ # Teleop wired to Coordinator (TeleopIK) # ----------------------------------------------------------------------------- -# Single XArm6 teleop: right controller -> xarm6 -# Usage: dimos run arm-teleop-xarm6 +# Single XArm7 teleop: right controller -> xarm7 +# Usage: dimos run arm-teleop-xarm7 -arm_teleop_xarm6 = autoconnect( +arm_teleop_xarm7 = autoconnect( arm_teleop_module(task_names={"right": "teleop_xarm"}), - coordinator_teleop_xarm6, + coordinator_teleop_xarm7, ).transports( { ("right_controller_output", PoseStamped): LCMTransport( @@ -110,5 +110,5 @@ "arm_teleop_dual", "arm_teleop_piper", "arm_teleop_visualizing", - "arm_teleop_xarm6", + "arm_teleop_xarm7", ] diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 9139b671ea..68ec279efb 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -26,6 +26,7 @@ from dimos.core.stream import Out from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped from dimos.teleop.quest.quest_teleop_module import Hand, QuestTeleopConfig, QuestTeleopModule +from dimos.teleop.quest.quest_types import Buttons, QuestControllerState from dimos.teleop.utils.teleop_visualization import ( visualize_buttons, visualize_pose, @@ -125,6 +126,19 @@ def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: ) super()._publish_msg(hand, output_msg) + def _publish_button_state( + self, + left: QuestControllerState | None, + right: QuestControllerState | None, + ) -> None: + """Publish Buttons with analog triggers packed into bits 16-29.""" + buttons = Buttons.from_controllers(left, right) + buttons.pack_analog_triggers( + left=left.trigger if left is not None else 0.0, + right=right.trigger if right is not None else 0.0, + ) + self.buttons.publish(buttons) + class VisualizingTeleopModule(ArmTeleopModule): """Quest teleop with Rerun visualization. diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index adfdb6aaff..d5a86960fe 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -145,6 +145,7 @@ def start(self) -> None: logger.info(f"Subscribed to: {', '.join(connected)}") self._start_server() + self._start_control_loop() logger.info("Quest Teleoperation Module started") @rpc diff --git a/dimos/teleop/quest/quest_types.py b/dimos/teleop/quest/quest_types.py index 9e5616101d..7fd991a76c 100644 --- a/dimos/teleop/quest/quest_types.py +++ b/dimos/teleop/quest/quest_types.py @@ -94,16 +94,19 @@ def from_joy(cls, joy: Joy, is_left: bool = True) -> "QuestControllerState": class Buttons(UInt32): """Packed button states for both controllers in a single UInt32. - All values are collapsed to bools for lightweight transport. Analog values - (trigger, grip) are thresholded at 0.5. If you need the original float - values, access them from QuestControllerState and publish them in a subclass. + Digital buttons are collapsed to bools. Analog trigger values are packed + as 7-bit integers in the upper 16 bits for proportional gripper control. Bit layout: - Left (bits 0-6): trigger, grip, touchpad, thumbstick, primary, secondary, menu - Right (bits 8-14): trigger, grip, touchpad, thumbstick, primary, secondary, menu + Left (bits 0-6): trigger, grip, touchpad, thumbstick, primary, secondary, menu + Right (bits 8-14): trigger, grip, touchpad, thumbstick, primary, secondary, menu + Bit 7, 15: reserved + Bits 16-22: left trigger analog (7-bit, 0=0.0 … 127=1.0) + Bits 23-29: right trigger analog (7-bit, 0=0.0 … 127=1.0) + Bits 30-31: unused (kept clear so LCM signed int32 never overflows) """ - # Bit positions + # Bit positions for digital buttons BITS = { "left_trigger": 0, "left_grip": 1, @@ -121,6 +124,32 @@ class Buttons(UInt32): "right_menu": 14, } + # Analog trigger packing constants + _LEFT_TRIGGER_SHIFT: int = 16 + _RIGHT_TRIGGER_SHIFT: int = 23 + _ANALOG_MASK: int = 0x7F + _ANALOG_MAX: int = 127 + + @property + def left_trigger_analog(self) -> float: + """Left trigger analog value [0.0, 1.0], decoded from bits 16-22.""" + return ((self.data >> self._LEFT_TRIGGER_SHIFT) & self._ANALOG_MASK) / self._ANALOG_MAX + + @property + def right_trigger_analog(self) -> float: + """Right trigger analog value [0.0, 1.0], decoded from bits 23-29.""" + return ((self.data >> self._RIGHT_TRIGGER_SHIFT) & self._ANALOG_MASK) / self._ANALOG_MAX + + def pack_analog_triggers(self, left: float, right: float) -> None: + """Pack analog trigger values [0.0, 1.0] into bits 16-22 and 23-29.""" + left_u7 = round(max(0.0, min(1.0, left)) * self._ANALOG_MAX) + right_u7 = round(max(0.0, min(1.0, right)) * self._ANALOG_MAX) + self.data = ( + (self.data & 0x0000FFFF) # clear bits 16-31 + | (left_u7 << self._LEFT_TRIGGER_SHIFT) + | (right_u7 << self._RIGHT_TRIGGER_SHIFT) + ) + def __getattr__(self, name: str) -> bool: if name in Buttons.BITS: return bool(self.data & (1 << Buttons.BITS[name]))