Unitree G1 Humanoid Control Stack

Four stages per grasp

Each stage is its own ROS 2 component, so a single one can be pulled apart without disturbing anything around it. Localization runs off to the side on its own sensors, feeding the control stack rather than the grasp itself.

A camera frame goes in at the top and 29 joint targets come out at the bottom. Every stage runs against Isaac Sim over the same ROS 2 and DDS interfaces the hardware uses, so what gets tested is what ships.

Real-time control of the whole robot

Everything further up the stack rides on this layer, so it has to be quick and it has to be predictable. The loop runs at 500 Hz: every two milliseconds it pushes position, velocity, torque and gain targets out to all 29 motors over the Unitree SDK2 DDS transport.

Behaviors are pluggable. A polymorphic skill interface sits under a finite state machine, so each behavior is a self-contained module and exactly one of them owns the robot at a time — two subsystems never end up fighting over the same joint. Standing, hold-pose, wave and manipulation are written; adding another means writing a class, nothing more.

Safety is not bolted on afterward. Command queuing is thread-safe, joint limits get checked on everything going out, messages carry checksums, and every run writes a log worth going back through.

From pixels to a point in metres

The trained detector is exported to ONNX and runs under ONNX Runtime inside a C++ ROS 2 node. Python never touches the runtime path.

Color and depth arrive synchronized, with temporal filtering smoothing out flicker on the depth side. Each detection is then back-projected through the calibrated camera intrinsics — pixel plus depth in, position in metres out — and TF2 walks that point through the camera extrinsics into the robot's planning frame, where the motion planner can do something useful with it.

On the wrist-camera frame the robot deploys from, the detector picks up both grips at roughly 0.81 confidence apiece and the wheelchair body at 0.85, with a metric depth attached to each. Two real grasp targets, expressed in the robot's own frame, handed straight to the planner.

Bimanual motion planning

Push handles need both hands at once, which makes this a coordinated bimanual problem rather than two arms working separately. MoveIt2 is configured specifically for the G1 through the Setup Assistant: left and right 7-DoF arms as kinematic chains, with a combined group planning both at once and executing through a joint trajectory controller.

RRT-Connect does the planning — bidirectional, collision-checked against the live scene, and quick enough that working with it stays interactive. The C++ ManipulationController, written against the MoveIt2 C++ API, picks up detected centroids, moves them into the planning frame with TF2, synthesizes grasp poses off them, keeps the collision scene current, checks whether the IK is even feasible, then plans and executes.

The planner was exercised on arbitrary reachable targets across both arms in RViz before perception was ever attached, so by the time detections started showing up, that half was known to be solid.

Knowing where it stands

A grasp is only as good as where the robot happens to be standing, so it needs to know its own position and walk itself into a standoff pose in front of whatever it is going after.

FAST-LIO2 handles the LiDAR-inertial stream, fusing Livox MID360 point clouds with IMU data into odometry and a map. The simulated MID360 is built on Isaac Sim's RTX LiDAR sensor, with a relay node adding the intensity fields FAST-LIO2 expects so the SLAM stack takes simulated scans without modification. IMU comes through Isaac Lab's ImuCfg, with the transform tree published to ROS 2 via OmniGraph, and Nav2 commands the base to a specific pose in the map before manipulation starts.

Simulation first

Calling this a simulation environment undersells it — it is where the project gets built. It started from Unitree's unitree_sim_isaaclab repository and kept getting extended until it publishes everything the physical robot does: RTX LiDAR point clouds, color and depth camera streams, IMU data and the full TF tree, all pushed out of Isaac Sim through OmniGraph ROS 2 nodes.

Camera intrinsics get read from the simulator's own camera info instead of assumed, so back-projection in sim returns the same metric answer hardware does. A forked ROS 2 GUI tool with per-joint sliders and an embedded RViz2 view carries the bring-up and integration work.

Making four stacks agree

A vendor SDK, a ROS 2 distribution, a motion planning framework and a simulator each arrive carrying their own assumptions about transport, build and runtime. Unitree SDK2 and ROS 2 Humble ship separate builds of CycloneDDS; getting both loaded into a single process meant working out the library ordering and linkage, then pinning the DDS domain, network interface and loopback config for simulation-only runs.

The MoveIt2 configuration was stood up end to end, Qt platform backend for the Setup Assistant included, plus mesh path resolution in the G1 URDF so the full collision geometry actually makes it into the planning scene.

Where it stands

Everything is built and running against the simulated G1: control stack, perception node, synthetic data pipeline, MoveIt2 configuration, SLAM and navigation. The detector resolves both push handles with a metric depth on the exact wrist-camera view the robot grasps from, and the planner turns out coordinated dual-arm motion to arbitrary reachable targets. The stages are assembled into a live end-to-end demo, detect through execute.

The ManipulationController is where the time goes right now — collision scene, IK feasibility checking, and the plan-and-execute path that closes the last gap between a detected centroid and a commanded motion. After that: more synthetic training data, then bring-up on the physical robot.