Genesis: a mobile manipulator, registers to VLA

Abstract

Genesis is a mobile manipulator I build to learn robotics end to end, from ESP32 registers to vision-language-action policies. It started as one 5-DOF arm on a mecanum base. It now covers a dual-arm variant on a lift column, a whole-body planner that solves on CPU, keypoint perception on a frozen vision backbone, and one task stack that runs in MuJoCo, Isaac Sim, Gazebo and on the robot.

Introduction

No client, no deadline. The rule is that every abstraction is earned: a layer gets built once the math under it is written down. So it grew upward: firmware, kinematics, perception, autonomy, learning. The mechanical and electrical sides are mine too. The chassis and the lift column are Fusion 360, the servo driver board is KiCad.

The platform

• Base. Four mecanum wheels, ESP32 firmware, register-level PWM, hardware encoders, per-wheel PID.
• Arms. SO-101 5-DOF, one on the first build, two on a lead-screw lift column on the current one.
• Compute. Raspberry Pi 5 and two ESP32s, ROS 2 Jazzy, a Zenoh bridge to the dev machine.
• Power. Two 4S 18650 packs, actuators and logic separated, BMS and per-rail buck converters.

Motion planning

Whole-body planning runs on CPU and stays there. Collision checks are vectorized over batches of configurations, not one at a time, IK is solved batched and hardware agnostic, a coarse pass prunes before the fine one runs, and edges drop out as soon as a lane hits an obstacle.Toggle noteSingle instruction, multiple data: one operation evaluates many configurations at once across the CPU's vector lanes. Pinocchio and Coal answer the kinematics and collision queries, OMPL searches, TOPPRA re-times the path against torque limits. A dual-arm solve lands near 600 ms.

Semantic perception

A frozen DINOv3 ViT-S/16 does the seeing. Patch tokens from layers 6, 9 and 12 concatenate into one dense feature map, and small heads on top predict task-specific 2D keypoints, grasp contacts rather than boxes. Those heads train only on synthetic renders. Freezing the backbone is what carries them to real cameras zero-shot. PnP against a floor plane or a second view lifts the keypoints to a 6-DoF pose, and the same features stream into the policy, so grasping stays closed-loop.

Autonomy

Tasks are YAML: pick, place, explore, approach, policy. Macros expand into whole-body goals at runtime, and every goal carries an on_failure branch, so the tree recovers instead of stopping. Targets are named frames resolved against the live scene just before planning, never fixed coordinates. MuJoCo and Isaac Sim sit behind one executor interface, and MuJoCo compiles in memory from the URDF at boot rather than from a brittle model file. The stack above never learns which one is underneath.

Learning

The planner doubles as a scripted expert, so demonstrations are generated without teleoperation. ACT and SmolVLA train on them today. A learned policy plugs back into the behavior tree as one segment, driving the same executor as the planner.

Implementation status

Subsystem	Feature	Status
Hardware	Custom servo driver PCB (KiCad)	✅
Hardware	Chassis, arm mounts and lift column (Fusion 360)	✅
Hardware	Dual SO-101 arms on the lift column	🧪
Hardware	Power system restructure	🧪
Firmware	Register-level PWM + PCNT encoder interface	✅
Firmware	Wheel PID (anti-windup, derivative filter)	✅
Firmware	ESP32 ↔ RPi5 serial communication protocol	✅
Mobile Base	Mecanum kinematics (FK + IK)	✅
Mobile Base	Good Boy autonomous ball retrieval	✅
Mobile Base	Wheel odometry + TF2 broadcast	🧪
Mobile Base	IMU integration + EKF sensor fusion	🧪
Mobile Base	Frontier exploration + SLAM	🧪
Mobile Base	A* / RRT* autonomous navigation	🧪
Manipulator	Analytical, Newton-Raphson and batched IK	✅
Manipulator	IBVS + PBVS visual servoing	✅
Manipulator	Grasp planning	🔧
Motion Planning	Pinocchio + Coal kinematics and collision queries	✅
Motion Planning	OMPL collision-free path search	✅
Motion Planning	SIMD batch collision checking + early exit	✅
Motion Planning	TOPPRA time-optimal re-timing	✅
Motion Planning	Whole-body coordination (base + arms)	✅
Motion Planning	Dual-arm whole-body solve ~600 ms on CPU	✅
Perception	Camera intrinsic + hand-eye calibration	✅
Perception	LiDAR integration + scan filtering	✅
Perception	Frozen DINOv3 ViT-S/16 feature extraction	✅
Perception	2D keypoint heatmap heads	✅
Perception	6-DOF pose via PnP + floor / multi-view	✅
Perception	Synthetic keypoint data (Replicator, BlenderProc)	✅
Perception	Depth camera pipeline	🗓️
Autonomy	Hierarchical YAML task definitions	✅
Autonomy	Behavior tree with on_failure recovery	✅
Autonomy	Dynamic frame resolution (semantic targets)	✅
Autonomy	Unified executor protocol (sim ↔ real)	✅
Teleoperation	Gamepad teleop (base + arm)	✅
Teleoperation	MediaMTX live video stream	✅
Teleoperation	Custom handheld controller (6-DOF + screen)	🗓️
Teleoperation	Leader-follower arm teleoperation	🗓️
Teleoperation	VR teleoperation (Quest 3)	🗓️
System	Zenoh distributed compute (RPi5 ↔ dev machine)	🧪
System	rosbag2 logging + telemetry pipeline	🗓️
Learning	LeRobot / VLA data collection pipeline	✅
Learning	Autonomous demonstration generation	✅
Learning	ACT imitation policy	✅
Learning	SmolVLA fine-tuning	✅
Learning	Keypoint-conditioned policies	🧪
Learning	HIL-SERL + reward classifiers	🧪
Learning	Dataset management + versioning	🧪
Learning	Sim-to-real transfer	🔧
Learning	RL dexterous manipulation	🔧
Learning	pi0 / GR00T export	🗓️
Simulation	Gazebo + ROS2 integration (URDF, sensors)	✅
Simulation	MuJoCo digital twin (MjSpec from URDF at boot)	✅
Simulation	Isaac Sim rendering + domain randomization	🔧
Simulation	Isaac Lab / MJX parallel environments	🔧
Simulation	RL policy training (PPO / SAC)	🔧

✅ Complete · 🧪 Testing · 🔧 In Progress · 🗓️ Planned

Limits and next steps

• Hardware trails simulation. The dual-arm variant is proven in sim first.
• Keypoint-conditioned policies are training but not yet validated on the real arms.
• Navigation on hardware waits on the power restructure and the EKF.
• Teleoperation for data collection comes next, leader-follower then VR.