Autonomous Mobile Manipulation — Perceive, Navigate, Dock, Grasp, Place

Every layer of the stack was built and benchmarked separately, then integrated into one finite-state machine: a classical AprilTag pose detector and a learned CNN pose estimator for perception, an analytical IK solver and three learned grasp policies (Behaviour Cloning, PPO) for manipulation, and a goal-conditioned SAC+HER policy for base navigation.

The most valuable results were not the success rates but the five diagnosed failure mechanisms found along the way — a physics-level parking bug that masqueraded as a controller problem, chaotic seed-sensitivity in numerical IK, a reward-hacking RL policy, the categorical necessity of HER for sparse goal-reaching, and a recovery mechanism that was silently hiding navigation failures.

The MJCF scene contains a mobile base (holonomic in simulation), a 6-DoF UR5-style serial arm, a parallel-jaw gripper, a head camera on a mast, an eye-in-hand RGB-D camera on the wrist, a table, and a 5 cm target object carrying a tag36h11 AprilTag. A ROS 2 bridge exposes /joint_states, /joint_commands, /cmd_vel, and /object_pose — the same contract a real robot driver would sit behind.

A ROS 2 node detects a tag36h11 fiducial on the target object using cv2.aruco plus solvePnP with the IPPE_SQUARE method, and publishes the 6-DoF pose as a PoseStamped in the head camera's optical frame. The effective tag size (0.0393 m) was calibrated by rendering the tag at known depths and measuring the marker side in pixels.

Bringing perception up exposed three genuine bugs in the simulation model: the head camera was aimed nearly straight down at the floor and never saw the table, the target object was sunk 2 cm into the table (dropping the tag onto the table edge and killing its quiet zone), and without multisample anti-aliasing the small tag aliased badly enough to fail decoding.

A headless MuJoCo script renders RGB-D frames while randomizing object pose, camera viewpoint, lighting, and object/table colour, writing ground-truth 6-DoF pose labels in the same optical-frame convention as the AprilTag detector so the two are directly comparable. 10,000 samples generate in a couple of minutes.

An ImageNet-pretrained MobileNetV2 backbone with a regression head predicts the object's 6-DoF pose. Position is regressed as 3 outputs normalized by dataset statistics; rotation uses the continuous 6-D representation (Zhou et al., 2019) decoded to a rotation matrix via Gram–Schmidt, since quaternions regress poorly because of their sign discontinuity. Loss is smooth-L1 on normalized position plus a Frobenius rotation-matrix term.

The two methods are complementary rather than competing. The AprilTag is far more accurate and orientation-exact, but needs a high-resolution image and a visible, decodable tag facing the camera. The CNN works from low-resolution images and from viewpoints where the tag is too small or faces away, at roughly 4× the position error. The high rotation tail is honest: the target is a 5×5×10 cm box with a square cross-section, so its orientation is genuinely ambiguous from appearance under 90° and 180° symmetries.

An IKPY chain for the 6-DoF arm was built by hand from the MJCF, rooted at base_link with the tip at the gripper's grasp_site.

• Forward kinematics matches MuJoCo to 0.00 mm — the hand-built chain is exact.
• IK round-trip: 100% success under 5 mm over 300 random reachable targets.
• The arm alone reaches only ~0.83 m, so the object at x ≈ 1.2 m is out of range from home — the base must drive to a standoff first, which is what motivated the navigation policy.

All three grasp methods share the same interface so they are directly swappable: a 19-dimensional observation in the base frame (6 arm joint positions, 6 velocities, gripper opening, object position, object-minus-gripper vector) and a 7-dimensional action (6 arm joint targets plus a gripper command), executed through a shared rate-limited stiff-PD low-level controller.

An MLP trained on 500 demonstrations from the scripted IK expert. The first attempt fit the training loss to nearly zero but scored 0% in closed loop from compounding errors. Four changes fixed it: a reactive Markovian expert giving unambiguous observation-to-action mappings, absolute waypoint targets producing strong restoring actions, rate-limited execution, and DART-style noise injection during collection so the policy sees off-trajectory states with clean relabelled targets and learns to recover.

A shaped-reward Gymnasium environment trained with PPO across 20 parallel CPU environments at ~7,000 environment-steps per second; 3M steps takes about 8 minutes. It reached "100% success" on the lift-above-5 cm metric — and that turned out to be the project's most instructive failure (see Findings below).

The ladder inverted relative to the original headline (IK 73% → BC 75% → PPO "100%"). Once the environment bug was fixed and "success" meant actually holding the object, the classical pipeline won outright and the RL policy's metric gaming was fully exposed.

The navigation environment is a goal-conditioned Gymnasium environment with a sparse "arrived" reward. The base must drive from a random start pose to the grasp standoff. Trained with SAC + Hindsight Experience Replay (future strategy, 4 sampled goals), it converged to 100% success in about 120k steps (~8 minutes on one CPU environment).

• Observation: heading (cos/sin yaw) and body velocities; goals as position plus heading.
• Action: three velocity commands in [−1, 1].
• Arrival tolerance: 5 cm position, 10° heading.

Two model-level gotchas surfaced only once the base actually had to move, having previously only ever been teleported and pinned. The wheel geoms rested exactly on the floor inside the contact margin with no vertical degree of freedom, so the solver generated ~3,200 N per wheel to resist an unresolvable penetration and the resulting drag exactly cancelled the base actuators. And the original stow pose stuck the arm forward, colliding with the table top and blocking the approach ~30 cm short — fixed with a compact fold-up-and-back stow.

Detection range and accuracy both scale with resolution. At the 0.35–0.45 m refine distance even 640×480 is fine — resolution only matters at range.

This ablation found a real bug. The lateral budget initially measured under 2 mm — absurdly tight for a gripper with 13 mm of finger clearance per side. Tracing showed it was not mechanical (see Finding 2).

Post-fix budget: ~7.5–10 mm lateral, ±20 mm longitudinal — now set by mechanics rather than solver luck. This single number dictates the entire two-stage perception design.

A defense-in-depth result: the grasp survives losing the refine stage, but only because two other robustness layers cover for it. Place accuracy is the first casualty of the coarser estimate, so refine stays.

The project's most valuable outputs are five diagnosed-and-fixed failure mechanisms, each with a general lesson.

The honest summary is that the architecture transfers nearly as-is while most individual numbers do not — and the simulation already identified which ones are load-bearing.

The robot was later rebuilt from proper CAD meshes and re-skinned to resemble a commercial 6-DoF desktop manipulator, with the base-mounted wrist camera replaced by an eye-in-hand RGB-D camera. Both were visual-only changes on identical kinematics, and the full regression ladder re-validated at the same rates with zero deltas.

The full source, per-phase technical write-ups, ablation tables, dated session logs, and a demo video of vision-in-the-loop episodes are all in the repository.