The task

A wheeled robot with two arms runs five phases:

1. drive to the table
2. pick four objects into a bin
3. grasp the bin with both arms
4. carry the bin to the shelf
5. drive to the end zone

A referee scores positions and contacts. The maximum is 205 points.

How the code is split

Three files own the three jobs.

• ioai_env.py owns control: physics, kinematics, frames, base motion. It is fixed, treat it as the API.
• robot_state_machine.py owns the sequence: which phase runs next.
• referee/referee.py owns scoring: it reads the simulation and awards points.

Between them sit five swappable parts: pose estimator, grasp predictor, motion planner, path planner, and the state machine that calls them.

The robot is a mobile manipulator

Galbot "foxtrot": a wheeled base, two arms of 7 joints, a 2 joint head, a 4 joint torso lift, two grippers.

A degree of freedom (DOF) is one independent joint. The base has 3 planar DOF (forward, sideways, turn). Each arm has 7.

An arm needs 6 DOF to reach any hand pose. The 7th is redundancy: many joint sets reach the same pose, so one can dodge a limit or obstacle.

Joint space and task space

The same arm, described two ways.

• Joint space: the list of joint angles, 7 numbers per arm. Motors take these.
• Task space: the hand pose in the world, position and orientation, 6 numbers. You plan grasps here.

Kinematics is the conversion between the two.

Forward kinematics: where is the hand

Forward kinematics answers one question: given every joint angle, where is the hand.

Picture your own arm. Fix your shoulder, elbow, and wrist at known angles and your fingertip lands in exactly one spot. You get no choice about where. To compute it, start at the shoulder and chain each joint rotation and each bone length outward until you reach the hand.

This is the easy direction: one set of angles in, one hand pose out, a direct calculation with a single answer.

In this project the simulator already tracks every body, so forward kinematics is just reading the gripper frame pose from the state and converting it to the robot frame.

Inverse kinematics: which joints reach a target

Inverse kinematics is the reverse question: given where you want the hand, what joint angles put it there.

This is what you do without thinking. You see a cup, you decide "hand goes there", and your shoulder, elbow, and wrist sort themselves out. You picked a hand location, not joint angles, and the joints followed.

It is the hard direction:

• there can be many answers (reach the same spot with the elbow up or down)
• there can be no answer (the target is out of reach)
• there is no single formula, because the joints combine through sines and cosines

Inverse kinematics: how we solve it

We do not solve it in one shot. We nudge toward the answer.

1. measure the gap between where the hand is and where you want it
2. work out a small joint movement that shrinks that gap
3. apply it, then measure again
4. repeat until the hand is close enough

This is iterative, also called differential, inverse kinematics. Each step is a tiny correction, closer to steering toward a target than to computing the whole route at once. This project runs up to 50 of these steps per target.

mink: the IK library we use

mink is an open source Python library for differential inverse kinematics on MuJoCo models. This project uses it directly: ioai_env.py imports mink and builds the IK in _setup_mink and compute_inverse_kinematics.

You describe the goal as tasks, not equations:

• a FrameTask moves the gripper frame to the target pose (the grasp itself)
• a PostureTask keeps the arm near a natural, comfortable pose
• more FrameTasks pin the torso and base so they hold still while the arm moves

Each step mink finds the joint velocity that best satisfies all tasks at once while respecting joint limits. It does this by solving a quadratic program, a standard way to pick the best answer under hard limits (the solver here is daqp). solve_ik returns the velocity, integrate_inplace updates the joint angles, and the loop repeats.

Why inverse kinematics matters here

The loop does not always converge. It can stall at a joint limit and return its best pose with error left over (we saw up to 0.11 m).

That residual is why small objects are the hardest to grasp: a few centimeters of hand error still catches the large drill but misses a small cube. The solver is in ioai_env.py, which is fixed, so its precision caps grasp reliability.

Coordinate frames

A position needs a frame to be measured in. This project uses four:

• world: the fixed global frame
• robot base: attached to the chassis, moves with it
• robot initial: where the robot started, used for odometry
• camera: attached to a lens

ioai_env.py has a transform for every pair, for example world_to_robot_frame and camera_to_robot_frame.

What a transform is

A frame transform is a rotation, then a translation.

A grasp seen by the camera arrives in the camera frame. To use it: camera to world (the camera pose is known), then world to base (the base pose is known). The two compose into one camera to base transform.

Reverse a transform and the arm reaches a mirrored spot. Frame errors are always the most common bugs in robotics.

Orientation: quaternions

Orientation is a quaternion: four numbers (x, y, z, w), an axis and an amount to rotate.

Roll, pitch, yaw angles are readable but jump and lock when axes line up. Quaternions have neither problem, so libraries use them.

One trap here: MuJoCo orders a quaternion (w, x, y, z), scipy and most of the code order it (x, y, z, w). The helper xyzw_to_wxyz exists for this. Wrong order is silently wrong orientation.

Model and data

MuJoCo splits the world in two, and every function takes both.

• model (mjModel): the constant description. Bodies, joints, geoms, masses, timestep. Built once.
• data (mjData): the changing state. Positions (qpos), velocities (qvel), controls (ctrl), contacts, sensors. Rewritten each step.

In code: simulator.model._model and simulator.data._data.

The scene tree

A scene is a tree of bodies. Four terms cover what you read in the files.

• body: a rigid link with mass and a frame
• geom: a shape on a body, used for collision and display
• site: a named massless frame (the gripper tool center point is one, the IK aims at it)
• joint: how a body moves vs its parent (hinge, slide, or free for table objects)

Geom groups

Geoms carry a group number, so a body can hold a detailed visual shape and a simpler collision shape at once.

• groups 1 and 2: visible shapes (the viewer and renderer show these)
• groups 0 and 3: collision shapes

The path planner reads the collision shapes, because those are what the robot can hit.

One physics step

mujoco.mj_step(model, data) does, in order:

1. read the control inputs
2. compute forces, including gravity and actuation
3. detect contacts and the forces that stop overlap
4. integrate velocity and position forward one timestep

The timestep is a few milliseconds, so seconds of motion are hundreds of steps. mujoco.mj_forward does the same without the time integration, to refresh state for rendering and planning.

Contacts and scoring

Contacts are recomputed every step and stored in data (data.ncon counts them, each names the two touching geoms).

The referee reads them directly:

• a pick needs both gripper finger geoms touching the object at once
• a no-collision bonus fails when a chassis geom touches a wall, cone, table, or bin

The rule names the chassis, so an arm brushing the table costs nothing. Only the base counts.

The control loop and callbacks

Two calls that look alike and are not.

• sim.step() advances physics only.
• sim.loop() advances physics, then runs the registered physics callbacks.

The state machine and referee are callbacks, so they run only under loop(). Our runner copies the loop body to add a stop condition and print the score.

step physics -> run callbacks -> optional render -> repeat

Actuators and commands

You do not write joint angles into the state. Joints are driven by actuators through the control array, and the physics moves them over time.

The interface hides this:

• arms, head, leg take target positions: set_joint_positions([...])
• the base takes velocities: set_joint_velocities([forward, side, yaw])
• smooth motion uses follow_trajectory(...)

Rendering without a screen

Cameras and these slide images come from offscreen rendering on the GPU.

renderer = mujoco.Renderer(model, height=720, width=1280)
renderer.update_scene(data, camera, options)
rgb = renderer.render()

The robot cameras use the same path for rgb and depth. Set MUJOCO_GL=egl for the headless GPU backend.

Viewer controls

The window is MuJoCo's launch_passive viewer. Navigation is mouse driven, there is no WASD.

• left drag: orbit the camera
• right drag: pan
• scroll: zoom
• double click: select a body
• Ctrl with left drag: twist the selected body
• Ctrl with right drag: push or pull it

Space pauses a managed run, Backspace resets, bracket keys cycle fixed cameras. Side panels toggle contacts, transparency, and geom groups.

The three scoring checks

Every rule in referee/rules.json is one of three kinds.

• 2D range: is the chassis world position inside a rectangle (the five zones)
• 3D range: is an object inside a box relative to another body (placements into bin or shelf)
• contact: read from the physics (picks and collisions)

The two score numbers

The referee reports two totals.

• total_score: full credit for every success
• state_based_score: reduced credit for detection tasks

Each pick and place carries a small state score. You earn the full value only if the object was found by vision, not read as ground truth.

Ground truth gives 205 total but 125 state based on the standard scene. Vision makes the 205 legitimate. That is why we built the vision path.

Architecture

Five components feed a state machine, which drives the environment.

The pose estimator reads the cameras. The grasp predictor turns a pose into a hand target. The motion planner solves arm IK. The path planner routes the base. The state machine calls them in order.

Each box is swappable. We replaced the path planner and the pose estimator, and removed two sources of randomness.

The state machine pattern

Each state is small and has two parts.

• on first entry: issue one command (follow a path, move the arm to a pose)
• on later calls: return whether it finished (a motion callback cleared, or a timer elapsed)

When a state reports done, the machine advances. One grasp is a chain: detect, get grasp pose, move above, lower, close, lift, move to bin, open.

Path planning: the problem

The baseline drew a straight line and ignored obstacles. On random scenes the cones move, so straight lines drive through them and lose the collision bonuses.

Our planner builds a real map and searches it. Black is blocked, orange marks cones, blue is the route from start (green) to table (red).

Path planning: building the map

The planner turns the scene into a grid of free and blocked cells.

1. read obstacle shapes from the model: table, walls, shelf, bin, cones
2. keep only parts near floor height, so the table top (above the base) is not a wall
3. grow each obstacle by the robot radius, so the robot becomes a single point
4. mark a cell blocked if its center falls in any grown shape

The growth in step 3 is the black border around each shape.

Path planning: the A* search

A* search finds a shortest path without exploring the whole grid.

It expands cells in order of a score: distance traveled so far, plus a straight-line estimate of distance left to the goal. The estimate never overshoots the truth, which is what makes the result a shortest path.

After the search, line-of-sight shortening drops waypoints the robot can reach directly, turning a staircase into smooth corners.

Where the map comes from

The grid is ground truth, not a sensor. The planner reads each obstacle's shape and current position straight from the MuJoCo model and state (model.geom_aabb plus data.geom_xpos), so there is no lidar and no perception in the path planner.

This is allowed for navigation. Only the object detection tasks lose points for using ground truth. The no-collision and zone rules do not, so reading obstacle positions costs nothing.

A real robot has no model to read. It would sense obstacles with a depth camera or lidar and build the map from those returns, as an occupancy grid or a costmap, updating it while it moves with SLAM.

Perception: pixels to a pose

Ground truth reads poses from the simulator, which is not allowed for full credit. For the four scored objects we estimate from the cameras:

1. YOLO segmentation marks the object's pixels (a mask)
2. masked depth becomes a 3D point cloud
3. the cloud is matched to the object's CAD mesh
4. the match gives the 6 DOF pose, in the robot frame

The bin uses ground truth (its tasks carry no detection penalty).

Perception: segmentation

Segmentation answers which pixels are this object. A YOLO model trained on labeled images outputs a mask per detection.

The shipped checkpoint did not detect the toy, so the toy was never found. I retrained the model to add it.

The mask isolates the object's depth pixels from the rest of the image.

How the model was trained: the data

The repository ships a labeled dataset: about 785 images, one polygon mask per object, six classes including the toy.

The labels are cheap because the simulator already knows the answer. The dataset generator in ioai/generate_yolo_dataset/ does this per frame:

• place a random set of objects on the table, render the head camera
• read the per-pixel segmentation, where each pixel carries the geom id that drew it
• keep the pixels whose geom id is the target, trace the contour, write it as a YOLO polygon

There is no hand labeling. The mask is exact because it comes from the renderer.

How the model was trained: the run

The shipped checkpoint did not detect the toy, so we fine-tuned a fresh one.

• base model: yolo11n-seg, the nano YOLO11 segmentation network
• data: 576 training images, 94 for validation, the six classes
• 40 epochs, image size 640, a fixed seed for a reproducible run
• about 8.6 minutes on the 4 GB GPU

Final validation mask mAP50 is about 0.99 and mAP50-95 about 0.84. The numbers are high because training and validation are both sim renders from one distribution, so this measures segmentation on sim images, not transfer to real photos.

Perception: depth to point cloud

A depth image stores distance per pixel. With the camera intrinsics (fx, fy, cx, cy), each masked pixel becomes a 3D point in the camera frame:

X = (u - cx) * depth / fx
Y = (v - cy) * depth / fy
Z = depth

Here u and v are the pixel column and row. Every masked pixel gives a partial point cloud, one view of the object surface.

Perception: registration

Registration finds the rigid transform that lays the CAD mesh onto the measured cloud. That transform is the pose.

• coarse: match shape descriptors, use RANSAC to find the transform that fits the most points
• fine: ICP pairs nearest points and tightens the fit

For symmetric shapes (cube, extrusion bar) the answer can be rotated 180 degrees and still be correct, which the grasp tolerates.

Grasping

With the pose known, the grasp predictor returns a hand pose. The state machine moves above the object, lowers, closes, lifts, carries to the bin, opens.

The referee counts the pick only when both fingers touch the object. The grasp is reliable for the larger drill and toy, marginal for the small cube and extrusion, where the IK residual leaves the fingers a few centimeters off.

Hidden randomness, twice

The same run scored differently each time, for the same reason in two places: a random input upstream of what we measured.

• the timed waits used wall-clock seconds, so the number of physics steps per wait changed with CPU load
• the Open3D registration samples points at random, so poses changed every run, and marginal grasps flipped

The fix

Both fixes are one idea: remove the randomness, then measure.

• waits now count simulation time, so a wait is always the same number of steps
• the registration is now seeded, so the same camera frame gives the same pose

The standard scene then became a stable 205 instead of swinging between 50, 180, and 205.

Rule to carry: a stage that feeds your metric must be deterministic before you tune anything downstream of it.

What we changed

How to run it

