Skip to content

feat: use Pink IK for teleop tasks - #3237

Draft
TomCC7 wants to merge 3 commits into
cc/feat/ik-task-self-collisionfrom
cc/feat/pinkik-for-teleop
Draft

feat: use Pink IK for teleop tasks#3237
TomCC7 wants to merge 3 commits into
cc/feat/ik-task-self-collisionfrom
cc/feat/pinkik-for-teleop

Conversation

@TomCC7

@TomCC7 TomCC7 commented Jul 28, 2026

Copy link
Copy Markdown
Member

Stack

Stacked on #2992. Review this PR against cc/feat/ik-task-self-collision; after #2992 merges, retarget this PR to main.

closes DIM-1358

Contribution path

Problem

PR #2992 moves Cartesian and EEF-twist control onto the shared Pink pipeline, but teleop_ik still uses the legacy Pinocchio solver with a model path and numeric joint ID. Quest teleop therefore has separate IK, safety, and model-configuration behavior.

The initial Pink migration also exposed two teleop-specific issues: orientation tracking needed balanced full-pose costs, and putting small per-tick velocity boxes directly in the ProxQP problem could produce false infeasibility during otherwise feasible motion.

Solution

Make TeleopIKTask specialize CartesianIKTask and reuse its measured-state Pink solve, bounded timestep, joint validation, joint-delta rejection, and measured-state hold behavior.

Teleop keeps engagement-relative control: it captures the measured end-effector pose once per engagement, composes controller translation and rotation deltas against that baseline, and discards the baseline on disengage, timeout, stop, clear, or E-STOP. E-STOP rejects pose and gripper commands while latched and cannot replay them after recovery.

Use a teleop-specific Pink policy with balanced translation and orientation costs, no canonical-posture task, small velocity damping, and a 1.0 rad/s global joint-speed ceiling. Keep position bounds in the QP, then uniformly scale the solved joint velocity against the effective model/global limits before integration. This preserves coordinated joint-space direction and avoids false ProxQP infeasibility from tiny velocity boxes.

Migrate Piper, xArm6, xArm7, and mixed-arm teleop blueprints to authoritative RobotModelConfig instances and named end-effector frames. Preserve gripper interpolation, resource claims, task routing, and the lower-priority xArm EEF-twist fallback.

How to test

Run a Quest teleop stack in simulation:

uv run --extra manipulation --extra misc dimos --simulation run teleop-quest-piper

For xArm6, use the right Quest controller:

uv run --extra manipulation --extra misc dimos --simulation run teleop-quest-xarm6

Validation completed:

  • Focused teleop, Cartesian IK, routing, coordinator, and blueprint tests pass.
  • Piper, xArm6, and xArm7 model checks produce bounded 1.0 rad/s effective limits.
  • Randomized Piper, xArm6, and xArm7 IK checks complete without QP failures.
  • Ruff formatting/lint and git diff --check pass.
  • GitHub CI passes on Python 3.10 through 3.14, Linux ARM, Linux self-hosted, macOS self-hosted, docs, Rust, and web jobs.

AI assistance

OpenAI Codex with GPT-5 was substantially involved in design exploration, implementation, tests, validation, and PR drafting.

Checklist

  • I have read and approved the CLA.

@codecov

codecov Bot commented Jul 28, 2026

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 82.47978% with 65 lines in your changes missing coverage. Please review.
✅ All tests successful. No failed tests found.

Files with missing lines Patch % Lines
...ol/tasks/cartesian_ik_task/test_pink_control_ik.py 9.37% 29 Missing ⚠️
...control/tasks/cartesian_ik_task/pink_control_ik.py 18.18% 18 Missing ⚠️
dimos/control/tasks/teleop_task/teleop_task.py 80.00% 6 Missing and 12 partials ⚠️
@@                        Coverage Diff                        @@
##             cc/feat/ik-task-self-collision    #3237   +/-   ##
=================================================================
  Coverage                                  ?   74.88%           
=================================================================
  Files                                     ?     1118           
  Lines                                     ?   106850           
  Branches                                  ?    10599           
=================================================================
  Hits                                      ?    80018           
  Misses                                    ?    23999           
  Partials                                  ?     2833           
Flag Coverage Δ
OS-ubuntu-24.04-arm 68.51% <81.40%> (?)
OS-ubuntu-latest 70.51% <81.40%> (?)
Py-3.10 70.51% <81.40%> (?)
Py-3.11 70.51% <81.40%> (?)
Py-3.12 70.50% <81.40%> (?)
Py-3.13 70.50% <81.40%> (?)
Py-3.14 70.51% <81.40%> (?)
Py-3.14t 70.51% <81.40%> (?)
SelfHosted-Large 29.31% <26.14%> (?)
SelfHosted-Linux 35.84% <26.14%> (?)
SelfHosted-macOS 34.95% <26.14%> (?)

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines Coverage Δ
.../tasks/cartesian_ik_task/test_cartesian_ik_task.py 100.00% <100.00%> (ø)
...imos/control/tasks/teleop_task/test_teleop_task.py 100.00% <100.00%> (ø)
dimos/robot/manipulators/common/blueprints.py 88.63% <100.00%> (ø)
dimos/robot/manipulators/common/mixed.py 100.00% <100.00%> (ø)
...imos/robot/manipulators/piper/blueprints/teleop.py 100.00% <ø> (ø)
dimos/robot/manipulators/piper/config.py 76.47% <ø> (ø)
dimos/robot/manipulators/test_blueprints.py 100.00% <100.00%> (ø)
dimos/robot/manipulators/xarm/blueprints/teleop.py 100.00% <100.00%> (ø)
dimos/robot/manipulators/xarm/config.py 89.28% <ø> (ø)
...control/tasks/cartesian_ik_task/pink_control_ik.py 24.15% <18.18%> (ø)
... and 2 more
🚀 New features to boost your workflow:
  • 📦 JS Bundle Analysis: Save yourself from yourself by tracking and limiting bundle sizes in JS merges.

@TomCC7
TomCC7 marked this pull request as ready for review July 28, 2026 06:16
@TomCC7
TomCC7 marked this pull request as draft July 28, 2026 06:16
@TomCC7
TomCC7 marked this pull request as ready for review July 28, 2026 21:35
@github-actions github-actions Bot added the ready-to-merge Required CI checks have passed on this PR label Jul 28, 2026
@TomCC7

TomCC7 commented Jul 28, 2026

Copy link
Copy Markdown
Member Author

waiting for @KrishnaH96

@TomCC7
TomCC7 marked this pull request as draft July 28, 2026 23:11
@github-actions github-actions Bot removed the ready-to-merge Required CI checks have passed on this PR label Jul 28, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant