Feat: Arm teleop with Pinocchio - single & dual arm#1246
Conversation
Greptile OverviewGreptile SummaryThis PR adds VR teleop support for any arm on DimOS by introducing a new Key changes:
Breaking changes:
Confidence Score: 4/5
Important Files Changed
Sequence DiagramsequenceDiagram
participant Quest as Quest Controller
participant ATM as ArmTeleopModule
participant Coord as ControlCoordinator
participant TIK as TeleopIKTask
participant PIK as PinocchioIK
participant HW as Hardware Adapter
Quest->>ATM: Controller pose delta + buttons
ATM->>ATM: Check task_names routing
ATM->>Coord: PoseStamped (frame_id=task_name)
ATM->>Coord: QuestButtons
Coord->>TIK: on_buttons(QuestButtons)
TIK->>TIK: Check hand-specific primary button
alt Primary button pressed
TIK->>TIK: Reset initial_ee_pose (engage)
else Primary button released
TIK->>TIK: Clear target_pose (disengage)
end
Coord->>TIK: on_cartesian_command(pose, t_now)
TIK->>TIK: Store delta_pose, set active
Coord->>Coord: Tick loop (100Hz)
Coord->>TIK: compute(state)
alt Active and not timed out
TIK->>TIK: Capture initial_ee_pose (if first compute)
TIK->>TIK: Apply delta to initial pose
TIK->>PIK: solve(target_pose, q_current)
PIK->>PIK: Damped least-squares IK
PIK-->>TIK: q_solution, converged, error
TIK->>TIK: Safety check: joint delta within limit
alt Delta exceeds limit
TIK-->>Coord: None (reject solution)
else Delta OK
TIK-->>Coord: JointCommandOutput(positions)
end
else Inactive or timed out
TIK-->>Coord: None
end
Coord->>Coord: Arbitrate tasks by priority
Coord->>HW: Send joint commands
HW->>HW: Execute motion
Last reviewed commit: 1c08c76 |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
dimos/control/coordinator.py
Outdated
| 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) # type: ignore[attr-defined] |
There was a problem hiding this comment.
Missing error handling - if task lacks on_cartesian_command, AttributeError will be raised instead of graceful warning like the old code did with hasattr check.
| task.on_cartesian_command(msg, t_now) # type: ignore[attr-defined] | |
| if hasattr(task, "on_cartesian_command"): | |
| task.on_cartesian_command(msg, t_now) # type: ignore[attr-defined] | |
| else: | |
| logger.warning(f"Task {task_name} does not support on_cartesian_command") |
There was a problem hiding this comment.
Fixed - Added error handling!
ruthwikdasyam
changed the title
ruthwikdasyam
changed the title
| def __init__(self, *args: Any, **kwargs: Any) -> None: | ||
| super().__init__(*args, **kwargs) | ||
| self._prev_primary: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} | ||
| cfg: ArmTeleopConfig = self.config # type: ignore[assignment] |
There was a problem hiding this comment.
This ignore is not needed. You have to define config: ArmTeleopConfig after default_config = ArmTeleopConfig. Then remove this line.
| # ========================================================================= | ||
| # Pose Conversion Helpers | ||
| # ========================================================================= | ||
|
|
||
| @staticmethod | ||
| def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: | ||
| """Convert Pose or PoseStamped to pinocchio SE3""" | ||
|
|
||
| position = np.array([pose.x, pose.y, pose.z]) | ||
| quat = pose.orientation | ||
| rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() | ||
| return pinocchio.SE3(rotation, position) | ||
|
|
||
| @staticmethod | ||
| def pose_dict_to_se3(pose: dict[str, float]) -> pinocchio.SE3: | ||
| """Convert a pose dict with RPY to pinocchio SE3""" | ||
|
|
||
| position = np.array([pose["x"], pose["y"], pose["z"]]) | ||
| rotation = pinocchio.rpy.rpyToMatrix(pose["roll"], pose["pitch"], pose["yaw"]) | ||
| return pinocchio.SE3(rotation, position) |
There was a problem hiding this comment.
These should be standalone functions too. They don't use PinocchioIK.
There was a problem hiding this comment.
can write them outside the class. can they be in the same file, along with other standalone methods (check_joint_delta(), get_worst_joint_delta()), or should they go to any utils file, as this is a type conversion?
There was a problem hiding this comment.
Changed it to standalone method
dimos/control/coordinator.py
Outdated
| # Also subscribe to buttons for engage/disengage | ||
| try: | ||
| if self.buttons.transport: | ||
| self._buttons_unsub = self.buttons.subscribe(self._on_buttons) | ||
| logger.info("Subscribed to buttons for engage/disengage") | ||
| except Exception as e: | ||
| logger.debug(f"Could not subscribe to buttons: {e}") |
There was a problem hiding this comment.
I see this a lot (checking if transport is truthy) and I don't think it's good.
In this case, if the button transport isn't working, the application is completely broken, no? Does it actually happen? If it does, that's a problem and we need to fix it.
But also, it's not good to catch an exception and log it as debug. It should be at least a warning.
There was a problem hiding this comment.
This check happens in if has_cartesian_ik or teleop_task. self.buttons.transport returns true for teleop_task and false for cartesian_ik (here, it should not fail, coz we dont need buttons for cartesian_ik).
I changed the structure now. Writing a different check for if has_teleop_task. So that it fails if there's no self.buttons.transport.
There was a problem hiding this comment.
There are three input streams. Joint_command, cartesian_command and buttons.
missing .transport in xase of self.joint_command.transport and self.cartesian_command.transport doesn't mean its broken coz the task methods can be also called via task_invoke RPC, without needing a stream at all. But, if using .transport is bad, I can directly try to subscribe and return a warning in case its missing (and not fail).
But, this should fail for buttons as there is no RPC alternative. implemented this.
@mustafab0 please confirm this won’t affect the manipulation module.
I verified it and nothing should break.
dimos/control/coordinator.py
Outdated
| """Forward button state to all tasks that accept it.""" | ||
| with self._task_lock: | ||
| for task in self._tasks.values(): | ||
| if hasattr(task, "on_buttons"): |
There was a problem hiding this comment.
hasattr is almost always bad. Same with type: ignore[attr-defined]
The code is essentially saying "on the off chance that this object has a on_buttons method, call it with some random param. If the method is changed to require an additional param, mypy will never tell you because it doesn't know about it.
The simplest way to fix this is to define on_buttons in the ControlTask Protocol. This forces you to implement it on all tasks. In most cases, you can just implement the method as pass. (If this is too much code, you can define a BaseControlTask which implements all empty methods for such listeners, and if you inherit from it, only TeleopIKTask has to actually implement on_buttons.)
There was a problem hiding this comment.
yeah, makes sense. So, remove all 'hasattr' from coordinator, which means implementing for all methods in the callbacks in either of ways you mentioned? Implementing this.
Added a BaseTask that implements all methods, and taks overrides and implements respective methods.
dimos/control/coordinator.py
Outdated
| if hasattr(task, "on_cartesian_command"): | ||
| task.on_cartesian_command(msg, t_now) # type: ignore[attr-defined] |
dimos/control/blueprints.py
Outdated
| from dimos.utils.data import get_data | ||
|
|
||
| _PIPER_MODEL_PATH = str( | ||
| get_data("piper_description") / "mujoco_model" / "piper_no_gripper_description.xml" |
There was a problem hiding this comment.
I didn't notice this before, but this calls get_data at import time. Meaning if the user doesn't have the repo, the program blocks here until the repo is downloaded.
I think someone else handled this case before by passing a lambda which is evaluated by the module. @leshy What do you think?
There was a problem hiding this comment.
This was the previous implementation. But, this creates multiple methods everytime. What's the best way
def _get_piper_model_path() -> str
from dimos.utils.data import get_data
piper_path = get_data("piper_description")
return str(piper_path / "mujoco_model" / "piper_no_gripper_description.xml")
There was a problem hiding this comment.
Fixed. used @jeff-hykin LfsPath for lazy loading - get_data() now defers to build time. Fixed bugs in implementation and verified import no longer triggers download.
|
In person notes:
|
|
My bad @ruthwikdasyam looks like the LfsPath I added causes some breakage. I'll work with you on this today so we can it merged. |
| cartesian_command: In[PoseStamped] | ||
|
|
||
| # Input: Teleop buttons for engage/disengage signaling | ||
| buttons: In[QuestButtons] |
There was a problem hiding this comment.
Quest = MetaQuest buttons right? I think this needs to be generic. What would it look like if we were to add PicoButtons?
There was a problem hiding this comment.
buttons stream name is generic, but the message type is QuestButtons since that's the only controller we support right now. Once we add a second device (Pico or VisionPro), the right generic interface will be clearer. abstracting before, without a second device/case usually leads to the wrong API. Keeping as-is for now.
3 participants
Feature
Problems
Issue - Linear
closes DIM-446
closes DIM-421
Approach/Solution
TeleopIKTask- new control taskAccepts streaming cartesian delta poses from VR controllers, solves IK internally via Pinocchio at 100Hz.
on_buttons()listens toQuestButtons- hold primary to track, release to stopmax_joint_delta_degper tick, auto-disengages on stream timeout"left"/"right"/ both) for dual-arm setupsPinocchioIK- new standalone IK solverLightweight Pinocchio-based solver for real-time control (unlike
JacobianIKwhich requires fullWorldSpec). Provides FK, damped least-squares IK, pose conversion helpers, and safety utilities (check_joint_delta,get_worst_joint_delta).CartesianIKTaskrefactored to usePinocchioIK- replaced inline IK/safety/pose codeControlCoordinator- newbuttons: In[QuestButtons]stream,_on_buttons()handler,teleop_iktask type in factory,handfield onTaskConfigcoordinator_teleop_xarm6,coordinator_teleop_piper,coordinator_teleop_dualarm_teleop_xarm6,arm_teleop_piper,arm_teleop_dualArmTeleopModulereworked - newArmTeleopConfig.task_namesfor per-hand routing viaframe_id, removed old toggle engageBreaking Changes/Info
CartesianIKTask.set_target_pose()->on_cartesian_command()CartesianIKTask.set_target_pose_dict()->on_cartesian_command_dict()CartesianIKTaskConfigremoved:ik_max_iter,ik_eps,ik_damp,ik_dt,max_velocity.CartesianIKTaskinternals_solve_ik(),_safety_check(),_pose_to_se3()moved toPinocchioIKCartesianIKTask._pose_to_se3()removed - Replaced by standalonepose_to_se3()indimos.manipulation.planning.kinematics.pinocchio_ik.Import path changed.How to Test
Blueprint verification (required)
Manual verification
teleop_server.tsand launch a teleop blueprint above with hardware connected