Kinodynamic planning on constraint manifolds

This report 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 validate the approach in significantly-complex systems.Peer ReviewedPreprin

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

Sampling-Based Motion Planning: A Comparative Review

Sampling-based motion planning is one of the fundamental paradigms to generate robot motions, and a cornerstone of robotics research. This comparative review provides an up-to-date guideline and reference manual for the use of sampling-based motion planning algorithms. This includes a history of motion planning, an overview about the most successful planners, and a discussion on their properties. It is also shown how planners can handle special cases and how extensions of motion planning can be accommodated. To put sampling-based motion planning into a larger context, a discussion of alternative motion generation frameworks is presented which highlights their respective differences to sampling-based motion planning. Finally, a set of sampling-based motion planners are compared on 24 challenging planning problems. This evaluation gives insights into which planners perform well in which situations and where future research would be required. This comparative review thereby provides not only a useful reference manual for researchers in the field, but also a guideline for practitioners to make informed algorithmic decisions.Comment: 25 pages, 7 figures, Accepted for Volume 7 (2024) of the Annual Review of Control, Robotics, and Autonomous System

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

Sampling-based optimal kinodynamic planning with motion primitives

This paper proposes a novel sampling-based motion planner, which integrates in RRT* (Rapidly exploring Random Tree star) a database of pre-computed motion primitives to alleviate its computational load and allow for motion planning in a dynamic or partially known environment. The database is built by considering a set of initial and final state pairs in some grid space, and determining for each pair an optimal trajectory that is compatible with the system dynamics and constraints, while minimizing a cost. Nodes are progressively added to the tree {of feasible trajectories in the RRT* by extracting at random a sample in the gridded state space and selecting the best obstacle-free motion primitive in the database that joins it to an existing node. The tree is rewired if some nodes can be reached from the new sampled state through an obstacle-free motion primitive with lower cost. The computationally more intensive part of motion planning is thus moved to the preliminary offline phase of the database construction at the price of some performance degradation due to gridding. Grid resolution can be tuned so as to compromise between (sub)optimality and size of the database. The planner is shown to be asymptotically optimal as the grid resolution goes to zero and the number of sampled states grows to infinity