Completeness of Randomized Kinodynamic Planners with State-based Steering

Probabilistic completeness is an important property in motion planning. Although it has been established with clear assumptions for geometric planners, the panorama of completeness results for kinodynamic planners is still incomplete, as most existing proofs rely on strong assumptions that are difficult, if not impossible, to verify on practical systems. In this paper, we focus on an important class of kinodynamic planners, namely those that interpolate trajectories in the state space. We provide a proof of probabilistic completeness for these planners under assumptions that can be readily verified from the system's equations of motion and the user-defined interpolation function. Our proof relies crucially on a property of interpolated trajectories, termed second-order continuity (SOC), which we show is tightly related to the ability of a planner to benefit from denser sampling. We analyze the impact of this property in simulations on a low-torque pendulum. Our results show that a simple RRT using a second-order continuous interpolation swiftly finds solution, while it is impossible for the same planner using standard Bezier curves (which are not SOC) to find any solution.Comment: 21 pages, 5 figure

A randomized kinodynamic planner for closed-chain robotic systems

Kinodynamic 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, cooperating arms manipulating an object, or legged robots keeping their feet in contact with the environ- ment. The state space of such systems is an implicitly-defined manifold, which complicates the design of the sampling and steering procedures, and leads to trajectories that drift away from the manifold when standard integration methods are used. To address these issues, this report presents a kinodynamic RRT planner that constructs an atlas of the state space incrementally, and uses this atlas to both generate ran- dom states, and to dynamically steer the system towards such states. The steering method is based on computing linear quadratic regulators from the atlas charts, which greatly increases the planner efficiency in comparison to the standard method that simulates random actions. The atlas also allows the integration of the equations of motion as a differential equation on the state space manifold, which eliminates any drift from such manifold and thus results in accurate trajectories. To the best of our knowledge, this is the first kinodynamic planner that explicitly takes closed kinematic chains into account. We illustrate the performance of the approach in significantly complex tasks, including planar and spatial robots that have to lift or throw a load at a given velocity using torque-limited actuators.Peer ReviewedPreprin

Probabilistic completeness of RRT for geometric and kinodynamic planning with forward propagation

The Rapidly-exploring Random Tree (RRT) algorithm has been one of the most prevalent and popular motion-planning techniques for two decades now. Surprisingly, in spite of its centrality, there has been an active debate under which conditions RRT is probabilistically complete. We provide two new proofs of probabilistic completeness (PC) of RRT with a reduced set of assumptions. The first one for the purely geometric setting, where we only require that the solution path has a certain clearance from the obstacles. For the kinodynamic case with forward propagation of random controls and duration, we only consider in addition mild Lipschitz-continuity conditions. These proofs fill a gap in the study of RRT itself. They also lay sound foundations for a variety of more recent and alternative sampling-based methods, whose PC property relies on that of RRT

Kinodynamic planning and control of closed-chain robotic systems

