The map is ready, but G1 stops halfway: from Visual SLAM to Nav2 navigation

The previous experiment built a colored map while G1 walked. We then implemented travel to a destination using that map. Investigating intermediate stops and adding observations enabled G1 to follow a Nav2 route around shelves and stop near the goal.

A recognizable point cloud is not yet a traversability map

Stereo disparity revealed shelves and walls, but the observed floor did not form a sufficiently connected route. Treating missing points as free space would incorrectly make unseen areas traversable.

We separated stereo cuVSLAM localization from floor/obstacle mapping with an ideal simulator depth camera. This depth is not a result of stereo depth estimation. Ground-truth warehouse geometry and positions were not passed to mapping or driving control.

Reaching the goal was not enough: the path crossed unknown space

The first detour reached its target, but post-run evaluation showed part of the trajectory crossing unknown cells. We rejected that run and prohibited unknown space in route following as well as planning. In the next test, Nav2’s forward collision prediction stopped the robot before the goal.

The map lacked floor and line-of-sight observations in part of the aisle. Instead of deleting forbidden areas or loosening free-space rules, we integrated depth from additional passes. Only records successfully matched to the same saved visual map were used. The rebuilt map contains 769 frames.

The grid is 10 cm and observed free area is about 188.83 m². Free cells require repeated floor and line-of-sight observations without obstacle observations. The height reference is estimated from the floor in each frame to reduce missed-floor detections caused by pose oscillation.

Relocalize, plan, follow and stop

We started 2 m beyond the mapping start and supplied a nearby initial pose estimate for image matching to the saved map. Motion waited for successful relocalization. Subsequent visual estimates provided Nav2’s pose. Planning uses Navfn and following uses Regulated Pure Pursuit.

The final run reached the original target after going around shelves. We recorded error at goal acceptance separately from error after deceleration and stopping.

Ground truth is used only for post-run evaluation, aligned at the initial mapping camera pose without whole-trajectory fitting. We measured collision geometry and checked that the whole-body envelope stayed inside observed free space. All 1,252 video frames were decoded to check for footage freezes.

What this verifies

This verifies travel from a specified starting region to a specified target in a static warehouse. It is not complete exploration; gray unobserved areas remain. A nearby initial pose estimate is required, so this is not global relocalization from arbitrary locations.

Physical robots, replanning around moving obstacles, success rates over multiple trials and real-time performance are untested. Ideal depth excludes real camera dropouts and noise. Separating a visible map from a map the full robot can traverse makes the remaining test conditions explicit.

Environment: Isaac Sim 5.1.0, Isaac Lab 2.3.0, ROS 2 Humble, cuVSLAM 17 and Unitree’s matched 29-joint G1 model and public walking policy. Results are from TeamZ’s September 6, 2026 implementation and test records. Implementation scope and results (Japanese).