PoliMopPolicy-in-the-Loop Motion Planning
in 3D Spaces for Mobile Robots
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.
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
The method
Put the policy inside the planning loop.
- 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.
- 02
Restore a state
Select the RRT tree node closest to the sample and restore its complete simulation state.
- 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.
- 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.
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.
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.
- Move to plank
- Pick up
- Move to gap
- Place plank
- 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
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}
}