feat: Visual servoing

[LuigiVan01]

Files Created

dimos/manipulation/dynamic_tracking/__init__.py

Package init.

dimos/manipulation/dynamic_tracking/aruco_tracker.py

ArUco marker tracker module — completely rewritten for main's architecture:

• Subscribes to In[Image] (color) and In[CameraInfo] (intrinsics) from RealSense
• Detects ArUco markers, averages multiple marker poses
• Publishes camera_optical_frame → aruco_avg transform to TF tree
• Visual servo loop: Looks up marker in base_link frame via TF, computes reach pose with configurable offset, publishes PoseStamped on Out[cartesian_command]
• The PoseStamped.frame_id is set to the CartesianIKTask name, so the coordinator routes it correctly
• No more OrchestratorClient/RPC or internal IK — CartesianIKTask handles IK in the coordinator tick loop

dimos/manipulation/dynamic_tracking/calibrate_hand_eye.py

Eye-in-hand calibration tool:

• Interactive CLI: connects to XArm + RealSense, prompts user to move arm to different poses
• Collects paired (EE pose, marker-in-camera pose) samples
• Solves AX=XB using cv2.calibrateHandEye() (5 methods available)
• Saves EEF→camera transform as JSON (translation + quaternion)
• Includes load_calibration() and calibration_to_transform() helpers for blueprint use
• Reports mean pairwise consistency error

dimos/manipulation/dynamic_tracking/blueprints.py

Three visual servoing blueprints:

Blueprint	CLI command	Description
aruco_tracker_realsense	dimos run aruco-tracker	Camera-only detection, no arm
aruco_servo_mock	dimos run aruco-servo-mock	Full servo loop with mock arm
aruco_servo_xarm6	dimos run aruco-servo-xarm6	Full servo loop with real XArm6

Each wires: RealSenseCamera → ArucoTracker → PoseStamped → ControlCoordinator (with CartesianIKTask using Pinocchio IK via
LfsPath("xarm_description/urdf/xarm6/xarm6.urdf"))

Files Modified

dimos/robot/all_blueprints.py

Added 3 new blueprint entries: aruco-servo-mock, aruco-servo-xarm6, aruco-tracker

TF Tree (Full Chain)

base_link → (FK from joint_state) → ee_link → camera_link
→ camera_color_frame → camera_color_optical_frame → aruco_avg                                                                                                                         

Architecture Decision

Rather than rebasing the 25-commit ruthwik_dynamic_tracking branch (which was 216 commits behind main and referenced a completely different ControlOrchestrator API), I ported the
core concepts onto a fresh branch from main. Main has evolved significantly — ControlOrchestrator → ControlCoordinator, new CartesianIKTask with built-in Pinocchio IK. The new implementation is ~50% less code because it leverages CartesianIKTask rather than doing IK internally.

One-line Test Command

dimos run aruco-servo-mock    # mock arm + real RealSense camera
dimos run aruco-servo-xarm6   # real xArm6 + real RealSense camera                                                                                                                    

Gradual Testing

1. Import check (no hardware needed)

Just verify the code loads without errors in your dimos environment:

python3 -c "from dimos.manipulation.dynamic_tracking.aruco_tracker import ArucoTracker; print('OK')"
python3 -c "from dimos.manipulation.dynamic_tracking.blueprints import aruco_servo_mock; print('OK')"

2. Camera-only test (RealSense needed, no arm)

dimos run aruco-tracker

This runs detection only — verifies ArUco markers are detected and TF transforms are published. You'll need ArUco markers (DICT_4X4_50, 15mm) visible to the camera. Check the Rerun
viewer for annotated images and TF frames.

3. Mock servo loop (RealSense needed, no arm)

dimos run aruco-servo-mock

Real camera + mock arm. Verifies the full pipeline: detection → TF lookup → PoseStamped → CartesianIKTask. The mock arm won't move physically but you can watch joint state changes in
Rerun or on the /coordinator/joint_state LCM topic.

4. Full hardware test (RealSense + XArm6)

dimos run aruco-servo-xarm6

Real camera + real arm.

[greptile-apps]

Greptile Summary

This PR introduces a complete visual servoing system using eye-in-hand ArUco marker tracking: a new ArucoTracker module, three ready-to-run blueprints (aruco_tracker_realsense, aruco_servo_mock, aruco_servo_xarm6), and an interactive hand-eye calibration tool. The implementation cleanly leverages the existing CartesianIKTask and TF infrastructure rather than re-implementing IK.

Previously flagged concerns around estimatePoseSingleMarkers deprecation and failed-pose contamination of the average are fully addressed. Open P1 items from prior review rounds (unverified XArmAdapter API surface in calibrate_hand_eye.py, potential mm/m unit mismatch) remain unresolved and should be confirmed before hardware use.

Confidence Score: 4/5

Safe to merge for software/simulation paths; real-hardware path in calibrate_hand_eye.py has unresolved P1 API concerns from prior review rounds.

Existing P1 findings about XArmAdapter method names and mm/m unit handling in calibrate_hand_eye.py are still unresolved. New findings in this pass are P2 only (default marker_size mismatch). Score is capped at 4/5 by the outstanding P1.

dimos/manipulation/dynamic_tracking/calibrate_hand_eye.py — XArmAdapter API surface (connect/disconnect/read_cartesian_position) and unit consistency need verification before real-hardware use.

Important Files Changed

Filename	Overview
dimos/manipulation/dynamic_tracking/aruco_tracker.py	Core ArUco tracker module; deprecated API replaced with solvePnP, failed-pose filtering added. Minor concern: default marker_size (0.1 m) doesn't match blueprint usage (0.015 m).
dimos/manipulation/dynamic_tracking/blueprints.py	Three wiring blueprints for camera-only tracking, mock-arm servo, and real XArm6 servo; IP now configurable via env var, depth disabled correctly in all three.
dimos/manipulation/dynamic_tracking/calibrate_hand_eye.py	Interactive eye-in-hand calibration tool; unit-mismatch concern and XArmAdapter API surface previously flagged but unit handling now documented in get_ee_pose docstring.
dimos/manipulation/dynamic_tracking/init.py	Package init file; no logic.
dimos/robot/all_blueprints.py	Adds three new blueprint registry entries for aruco-servo-mock, aruco-servo-xarm6, and aruco-tracker-realsense, plus the aruco-tracker module entry.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart TD
    RS[RealSenseCamera\ncolor_image + camera_info] --> AT[ArucoTracker]
    AT -- solvePnP per marker --> PE[Marker Pose Estimation]
    PE -- avg position + quaternion --> TF[TF Publish\ncamera_optical_frame → aruco_avg]
    TF --> TFL[TF Lookup\nbase_link → aruco_avg]
    TFL -- PoseStamped\nframe_id=task_name --> CC[ControlCoordinator\nCartesianIKTask]
    CC -- Pinocchio IK --> JC[Joint Commands]
    JC --> HW{Hardware}
    HW --> MOCK[Mock Arm\naruco-servo-mock]
    HW --> XARM[XArm6\naruco-servo-xarm6]
    AT -- annotated_image --> VIZ[Rerun Viewer]

Loading

_{Reviews (4): Last reviewed commit: "Fix mypy errors" | Re-trigger Greptile}