feat: integrate gripper into coordinator tick loop#1371
Conversation
Greptile SummaryIntegrated gripper control into the coordinator tick loop by treating it as a first-class hardware component alongside arm joints. Key Changes:
Architecture: Confidence Score: 5/5
Important Files Changed
Sequence DiagramsequenceDiagram
participant VR as Quest Controller
participant QT as QuestTeleopModule
participant TIK as TeleopIKTask
participant Coord as ControlCoordinator
participant CG as ConnectedGripper
participant Adapter as XArmAdapter
participant HW as xArm Hardware
Note over VR,HW: VR Trigger Squeeze (Analog 0.0 → 1.0)
VR->>QT: Joy message (trigger analog)
QT->>QT: Pack trigger into Buttons (7-bit, bits 23-29)
QT->>Coord: Publish Buttons message
Coord->>TIK: on_buttons(msg)
TIK->>TIK: Decode trigger analog<br/>Map to gripper position
Note over Coord,HW: Coordinator Tick Loop (100Hz)
Coord->>CG: read_state()
CG->>Adapter: read_gripper_position()
Adapter->>HW: get_gripper_position()
HW-->>Adapter: position (mm)
Adapter-->>CG: position (meters)
CG-->>Coord: JointState
Coord->>TIK: compute(state)
TIK->>TIK: Solve IK for arm<br/>Append gripper position
TIK-->>Coord: JointCommandOutput<br/>(arm joints + gripper)
Coord->>Coord: Arbitrate commands<br/>(per-joint priority)
Coord->>CG: write_command(gripper_pos)
CG->>Adapter: write_gripper_position(pos)
Adapter->>HW: set_gripper_position(pos_mm, wait=False)
Coord->>Coord: Publish JointState<br/>(arm + gripper)
Last reviewed commit: 7837c7f |
mustafab0
left a comment
There was a problem hiding this comment.
Need to justify why a new hardware interface is necessary
| "coordinator_teleop_dual", | ||
| "coordinator_teleop_piper", | ||
| "coordinator_teleop_xarm6", | ||
| "coordinator_teleop_xarm7", |
There was a problem hiding this comment.
This is okay and not a problem at all. But let's thin out control blueprints in the future to only have 1 example of each use cases and only with mock hardware
For example control blueprints should have coordinator_teleop_mock, coordinator_visualservo-mock, coordinator-dual-mock.
There was a problem hiding this comment.
Right, blueprints are overflowing on the coordinator/manipulators end. We can reduce the list and have smaller definitions.
dimos/control/hardware_interface.py
Outdated
|
|
||
| def __init__( | ||
| self, | ||
| adapter: ManipulatorAdapter, |
There was a problem hiding this comment.
Why do we need ManipulatorAdapter here?
There was a problem hiding this comment.
ConnectedGripper needs ManipulatorAdapter because right now grippers does not have a seperate SDK/adapter and we reply on arm's adapter for gripper controls -- which means same adapter for both.
It reuses the parent arm's adapter to read and write gripper poses.
| self._dof = dof | ||
| self._arm: XArmAPI | None = None | ||
| self._control_mode: ControlMode = ControlMode.POSITION | ||
| self._gripper_enabled: bool = False |
There was a problem hiding this comment.
I added gripper_enabled to run arm.gripper_enable method to activate gripper. But, i see that from manipulation_module, gripper is able to take commands without any arm.gripper_enable method. - which means gripper should always be active.
I can remove this check, if we get some issues later, we will add it.
There was a problem hiding this comment.
I think okay to keep it here, considering more complicated EE such as pneumatic grippers might want to be enabled disabled to save power,etc.
| 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) |
| joints=make_joints("arm", 7), | ||
| adapter_type="xarm", | ||
| address="192.168.1.210", | ||
| address="192.168.2.235", |
There was a problem hiding this comment.
This is personal config, it doesn't belong in a blueprint. One idea would be to have GlobalConfig.arm_ip with a default of None. And you can put ARM_IP=192.168.2.235 in your .env file.
There was a problem hiding this comment.
True. Currently all manipulation blueprints hardcode the arm IP the same way. Moving these to .env is a good idea - I can do a seperate PR cleaning up across all blueprints
dimos/control/hardware_interface.py
Outdated
| self._initialized = True | ||
|
|
||
| def disconnect(self) -> None: | ||
| """No-op: lifecycle is owned by the parent ConnectedHardware.""" |
There was a problem hiding this comment.
It's not clear to me why ConnectedHardware.disconnect calls self._adapter.disconnect() to begin with. If the life cycle of self._adapter is managed outside ConnectedHardware then it shouldn't call disconnect to begin with (there no connect method for example).
There was a problem hiding this comment.
Hm that's true. I see connect method called in coordinator.py line-244. perhaps, we can call disconnect from coordinator too.
@mustafab0 should be able to reason this precisely
There was a problem hiding this comment.
seld._adapter is simply a wrapper for the robot sdk. The ConnectedHardware is the actual runtime instance and has a lifecycle.
Every HardwareInterface needs a connect/disconnect which are generic for the ControlCoordinator to call. But the specific order of operations are encoded in the adapter.connect()/disconnect() methods.
@ruthwikdasyam you are right about the ControlCoordinator calling connect, that is an oversight. The coordinator shouldn't directly call connect and should always call the hardware interface to do so.
This likely is the case because when I was initially designing control coordinator a HardwareInterface did not exist at all.
dimos/control/tasks/teleop_task.py
Outdated
| logger.warning( | ||
| f"TeleopIKTask {self._name}: gripper_joint is set but hand is not " | ||
| f"'left' or 'right' — cannot determine which trigger to use" | ||
| ) |
There was a problem hiding this comment.
We should rely more on the python type system.
TeleopIKTaskConfig.hand has a type of str with a default of "" (which is invalid).
But if only "left" and "right" are valid, its type should be set to Literal["left", "right"]
There was a problem hiding this comment.
Agreed. Was using "" as a catch-all (both left and right), but Literal["left", "right"] is cleaner. Will update both TaskConfig and TeleopIKTaskConfig
| # 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] |
There was a problem hiding this comment.
A bit confused about the semantic change.
Is it because we want to make sure the gripper is added exactly after the specific ConnectedHardware manipulator instance?
Or is it to distinguish between TwistBase Hardware and Manipulator Hardware?
There was a problem hiding this comment.
yes, its to make sure we write joint commands to this ordered list - which only has arm_joints, and no grippers.
The old _build_ordered_command() iterated all joints including gripper which would break here,
howevr used by TwistBase where it works fine - coz they don't have gripper joints, so joint_names is just the velocity DOFs
fb45d3b
Problem
Gripper control bypasses the coordinator tick loop -
set_gripper_positionandget_gripper_positionRPCs call the adapter directly, so gripper commands aren't synchronized with arm joints, or published in JointState. This makes gripper control from VR triggers during teleop impossible.Closes DIM-598
Closes DIM-537
Solution
Integrate gripper into the tick loop as a first-class hardware component.
ConnectedGripperwraps the parent arm's adapter, routingread_state()/write_command()throughread_gripper_position()/write_gripper_position()so the tick loop handles it uniformly alongside arm joints.HardwareComponent.parent_hardware_idlets gripper components reference their parent arm - no duplicate adapter connections.Buttons- VR trigger floats packed as 7-bit values in bits 16-29 of the existing UInt32 message.TeleopIKTaskgripper support - newgripper_joint,gripper_open_pos,gripper_closed_posconfig.on_gripper_trigger()maps analog trigger [0,1] → gripper positionBreaking Changes
None
How to Test
dimos run arm-teleop-xarm7