nix develop

python ioai/main_astar.py --scenario standard --perception vision
python ioai/main_astar.py --scenario random --seed 7 --gui
python ioai/main_astar.py --planner interp --scenario random --seed 42

Flags: --planner astar|interp, --scenario standard|random, --seed N, --perception gt|vision, --gui, --show-vision, --rerun, --rerun-save FILE. It prints the scorecard at the end.

Watching a run

Two ways to see perception and planning as the episode runs.

# cv2 window: head camera with the YOLO overlay
python ioai/main_astar.py --scenario standard --perception vision --show-vision --gui

# rerun: live viewer with every stream on one timeline
python ioai/main_astar.py --scenario standard --perception vision --rerun

# rerun: save a recording to open later (headless friendly)
python ioai/main_astar.py --scenario random --seed 7 --perception vision --rerun-save run.rrd

The rerun recording carries the head camera, the depth image, the YOLO detections, the A* occupancy grid with obstacle boxes, the robot trajectory, and the score, all on one sim-time clock (ioai/rerun_viz.py).

Where the score stands

The fixed scene is solved. The random gap is grasp precision on small objects, bounded by the IK in the fixed environment file.

Key points

• MuJoCo is a constant model plus a changing data, stepped in a loop, contacts every step.
• Robotics here is frames, forward and inverse kinematics, joint space and task space.
• The solution is a swappable pipeline: perception, grasp, motion, path, state machine.
• The wins: A* that reads real geometry, a vision path that includes the toy, removing two sources of randomness.
• Result: 205 on the fixed scene, 110 to 140 on random scenes.

Further reading: concepts

• Kinematics: forward, inverse, degrees of freedom
• Orientation: quaternions and rotation
• Planning: A* search, occupancy grid, SLAM, lidar, costmap
• Optimization: quadratic programming
• Perception: image segmentation, point cloud, pinhole camera model, ICP, RANSAC

Further reading: libraries and tools

• Simulator: MuJoCo docs, MuJoCo on GitHub
• Inverse kinematics: mink, daqp
• Perception: Ultralytics YOLO, Open3D, Open3D global registration
• Slides: Marp

First, know where the points are

205 total, in four buckets. Work against the rubric, not against guesswork.

A point lost to a collision counts the same as a point lost to a missed grasp. The cheapest points are the ones you already had.

Where this solution actually loses them

Run the referee across many seeds, then count how often each rule fails. From my own logs the ranking holds, though the absolute rates are inflated by debug runs:

Three bottlenecks: thin-object grasps, the arm clipping things, and the dual-hand bin grasp.

The one file you cannot edit

ioai_env.py is fixed. The IK solver, forward kinematics, and chassis control are off-limits.

So you do not change the solver. You change what you ask of it:

• a better target pose to solve for
• a second attempt when the first one misses
• a cleaner point cloud feeding that target
• the choice of which arm reaches

Everything below lives outside that file.

Tier 1: biggest return

Close the grasp loop, which recovers the extrusion and cube:

• re-detect from the wrist camera at pre-grasp, refine the target, then close
• detect a failed grasp and retry, instead of moving on
• match the gripper to the object's long axis (the PCA is already computed)

Pick the nearer arm:

• the standard scene hides this, since every object sits on the left
• random scenes scatter them, and anything on the right is out of left-arm reach

Plan the arm motion around obstacles:

• straight-line joint interpolation drags the arm through the table
• swap in a planner and lift before traversing, since 45 points of bonuses depend on it

Tier 2: better perception feeds everything

• multi-view fusion: nudge the head or base, fuse depth from a few angles, register a denser and less occluded cloud
• stronger global registration: TEASER++ handles outliers better than the RANSAC step, and it is already vendored in 3rd_party/ but unused
• temporal averaging: estimate the pose over a few frames and median-filter it

A better pose is a better grasp target, which is a better grasp. Perception sits upstream of everything in Tier 1.

Tier 3 and beyond

The bin is worth 50 points, and the dual-hand grasp is the flaky part. Take symmetric contact points from the detected bin pose, coordinate the timing, and check the gripper width before lifting.

Research directions:

• learned grasping: Contact-GraspNet predicts grasps straight from the point cloud
• learned 6D pose: FoundationPose drops the CAD registration step
• the honest map: build the A* grid from lidar or depth (SLAM), not the ground-truth boxes it reads now
• end-to-end imitation or reinforcement learning for the grasp itself

Things you should do:

Measure!!!, never guess.

1. run the referee across many seeds
2. aggregate the per-rule pass rate
3. attack the single most-failed rule
4. change one thing, re-run, compare

That loop finds where the points leak, the extrusion grasp and the arm collision, instead of polishing something that already worked. Seed everything first, or the measurement lies to you.