A Randomized Kinodynamic Planner for Closed-Chain Robotic Systems

Kinodynamic rapidly-exploring random tree (RRT) planners are effective tools for finding feasible trajectories in many classes of robotic systems. However, they are hard to apply to systems with closed-kinematic chains, like parallel robots, collaborative arms manipulating an object, or legged robots keeping their feet in contact with the environment. The state space of such systems is an implicitly-defined manifold that complicates the design of the sampling and steering procedures, and leads to trajectories that drift from the manifold if standard integration methods are used. To address these issues, this article presents a kinodynamic RRT planner that constructs an atlas of the state space incrementally, and uses this atlas to generate random states, and to dynamically steer the system toward such states. The steering method exploits the atlas charts to compute locally optimal controls based on linear quadratic regulators. The atlas also allows the integration of the equations of motion using local coordinates, which eliminates any drift from the state space manifold and results in accurate trajectories. To the best of our knowledge, this is the first kinodynamic planner that explicitly takes closed kinematic chains into account. In this article, we illustrate the planner performance in significantly complex tasks involving planar and spatial robots that have to lift or throw a load using torque-limited actuators.