ROS 2 · 6D pose · zero-shot

FoundationPose ROS 2 Tracker

Track any object in 6D from just its 3D mesh and its name. YOLO-World finds it from a text prompt, SAM 2 segments it, and FoundationPose registers and tracks its pose. The node publishes a gravity-aligned PoseStamped that a robot can grasp from, with no per-object training.

Code & setup guide Demo bag (v1.0) Sample run data

Recorded RealSense RGB-D bag, tracked with the prompt "green cube" and green_cube.obj. The cube is lifted by hand and put back while three distractor objects stay on the table. Thick axes show the published Z-up pose.

How it works

01 · detectYOLO-World + SAM 2The object name is the detection prompt, and SAM 2 turns the box into a mask. You can confirm the proposal or click the object.

02 · registerFoundationPoseFull 6D pose from the masked RGB-D frame and the CAD or scanned mesh.

03 · trackHybrid looptrack_one() every frame on hole-filled depth, plus periodic re-anchoring and automatic re-acquisition.

04 · publishRobot-ready posePose in the world frame with Z up and yaw locked. It stays attached to the hand TF while a grasp hides the object.

On the robot cell

Live tracking with the top-down RealSense of a dual UR3 + Allegro Hand cell. The tracked pose drives a dexterous grasp planner.

Red cube pick-and-place: RViz, tracker window and lab camera

Plush tiger: a non-convex, deformable-looking object

Top-down overlay during approach, grasp and lift

mustard bottle tracked — Mustard bottle moved by hand

plush toy grasped — Plush toy in an Allegro grasp

Sample run

0.70YOLO-World confidence for "green cube" on first sight

345poses published over 26 s of recording

~9 cmhand-held motion (lift and put back) tracked without losing the lock

1.000Z-up dot product on every published pose

Published trajectory and orientation of the green cube

Try it

git clone https://github.com/ismael-esp/foundationpose-ros2-tracker.git
cd foundationpose-ros2-tracker
bash scripts/install.sh                     # conda env + FoundationPose + CUDA extensions + weights
bash scripts/replay_demo.sh "green cube"    # offline demo on the recorded bag, no camera needed

Tested on Ubuntu 22.04, ROS 2 Humble, an RTX 5070 Ti (Blackwell) with PyTorch 2.11 + CUDA 12.8, and RealSense D4xx. The README covers installation, configuration, adding your own objects, and troubleshooting.