← Paper Digest

PoliMop: Policy-in-the-Loop Motion Planning in 3D Spaces for Mobile Robots

Yael Fassbind, Valentin N. Hartmann, Huang Su, Yijiang Huang, Stelian Coros

November 2026

Construction site: a plank is placed over the gap and the humanoid walks its planned path across.

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

Humanoid walking a planned path over steps through the living-room scene.
Quadruped following a planned path under a pergola and around ponds.

Figures

From Figure 1: With the same planner, a quadruped (left), humanoid (center) and wheeled robot (right) navigate low pa...
From Figure 1: With the same planner, a quadruped (left), humanoid (center) and wheeled robot (right) navigate low passages, stairs and narrow scaffold ramps.
From Figure 5: Planning in the living-room scene. Left: the mass-point guide path (blue), the configurations sampled ...
From Figure 5: Planning in the living-room scene. Left: the mass-point guide path (blue), the configurations sampled in 3D (yellow dots) and the RRT edges (thin yellow lines). Right: the humanoid walks the resulting path from start (blue dot) to goal (red dot).
From Figure 3: In the construction-site scene the humanoid must pick up a plank and place it over the gap before it c...
From Figure 3: In the construction-site scene the humanoid must pick up a plank and place it over the gap before it can reach the goal. The blue line is the planned path; the dark blue dot is the start and the red dot the goal of a task.
From Figure 4: The wheeled robot must take the longer route via the ramp to reach the elevated platform, while the hu...
From Figure 4: The wheeled robot must take the longer route via the ramp to reach the elevated platform, while the humanoid takes a shorter route up the stairs.
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.