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.
Figures & tables
Fig. 1: 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.
Fig. 2: Motion planning for a wheeled robot without (left) and with (right) path-guided sampling. Uniform sampling leads to broad exploration, whereas prioritizing unreached waypoints along a mass-point guide path concentrates the search along this route to the goal and reduces planning time ( Experiment E3 ).
Fig. 3: A humanoid in the construction-site scene, where it must first pick up and place a plank over the gap to reach the goal location. The blue line indicates the planned path. The dark blue dot indicates the start state, and the red dot indicates the goal state of a task. In this example, we have the task plan: 1) MoveTo the plank, 2) PickUp the plank, 3) MoveTo the gap, 4) Place the plank over the gap, and 5) MoveTo the goal location.
Fig. 4: In this scene, the wheeled robot must take the longer route via the ramp to reach the elevated platform, whereas the humanoid can follow a shorter route by climbing the stairs.
Config
Success
Time [s]
q25 [s]
q75 [s]
Preplan [s]
Path-Guided
99.7%
27.5
2.9
16.7
6.3
Whole-Path
98.6 %
31.0
2.5
18.9
6.4
No-Shortcut
98.2 %
30.3
3.7
20.6
6.2
Unguided
95.6 %
92.4
7.5
92.0
–
Misleading
92.6 %
129.8
11.0
94.9
6.1
TABLE I: Comparison of path-guided sampling configurations, aggregated across scenes and robots.
Scene type
Completed
Obstacle navigation
280/280
Elevation changes
310/310
Mazes
120/120
Planar baseline
120/120
Overall
830/830
TABLE II: Motion-planning task completion by scene type, aggregated across robots.
Scene
Robot
Completed
Time [s]
construction site
g1_hiking
10/10
41.1±
57.7
go2_parkour
10/10
132.4±
323.2
living room
g1_hiking
10/10
11.3±
18.9
go2_parkour
10/10
163.0±
150.3
small spiral ramp
car
10/10
25.5±
21.8
g1_hiking
10/10
92.9±
110.3
TABLE III: Task plan completion and planning times for representative scenes. Wall times are summed over motion-planning tasks within each successful task plan. A task plan is successful only if all its motion-planning tasks succeed and no error occurs. The construction-site task plan contains three motion-planning tasks; the other listed task plans contain one. The construction-site and living-room scenes are illustrated in Figs. 3 and 5 , respectively.
Fig. 5: This figure shows a humanoid in the living-room scene. The left image shows the planning process: The blue path is the guidance path generated using a simplified mass-point model, yellow dots indicate the sampled configurations in 3D space, which are used to grow the RRT. The thin yellow lines indicate the edges of the RRT that is built. The right image shows the humanoid executing the resulting path by walking from the start configuration (blue dot) to the goal configuration (red dot).
Robot
Configuration
Completed
Time [s]
go2_parkour
Aicardi
205/205
24.9 ±
49.1
Heading-Error
204/205
27.6±
136.6
Guided Monte Carlo
205/205
83.7±
292.9
g1_hiking
Aicardi
201/205
34.7 ±
160.1
Heading-Error
199/205
36.7±
180.9
Guided Monte Carlo
204/205
105.9±
471.1
TABLE IV: Comparison of high-level controllers by robot, aggregated across scenes. Times are reported as mean ± standard deviation.
Config
Success
Time [s]
q25 [s]
q75 [s]
Preplan [s]
Total [s]
3D
99.7%
27.5
2.9
16.7
6.3
33.8
Patch
99.1 %
35.1
3.8
20.3
24.4
59.5
TABLE V: Comparison of 3D and patch-based sampling, aggregated across scenes and robots. Total is the sum of the mean planning time and mean preplan time.
Robot
Config
Success
Time [s]
Preplan [s]
Total [s]
car
3D
100 %
21.1
23.9
45.0
Patch
100 %
36.9
70.9
107.8
go2_parkour
3D
100 %
63.1
15.2
78.3
Patch
100 %
59.7
69.5
129.2
g1_hiking
3D
100 %
111.9
16.5
128.4
Patch
100 %
226.9
79.2
306.1
TABLE VI: Comparison of 3D and patch-based sampling in the spiral ramp scene, with 10 runs per robot and sampling method. Total is the sum of the mean planning time and mean preplan time.
Rapidly-exploring random trees (RRTs) have been widely adopted for robot motion planning due to their robustness and theoretical guarantees. However, existing RRT-based planners require explicit goal configurations specified as numerical joint angles, while many practical applications provide goal specifications through visual observations such as images or demonstration videos where precise goal configurations are unavailable. In this paper, we propose visual-RRT (vRRT), a motion planner that enables visual-goal planning by unifying gradient-based exploitation from differentiable robot rendering with sampling-based exploration from RRTs. We further introduce (i) a frontier-based exploration-exploitation strategy that adaptively prioritizes visually promising search regions, and (ii) inertial gradient tree expansion that inherits optimization states across tree branches for momentum-consistent gradient exploitation. Extensive experiments across various robot manipulators including Franka, UR5e, and Fetch demonstrate that vRRT achieves effective visual-goal planning in both simulated and real-world settings, bridging the gap between sampling-based planning and vision-centric robot applications. Our code is available at https://sgvr.kaist.ac.kr/Visual-RRT.
The exploration of high-dimensional spaces remains a challenging problem, in particular in the presence of narrow passages and small clearances. We propose novel sampling-based path-planning methods for high-dimensional spaces combining Rapidly-exploring Random Trees (RRT) and Hit-and-Run (HAR) random walks by decoupling the point being extended from the direction of extension. We also show that RRT and HAR appear as special cases of a generic algorithm coupling the biases used for the point and direction extension, respectively. We further study a sparse-move strategy in which only a fraction p_r of the robots is moved at each step, helping both RRT and the proposed HAR algorithms handle cluttered instances. Tests are presented for two families of models: classical piano mover problems in 3D, and complex molecular systems involving tens of rigid domains moving relatively to one another -- the latter viewed as independent robots exploring the motion space SE(3)N . Within seconds on a standard laptop, our algorithms solve instances with up to 64 robots and 384 degrees of freedom. We conclude by suggesting one of our methods, HARF, as the method of choice for complex multi-robot planning problems, being up to two orders of magnitude faster than the classical RRT moving all robots at each step--when it succeeds at all, and still up to 2.4 fold faster on most instances when both use their best p_r.
Human videos could reduce the reliance on expensive robot demonstrations, but extracting task-relevant features remains challenging. We introduce 3PoinTr, a method that provides robot policies with a global 3D point-track plan predicted from human videos. Given an initial point cloud, 3PoinTr predicts dense 3D point tracks spanning the full task and compresses them into a small set of tokens that remain fixed throughout execution. At each policy query, the policy combines these plan tokens with the current observation to predict robot actions. This formulation enables the policy to extract useful motion cues from human videos. We find this formulation transfers exceptionally well, even when human demonstration motions are infeasible for robots. In controlled simulation and real-world evaluations, 3PoinTr achieves an average success rate 25.0 percentage points higher than the strongest behavior-cloning and video-pretraining baselines on real-world tasks, and an improvement of 29.6 percentage points in simulation. Ablations establish the benefits of our method, including predicting non-embodiment 3D tracks once as a global plan, Perceiver-IO compression, and a residual cross-attention mechanism. Our point-track prediction network also outperforms a strong 3D-flow baseline by retaining supervision for temporarily occluded points. Project page: https://adamhung60.github.io/3PoinTr/
Adam Hung, Bardienus Pieter Duisterhof, Jeffrey Ichnowski