Feat: Arm teleop with Pinocchio - single & dual arm

[ruthwikdasyam]

Feature

• Arm teleop support for Quest VR stack - any arm on DimOS can be teleoperated with Quest controllers
• Blueprints added to test with Piper, XArm6, or both (dual-arm)

Problems

• Safety Clamping is not a good idea, It might give out joint_positions that aren't IK valid. rejecting is better.
• pinnocchio IK must be defined seperately in dimos/manipulation/. Can be used for any IK computation

Issue - Linear

closes DIM-446
closes DIM-421

closes #1186

Approach/Solution

1. TeleopIKTask - new control task
   Accepts streaming cartesian delta poses from VR controllers, solves IK internally via Pinocchio at 100Hz.
   • Captures EE pose on engage; incoming poses applied as deltas from that reference
   • on_buttons() listens to QuestButtons - hold primary to track, release to stop
   • Safety: rejects solutions exceeding max_joint_delta_deg per tick, auto-disengages on stream timeout
   • Per-hand config ("left" / "right" / both) for dual-arm setups
2. PinocchioIK - new standalone IK solver
   Lightweight Pinocchio-based solver for real-time control (unlike JacobianIK which requires full WorldSpec). Provides FK, damped least-squares IK, pose conversion helpers, and safety utilities (check_joint_delta, get_worst_joint_delta).
3. CartesianIKTask refactored to use PinocchioIK - replaced inline IK/safety/pose code
4. ControlCoordinator - new buttons: In[QuestButtons] stream, _on_buttons() handler, teleop_ik task type in factory, hand field on TaskConfig
5. Teleop coordinator blueprints - coordinator_teleop_xarm6, coordinator_teleop_piper, coordinator_teleop_dual
6. End-to-end blueprints - arm_teleop_xarm6, arm_teleop_piper, arm_teleop_dual
7. ArmTeleopModule reworked - new ArmTeleopConfig.task_names for per-hand routing via frame_id, removed old toggle engage
8. XArm6 URDF added to LFS (Piper already existed)

Breaking Changes/Info

• Renamed CartesianIKTask.set_target_pose() -> on_cartesian_command()
• Renamed CartesianIKTask.set_target_pose_dict() -> on_cartesian_command_dict()
• CartesianIKTaskConfig removed: ik_max_iter, ik_eps, ik_damp, ik_dt, max_velocity.
  CartesianIKTask internals _solve_ik(), _safety_check(), _pose_to_se3() moved to PinocchioIK
• CartesianIKTask._pose_to_se3() removed - Replaced by standalone pose_to_se3() in dimos.manipulation.planning.kinematics.pinocchio_ik. Import path changed.
• CartesianIKTask safety: clamp -> reject - Previously, large joint deltas were clamped (interpolated toward target). Now the entire solution is rejected (returns None). Arm freezes instead of moving partially when delta exceeds limit.
• All tasks inherit BaseControlTask instead of ControlTask

How to Test

Blueprint verification (required)

# Start the WebXR bridge (in a separate terminal)
./dimos/teleop/quest/web/teleop_server.ts

Step - 2
# Single XArm6 arm teleop
dimos run arm-teleop-xarm6

# Single Piper arm teleop
dimos run arm-teleop-piper

# Dual arm teleop (XArm6 + Piper)
dimos run arm-teleop-dual

Manual verification

1. Start teleop_server.ts and launch a teleop blueprint above with hardware connected
2. Open Quest 3 headset, navigate to WebXR teleop page
3. Hold primary button (A on right / X on left) - arm should track controller deltas
4. Release button - arm should freeze at last position
5. Verify timeout: disconnect Quest mid-hold - arm should stop after 0.5s
6. Dual arm: verify each controller independently drives its assigned arm

[greptile-apps]

Greptile Overview

Greptile Summary

This PR adds VR teleop support for any arm on DimOS by introducing a new TeleopIKTask control task and refactoring IK solving into a standalone PinocchioIK utility.

Key changes:

