feat: use Pink IK for teleop tasks - #3237
Draft
TomCC7 wants to merge 3 commits into
Draft
Conversation
Codecov Report❌ Patch coverage is @@ Coverage Diff @@
## cc/feat/ik-task-self-collision #3237 +/- ##
=================================================================
Coverage ? 74.88%
=================================================================
Files ? 1118
Lines ? 106850
Branches ? 10599
=================================================================
Hits ? 80018
Misses ? 23999
Partials ? 2833
Flags with carried forward coverage won't be shown. Click here to find out more.
🚀 New features to boost your workflow:
|
TomCC7
marked this pull request as ready for review
July 28, 2026 06:16
TomCC7
requested review from
Dreamsorcerer,
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
July 28, 2026 06:16
TomCC7
marked this pull request as draft
July 28, 2026 06:16
TomCC7
marked this pull request as ready for review
July 28, 2026 21:35
Member
Author
|
waiting for @KrishnaH96 |
TomCC7
marked this pull request as draft
July 28, 2026 23:11
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Stack
Stacked on #2992. Review this PR against
cc/feat/ik-task-self-collision; after #2992 merges, retarget this PR tomain.closes DIM-1358
Contribution path
Problem
PR #2992 moves Cartesian and EEF-twist control onto the shared Pink pipeline, but
teleop_ikstill 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
TeleopIKTaskspecializeCartesianIKTaskand 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
RobotModelConfiginstances 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:
For xArm6, use the right Quest controller:
Validation completed:
git diff --checkpass.AI assistance
OpenAI Codex with GPT-5 was substantially involved in design exploration, implementation, tests, validation, and PR drafting.
Checklist