feat: use Pink IK for teleop tasks

[TomCC7]

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

• Small, safe change that does not need a tracking issue
• Linked issue or discussion: feat: use Pink for controller ik tasks #2992 (DIM-1272)

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]

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
3622	1	3621	76

View the top 1 failed test(s) by shortest run time

dimos.navigation.cmu_nav.modules.pgo.test_pgo_rosbag.TestPGORosbag::test_pgo_corrected_odometry

Stack Traces | 68.5s run time

self = <dimos.navigation.cmu_nav.modules.pgo.test_pgo_rosbag.TestPGORosbag object at 0x76e61b0e1070>

    def test_pgo_corrected_odometry(self) -> None:
        """Feed scan + odom at original timing and validate PGO outputs.
    
        Checks:
        - PGO produces corrected odometry messages
        - Corrected odometry tracks the input trajectory (no wild divergence)
        - Global map is published with non-zero points
        - TF corrections are published
        """
        if not PGO_BIN.exists():
            pytest.skip(f"PGO binary not found: {PGO_BIN}")
    
        window = load_rosbag_window()
        assert len(window.scans) > 0, "No scans in rosbag fixture"
        assert len(window.odom) > 0, "No odometry in rosbag fixture"
    
        lcm_instance = lcmlib.LCM()
    
        corrected_odom_collector = LcmCollector(topic=CORRECTED_ODOM_LCM, msg_type=Odometry)
        global_map_collector = LcmCollector(topic=GLOBAL_MAP_LCM, msg_type=PointCloud2)
        tf_collector = LcmCollector(topic=TF_LCM, msg_type=Odometry)
    
        corrected_odom_collector.start(lcm_instance)
        global_map_collector.start(lcm_instance)
        tf_collector.start(lcm_instance)
    
        stop_event = threading.Event()
        handle_thread = threading.Thread(
            target=lcm_handle_loop, args=(lcm_instance, stop_event), daemon=True
        )
        handle_thread.start()
    
        runner = NativeProcessRunner(
            binary_path=str(PGO_BIN),
            args=[
                "--registered_scan",
                SCAN_LCM,
                "--odometry",
                ODOM_LCM,
                "--corrected_odometry",
                CORRECTED_ODOM_LCM,
                "--global_map",
                GLOBAL_MAP_LCM,
                "--pgo_tf",
                TF_LCM,
                # Config params matching pgo_unity_sim.yaml
                "--key_pose_delta_deg",
                "10.0",
                "--key_pose_delta_trans",
                "0.5",
                "--loop_search_radius",
                "1.0",
                "--loop_time_thresh",
                "60.0",
                "--loop_score_thresh",
                "0.15",
                "--loop_submap_half_range",
                "5",
                "--submap_resolution",
                "0.1",
                "--min_loop_detect_duration",
                "5.0",
                "--global_map_voxel_size",
                "0.1",
                "--global_map_publish_rate",
                "1.0",
                "--unregister_input",
                "true",
                "--world_frame",
                "map",
                "--local_frame",
                "odom",
            ],
        )
    
        try:
            runner.start(capture_stderr=True)
            assert runner.is_running, "PGO binary failed to start"
            time.sleep(_PROCESS_STARTUP_SEC)
    
            feed_at_original_timing(
                lcm_instance,
                window,
                topic_map={
                    "odom": ODOM_LCM,
                    "scan": SCAN_LCM,
                },
            )
    
            time.sleep(_POST_FEED_DRAIN_SEC)
    
        finally:
            runner.stop()
            stop_event.set()
            handle_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT)
            corrected_odom_collector.stop(lcm_instance)
            global_map_collector.stop(lcm_instance)
            tf_collector.stop(lcm_instance)
    
        # -- Analysis --
        corrected_count = len(corrected_odom_collector.messages)
        global_map_count = len(global_map_collector.messages)
        tf_count = len(tf_collector.messages)
    
        logger.info(f"\n{'=' * 60}")
        logger.info("PGO NATIVE ROSBAG DEVIATION SCORE")
        logger.info(f"  Input scans:            {len(window.scans)}")
        logger.info(f"  Input odom messages:     {len(window.odom)}")
        logger.info(f"  Corrected odom outputs:  {corrected_count}")
        logger.info(f"  Global map outputs:      {global_map_count}")
        logger.info(f"  TF outputs:              {tf_count}")
    
        # Basic output checks
>       assert corrected_count > 0, "PGO produced no corrected odometry"
E       AssertionError: PGO produced no corrected odometry
E       assert 0 > 0

corrected_count = 0
corrected_odom_collector = LcmCollector(topic='/rbpgo_corr_odom#nav_msgs.Odometry', msg_type=<class 'dimos.msgs.nav_msgs.Odometry.Odometry'>, messages=[], timestamps=[])
global_map_collector = LcmCollector(topic='/rbpgo_global_map#sensor_msgs.PointCloud2', msg_type=<class 'dimos.msgs.sensor_msgs.PointCloud2.PointCloud2'>, messages=[], timestamps=[])
global_map_count = 0
handle_thread = <Thread(Thread-169 (lcm_handle_loop), stopped daemon 130729971279552)>
lcm_instance = <LCM object at 0x76e5e3ead230>
runner     = NativeProcessRunner(binary_path='.../result/bin/pgo', args=['--r...1', '--global_map_publish_rate', '1.0', '--unregister_input', 'true', '--world_frame', 'map', '--local_frame', 'odom'])
self       = <dimos.navigation.cmu_nav.modules.pgo.test_pgo_rosbag.TestPGORosbag object at 0x76e61b0e1070>
stop_event = <threading.Event at 0x76e5e3eafb90: set>
tf_collector = LcmCollector(topic='/rbpgo_tf#nav_msgs.Odometry', msg_type=<class 'dimos.msgs.nav_msgs.Odometry.Odometry'>, messages=[], timestamps=[])
tf_count   = 0
window     = RosbagWindow(odom=array([[     5.3674,           0,           0, ...,           0,           0,           1],
       [...,           1],
       [     0.2975,           0,           0,           0,           0,           0,           1]]))])

.../modules/pgo/test_pgo_rosbag.py:172: AssertionError

To view more test analytics, go to the Test Analytics Dashboard
_{📋 Got 3 mins? Take this short survey to help us improve Test Analytics.}

[TomCC7]

waiting for @KrishnaH96

[paul-nechifor]

People seem to really like these blueprint tests so I'll stop complaining...

[TomCC7]

No I guess it's the problem of how should we harness the agent to not do this 😂 I'll investigate

[paul-nechifor]

Usually Any is more convenient than object, but, of course, actually using a TypedDict is the best option. :)