PoliMop: Policy-in-the-Loop Motion Planning in 3D Spaces for Mobile Robots
TL;DR A kinodynamic RRT planner that grows its tree with simulated rollouts of the robot's own locomotion policy, so one planner finds routes for a quadruped, a humanoid and a wheeled robot through multi-level 3D scenes. Guiding samples along a mass-point path cuts mean planning time by about 63%, and it solves all 830 benchmark planning problems in simulation.
What they do
Whether a robot can reach a goal depends on its body, its control policy and the current scene. PoliMop is a kinodynamic RRT planner that puts a physics simulator (MuJoCo) and the robot’s deployed locomotion policy inside the search loop, so the tree only grows where that robot-policy pair can actually go. It samples in 3D rather than on a 2.5D elevation map, so it plans across overlapping levels. Objects moved by prescribed task-plan steps, such as a plank placed as a bridge, stay moved for the rest of the search.
How it works
- Reduced state, full snapshots. Samples are compared by center-of-mass position, velocity and heading. Each node also stores the full simulator state so expansion can resume from it. A rollout is kept only if the robot stays upright and avoids disallowed collisions.
- Steering. A high-level controller (the Aicardi unicycle law by default) turns each sample into speed and yaw-rate commands. A pretrained policy (Go2 parkour, Unitree G1 hiking) or Guided Monte Carlo turns them into actuator inputs.
- Path-guided sampling. A mass-point model with a set jump height first plans a guide path. Half the samples come from its next unreached waypoints, and the rest stay global so a wrong guide can be abandoned. Dead-end leaves are pruned.
Results
- Over 660 problems per configuration, guided sampling succeeds in 99.7% vs 95.6% unguided, with 27.5 s mean planning (+6.3 s preplanning) vs 92.4 s, about 63% less. A deliberately misleading guide still gets 92.6%, at 129.8 s.
- With tuned per-robot settings, all 830 problems across obstacle, elevation, maze and flat scenes were solved.
- On a high stair only the G1 hiking policy succeeded. On ramp and low stair the car reached 90% and 70%, while the plain go2 setup failed every trial.
- Without the prescribed plank and block placements no robot reached the goal within 30,000 iterations; with them every robot-scene pair succeeded in 5 of 5 runs.
Limitations
Simulation only, never run on physical robots. The task plan is fixed and given, and pick and place are assumed to work. It needs robot-specific tuning, has no completeness guarantee, and assumes the mass-point guide transfers to the robot.
Videos
Figures




Abstract
Reaching a goal state in a complex environment often requires a robot to overcome challenging terrain and obstacles. Which routes are accessible depends on the robot's physical design, the control policy it uses, and how it modifies its surroundings. We present a kinodynamic rapidly-exploring random tree (RRT) planner that uses physics simulation to exploit skills encoded in the control policy, which allows it to find a path in the scene with respect to the robot's capabilities. The planner samples states from a reduced state space in 3D around the scene and expands the RRT toward each sample using policy rollouts. We guide sampling with a preplanned path for a simple mass-point model to focus exploration on promising regions leading toward the goal state, reducing planning time by up to 63%. Simulation experiments with quadrupeds, humanoids, and wheeled robots demonstrate that our approach supports drop-in integration of various robots and control policies, exploiting their physical and policy capabilities without modifying the underlying planning algorithm. Predefined environmental modifications, such as placing a plank across a gap to build a bridge, will automatically be picked up by the planner as the robot's reachable space expands.
Summaries are written automatically from each paper's text. Figures and videos come from the paper and its project page; all credit goes to the authors.