Aplicat embargament des de la data de defensa fins el dia 1/6/2022This work proposes a methodology for kinodynamic planning and trajectory control in robots with closed kinematic chains. The ability to plan trajectories is key in a robotic system, as it provides a means to convert high-level task commands¾like “move to that location'', or “throw the object at such a speed''¾into low-level controls to be followed by the actuators. In contrast to purely kinematic planners, which only generate collision-free paths in configuration space, kinodynamic planners compute state-space trajectories that also account for the dynamics and force limits of the robot. In doing so, the resulting motions are more realistic and exploit gravity, inertia, and centripetal forces to the benefit of the task. Existing kinodynamic planners are fairly general and can deal with complex problems, but they require the state coordinates to be independent. Therefore, they are hard to apply to robots with loop-closure constraints whose state space is not globally parameterizable. These constraints define a nonlinear manifold on which the trajectories must be confined, and they appear in many systems, like parallel robots, cooperative arms manipulating an object, or systems that keep multiple contacts with the environment. In this work, we propose three steps to generate optimal trajectories for such systems. In a first step, we determine a trajectory that avoids the collisions with obstacles and satisfies all kinodynamic constraints of the robot, including loop-closure constraints, the equations of motion, or any limits on the velocities or on the motor and constraint forces. This is achieved with a sampling-based planner that constructs local charts of the state space numerically, and with an efficient steering method based on linear quadratic regulators. In a second step, the trajectory is optimized according to a cost function of interest. To this end we introduce two new collocation methods for trajectory optimization. While current methods easily violate the kinematic constraints, those we propose satisfy these constraints along the obtained trajectories. During the execution of a task, however, the trajectory may be affected by unforeseen disturbances or model errors. That is why, in a third step, we propose two trajectory control methods for closed-chain robots. The first method enjoys global stability, but it can only control trajectories that avoid forward singularities. The second method, in contrast, has local stability, but allows these singularities to be traversed robustly. The combination of these three steps expands the range of systems in which motion planning can be successfully applied.Aquest treball proposa una metodologia per a la planificació cinetodinàmica i el control de trajectòries en robots amb cadenes cinemàtiques tancades. La capacitat de planificar trajectòries és clau en un robot, ja que permet traduir instruccions d'alt nivell com ara ¿mou-te cap aquella posició'' o ¿llença l'objecte amb aquesta velocitat'' en senyals de referència que puguin ser seguits pels actuadors. En comparació amb els planificadors purament cinemàtics, que només generen camins lliures de col·lisions a l'espai de configuracions, els planificadors cinetodinàmics obtenen trajectòries a l'espai d'estats que són compatibles amb les restriccions dinàmiques i els límits de força del robot. Els moviments que en resulten són més realistes i aprofiten la gravetat, la inèrcia i les forces centrípetes en benefici de la tasca que es vol realitzar. Els planificadors cinetodinàmics actuals són força generals i poden resoldre problemes complexos, però assumeixen que les coordenades d'estat són independents. Per tant, no es poden aplicar a robots amb restriccions de clausura cinemàtica en els quals l'espai d'estats no admeti una parametrització global. Aquestes restriccions defineixen una varietat diferencial sobre la qual cal mantenir les trajectòries, i apareixen en sistemes com ara els robots paral·lels, els braços que manipulen objectes coordinadament o els sistemes amb extremitats en contacte amb l'entorn. En aquest treball, proposem tres passos per generar trajectòries òptimes per a aquests sistemes. En un primer pas, determinem una trajectòria que evita les col·lisions amb els obstacles i satisfà totes les restriccions cinetodinàmiques, incloses les de clausura cinemàtica, les equacions del moviment o els límits en les velocitats i en les forces d'actuació o d'enllaç. Això s'aconsegueix mitjançant un planificador basat en mostratge aleatori que utilitza cartes locals construïdes numèricament, i amb un mètode eficient de navegació local basat en reguladors quadràtics lineals. En un segon pas, la trajectòria s'optimitza segons una funció de cost donada. A tal efecte, introduïm dos nous mètodes de col·locació per a l'optimització de trajectòries. Mentre els mètodes existents violen fàcilment les restriccions cinemàtiques, els que proposem satisfan aquestes restriccions al llarg de les trajectòries obtingudes. Durant l'execució de la tasca, tanmateix, la trajectòria pot veure's afectada per pertorbacions imprevistes o per errors deguts a incerteses en el model dinàmic. És per això que, en un tercer pas, proposem dos mètodes de control de trajectòries per robots amb cadenes tancades. El primer mètode gaudeix d'estabilitat global, però només permet controlar trajectòries que no travessin singularitats directes del robot. El segon mètode, en canvi, té estabilitat local, però permet travessar aquestes singularitats de manera robusta. La combinació d'aquests tres passos amplia el ventall de sistemes en els quals es pot aplicar amb èxit la planificació cinetodinàmica.Postprint (published version

Generalizing Informed Sampling for Asymptotically Optimal Sampling-based Kinodynamic Planning via Markov Chain Monte Carlo

Asymptotically-optimal motion planners such as RRT* have been shown to incrementally approximate the shortest path between start and goal states. Once an initial solution is found, their performance can be dramatically improved by restricting subsequent samples to regions of the state space that can potentially improve the current solution. When the motion planning problem lies in a Euclidean space, this region $X_{inf}$, called the informed set, can be sampled directly. However, when planning with differential constraints in non-Euclidean state spaces, no analytic solutions exists to sampling $X_{inf}$ directly. State-of-the-art approaches to sampling $X_{inf}$ in such domains such as Hierarchical Rejection Sampling (HRS) may still be slow in high-dimensional state space. This may cause the planning algorithm to spend most of its time trying to produces samples in $X_{inf}$ rather than explore it. In this paper, we suggest an alternative approach to produce samples in the informed set $X_{inf}$ for a wide range of settings. Our main insight is to recast this problem as one of sampling uniformly within the sub-level-set of an implicit non-convex function. This recasting enables us to apply Monte Carlo sampling methods, used very effectively in the Machine Learning and Optimization communities, to solve our problem. We show for a wide range of scenarios that using our sampler can accelerate the convergence rate to high-quality solutions in high-dimensional problems

Randomized kinodynamic planning for constrained systems

© 20xx IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.Kinodynamic RRT planners are considered to be general tools for effectively finding feasible trajectories for high-dimensional dynamical systems. However, they struggle when holonomic constraints are present in the system, such as those arising in parallel manipulators, in robots that cooperate to fulfill a given task, or in situations involving contacts with the environment. In such cases, the state space becomes an implicitly-defined manifold, which makes the diffusion heuristic inefficient and leads to inaccurate dynamical simulations. To address these issues, this paper presents an extension of the kinodynamic RRT planner that constructs an atlas of the state-space manifold incrementally, and uses this atlas both to generate random states and to dynamically steer the system towards such states. To the best of our knowledge, this is the first randomized kinodynamic planner that explicitly takes holonomic constraints into account. We validate the approach in significantly-complex systems.Postprint (author's final draft