Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness
(To Appear at the Transactions on Robotics)
The authors are affiliated to the Department of Computer Science at Rice University, Houston, TX, USA.
Lydia E. Kavraki is also with the Ken Kennedy Institute, Houston, TX, 77005 USA.
Corresponding author can be contacted at thaiduong (at) rice (dot) edu.
This work was supported in part by NSF 2336612, NSF 2411219 and ERDC W912HZ2320003.
Abstract: — Motion planning under dynamics constraints, i.e., kinodynamic planning, enables safe robot operation by generating dynamically feasible trajectories that the robot can accurately track. For high-DOF robots such as manipulators, sampling-based motion planners are commonly used, especially for complex tasks in cluttered environments. However, enforcing constraints on robot dynamics in such planners requires solving either challenging two-point boundary value problems (BVPs) or propagating robot dynamics, both of which cause computational~bottlenecks that drastically increase planning times. Meanwhile, recent efforts have shown that sampling-based motion planners can generate plans in microseconds using parallelization, but are limited to geometric paths. This paper develops FLASK, a fast parallelized sampling-based kinodynamic motion planning framework for a broad class of differentially flat robot systems, including manipulators, ground and aerial vehicles, and more. Differential flatness allows us to transform the motion planning problem from the original state space to a flat output space, where an analytical time-parameterized solution of the BVP problem can be obtained. A trajectory in the flat output space is then converted back to a closed-form dynamically feasible trajectory in the original state space, enabling fast validation via ``single instruction, multiple data" parallelism. Our framework is fast, exact, and compatible with any sampling-based motion planner, while offering theoretical guarantees on probabilistic exhaustivity and asymptotic optimality based on the closed-form BVP~solutions. We extensively verify the effectiveness of our approach in both simulated benchmarks and real experiments with cluttered and dynamic environments, requiring mere microseconds to milliseconds of planning time.
Overview
Our FLASK Approach
Ultrafast kinodynamic planning with FLASK: Motion planning for a "pick and place" task in a cluttered environment: a dynamically feasible trajectory (a) generated from our FLASK-RRTConnect can be accurately tracked by a UR5 robot. Meanwhile, tracking a geometric path (b) leads to collisions (shown in red) that topple the nearby boxes. Multiple intermediate states are overlaid to illustrate the robot's motion. Our planning framework is real-time and generates trajectories in ~90 microseconds by leveraging differential flatness and ``single instruction, multiple data" (SIMD) parallelism.
What is the main idea of FLASK? Our main approach is to transform the kinodynamic planning problem from the original state space to a lower-dimensional "flat" state space with linear dynamic. This leads to a closed-form time-parameterized solution of the boundary value problem, which enables fast forward kinematics, collision checking, and overall planning times.
How about completeness and optimality? Our FLASK approach can transform a geometric planner X into a kinodynamic planner FLASK-X, without breaking its probabilistic completeness and asymptotic optimality thanks to FLASK's probabilistic exhaustivity property.
Performance with DynoBench benchmarks: the trajectories from our kinodynamic FLASK-RRTConnect and our BVP-augmented FLASK-SST are plotted as blue and magenta solid curves, respectively, while those from the iDb-A* and SST* baselines are shown as orange dashed and green dotted curves, respectively, and the Kino-PAX trajectories are plotted as purple dash-dotted curves. FLASK-RRTConnect takes 0.5 - 6ms on average to generate a trajectory.
Performance with MotionBenchMaker benchmarks: robot trajectories are generated by our FLASK-RRT-Connect (top) and by the baseline planner, geometric RRTConnect + TOPP-RA (bottom), for a Franka Emika Panda robot in: (a) cage, (b) box, (c) bookshelf thin, and (d) table pick environments. For each environment, the bottom image shows a trajectory from the baseline with collided configurations in red (as time-parameterization by TOPP-RA does not consider collision avoidance), while the top image demonstrates that our trajectory is collision-free. FLASK-RRTConnect takes ~3.5ms on average to generate a trajectory.
Reactive planning with dynamic obstacles: our UR5 robot successfully performs a "pick, place, and reset" loop with our FLASK-RRTConnect planner without colliding with any of the static or moving obstacles. The obstacles are observed by an overhead Intel Realsense camera. FLASK-RRTConnect only takes ~90 microseconds to generate a trajectory in this scenario.
Citation
If you find our papers/code useful for your research, please cite our work as follows.
@article{duong26flask,
author = {Thai Duong, Clayton W. Ramsey, Zachary Kingston, Wil Thomason, and Lydia E. Kavraki},
title = {Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness},
journal = {IEEE Transactions on Robotics},
year = {2026},
}
Rice University Houston TX USA