Title: Kinodynamic Planning on Constraint Manifolds,
Abstract: This paper presents a motion planner for systems subject to kinematic and
dynamic constraints. The former appear when kinematic loops are present in the
system, such as in parallel manipulators, in robots that cooperate to achieve a
given task, or in situations involving contacts with the environment. The
latter are necessary to obtain realistic trajectories, taking into account the
forces acting on the system. The kinematic constraints make the state space
become an implicitly-defined manifold, which complicates the application of
common motion planning techniques. To address this issue, the planner
constructs an atlas of the state space manifold incrementally, and uses this
atlas both to generate random states and to dynamically simulate the steering
of the system towards such states. The resulting tools are then exploited to
construct a rapidly-exploring random tree (RRT) over the state space. To the
best of our knowledge, this is the first randomized kinodynamic planner for
implicitly-defined state spaces. The test cases presented in this paper
validate the approach in significantly-complex systems.