PoliMopPolicy-in-the-Loop Motion Planning
in 3D Spaces for Mobile Robots

Yael FassbindValentin HartmannHuang SuYijiang HuangStelian Coros

Computational Robotics Lab, ETH Zurich, Switzerland

Plan with what a robot can do.

PoliMop finds paths that reflect the physical and control-policy capabilities of the selected robot–policy combination. By evaluating motions through policy rollouts in physics simulation, it discovers routes the robot can execute without changing the underlying planning algorithm.

A quadruped navigating a low passage, a humanoid climbing stairs, and a wheeled robot traversing a multilevel scaffold with narrow ramps. Each scene is shown above a close-up of the robot.
Our policy-in-the-loop motion planner enables diverse robots to traverse challenging environments. A quadruped (left), humanoid (center), and wheeled robot (right) navigate low passages, stairs, and narrow scaffold ramps.

Overview

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.

PoliMop in motion

Project video

Policy-in-the-loop planning, from the search process to navigation in complex 3D scenes.2:59

The method

Put the policy inside the planning loop.

  1. 01

    Sample a state

    Sample a reduced state in 3D space around the scene. The reduced state consists of 3D position, linear velocity, and heading angle.

  2. 02

    Restore a state

    Select the RRT tree node closest to the sample and restore its complete simulation state.

  3. 03

    Roll out the policy

    Using a high-level controller and a control policy, we roll out a motion that moves the robot toward the sampled state.

  4. 04

    Grow the tree

    Keep the motion if it satisfies the validity constraints. Repeat until the goal is reached.

Path-guided sampling for faster planning

Unguided exploration can spend time in regions that do not lead toward the goal, generating unnecessary rollouts. To focus exploration and reduce planning time, PoliMop uses a path-guided sampling strategy.

A simplified mass-point model first computes a guide path. PoliMop samples from its unreached waypoints to concentrate exploration on promising regions, while retaining global sampling so the search can deviate from the guide.

Rollouts of the actual robot and its control policy determine which motions are feasible.

UnguidedPath-guided
Two search trees for a wheeled robot: unguided sampling explores broadly, while guided sampling follows a shorter corridor toward the goal.
Path guidance concentrates the search. Figure 2 from the paper.
01 / Capabilities

The route depends on the robot and its policy.

In the same scene, a wheeled robot takes the longer route via a ramp, while a humanoid can reach the platform directly by climbing the stairs. These capabilities emerge from the simulated rollouts.

Side-by-side routes to an elevated platform: the wheeled robot uses the ramp on the left; the humanoid climbs stairs on the right.
Different feasible routes to the same goal. Figure 4 from the paper.
02 / Changing environments

New routes emerge as the environment changes.

A prescribed task sequence places a plank across a gap. The changed environment persists in the simulator, allowing the next motion-planning task to discover the newly reachable region.

A humanoid transports a plank in a construction-site simulation and crosses the gap after the plank has been placed as a bridge.
A supplied sequence of navigation, pick-up, and placement tasks. Figure 3 from the paper.
  1. Move to plank
  2. Pick up
  3. Move to gap
  4. Place plank
  5. Move to goal

The task order and placement are supplied in advance. PoliMop plans the motions within this sequence; it does not generate the manipulation strategy.

Explore the work

Paper & resources

PDFRead the full paperMethod, experiments, and discussion · 8 pages

Code coming soonThe implementation will be linked here when available.

Citation

@misc{fassbind_polimop,
  title = {{PoliMop}: Policy-in-the-Loop Motion Planning in 3D Spaces for Mobile Robots},
  author = {Fassbind, Yael and Hartmann, Valentin N. and Su, Huang and Huang, Yijiang and Coros, Stelian}
}