• New PinocchioIK solver - lightweight Pinocchio-based IK with damped least-squares, independent of WorldSpec, provides FK, safety utilities, and pose conversion helpers
• New TeleopIKTask - accepts streaming cartesian delta poses from VR controllers, solves IK at 100Hz with button-based engage/disengage, timeout safety (0.5s), and per-hand config for dual-arm
• CartesianIKTask refactored - replaced ~70 lines of inline IK/safety code with PinocchioIK calls, renamed methods from set_target_pose* to on_cartesian_command*, changed from clamping excessive joint deltas to rejecting them entirely
• ControlCoordinator - added buttons input stream, _on_buttons() handler forwarding to all tasks, teleop_ik task type in factory, hand field on TaskConfig
• ArmTeleopModule - refactored to use task_names dict for per-hand routing via frame_id, removed old toggle engage logic (now uses parent's press-and-hold)
• Blueprints - added 3 coordinator blueprints (coordinator_teleop_xarm6, coordinator_teleop_piper, coordinator_teleop_dual) and 3 end-to-end blueprints (arm_teleop_xarm6, arm_teleop_piper, arm_teleop_dual)
• XArm6 URDF added to LFS

Breaking changes:

• CartesianIKTask.set_target_pose() → on_cartesian_command()
• CartesianIKTask.set_target_pose_dict() → on_cartesian_command_dict()
• CartesianIKTaskConfig removed fields: ik_max_iter, ik_eps, ik_damp, ik_dt, max_velocity (now in PinocchioIKConfig)
• Safety behavior change: CartesianIK now rejects solutions instead of clamping deltas

Confidence Score: 4/5

• This PR is mostly safe to merge with one logic issue that needs fixing
• The implementation is well-structured with good separation of concerns (PinocchioIK extraction, new TeleopIKTask). However, there's a missing error handler in coordinator.py that could cause runtime errors, and the CartesianIKTask behavior change from clamping to rejecting may cause usability issues during rapid motions
• dimos/control/coordinator.py needs the missing hasattr check restored to prevent AttributeError

Important Files Changed

Filename	Overview
dimos/manipulation/planning/kinematics/pinocchio_ik.py	New standalone Pinocchio-based IK solver with damped least-squares, forward kinematics, pose conversion utilities, and safety checking functions
dimos/control/tasks/teleop_task.py	New TeleopIKTask implementing delta-pose teleop control with button-based engage, timeout safety, and internal IK solving
dimos/control/tasks/cartesian_ik_task.py	Refactored to use PinocchioIK solver, replaced inline IK/safety code with shared utilities, renamed methods to on_cartesian_command* pattern
dimos/control/coordinator.py	Added buttons input stream, teleop_ik task type factory, hand config field, and button forwarding to all tasks with on_buttons method
dimos/control/blueprints.py	Added three teleop coordinator blueprints (xarm6, piper, dual) with TeleopIK tasks configured per hand, shared model path constants
dimos/teleop/blueprints.py	Added three end-to-end teleop blueprints connecting ArmTeleopModule to coordinators with task_names routing for single/dual arm setups
dimos/teleop/quest/quest_extensions.py	Refactored ArmTeleopModule to use task_names routing via frame_id instead of toggle engage (now uses parent's press-and-hold)

Sequence Diagram

sequenceDiagram
    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

Loading

_{Last reviewed commit: 1c08c76}

[greptile-apps]

_{11 files reviewed, 1 comment}

_{Edit Code Review Agent Settings | Greptile}

[greptile-apps]

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.

Suggested change

	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")

[ruthwikdasyam]

Fixed - Added error handling!

[paul-nechifor]

This ignore is not needed. You have to define config: ArmTeleopConfig after default_config = ArmTeleopConfig. Then remove this line.

[paul-nechifor]

These should be standalone functions too. They don't use PinocchioIK.

[ruthwikdasyam]

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?

[ruthwikdasyam]

Changed it to standalone method

[paul-nechifor]

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.

[ruthwikdasyam]

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.

[ruthwikdasyam]

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.

[paul-nechifor]

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.)

[ruthwikdasyam]

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.

[paul-nechifor]

Same as bellow.

[paul-nechifor]

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?

[ruthwikdasyam]

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")

[ruthwikdasyam]

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.

[jeff-hykin]

In person notes:

• AbsoluteDirectControl
• RelativeDirectControl

[jeff-hykin]

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.

[jeff-hykin]

Quest = MetaQuest buttons right? I think this needs to be generic. What would it look like if we were to add PicoButtons?

[ruthwikdasyam]

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.