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.
 This paper presents the first randomized approach to kinodynamic planning (also known as trajectory planning or trajectory design). The task is to determine control inputs to drive a robot from an ini ial configuration and velocity to a goal configuration and velocity while obeying physically based dynamical models and avoiding obstacles in the robot’s environment. The authors consider generic systems that express the nonlinear dynamics of a robot in terms of the robot’s high-dimensional configuration space. Kinodynamic planning is treated as a motion-planning problem in a higher dimensional state space that has both first-order differential constraints and obstacle-based global constraints. The state space serves the same role as the configuration space for basic path planning; however, standard randomized path-planning techniques do not directly apply to planning trajectories in the state space. The authors have developed a randomized planning approach that is particularly tailored to trajectory plannin... By means of the Udwadia-Kalaba approach we propose an explicit equation of constrained motion developed to simulate constrained dynamical systems without error accumulation due to constraint drift. The basic idea is to embed a small virtual force and a small virtual impulse to the equation of motion, in order to avoid the drift typically experienced in constrained multibody simulations. The embedded correction terms are selected to minimally alter the dynamics in an acceleration and kinetic energy norm sense. The formulation allows one to use a standard ODE solver, avoiding the need for iterative constraint stabilization. The equation is based on the pseudoinverse of a constraint matrix such that it can be used under redundant constraints and kinematic singularities. The proposed method takes into account the finite word-length of the computational environment, and also accommodates possibly inconsistent initial conditions. Multibody systems are often modeled as constrained systems, and theconstraint equations are involved in the dynamics formulations. To makethe arising governing equations more tractable, the constraint equationsare differentiated with respect to time, and this results in unstablenumerical solutions which may violate the lower-order constraintequations. In this paper we develop a methodology for numerically exactelimination of the constraint violations, based on appropriatecorrections of the state variables (after each integration step) withoutany modification in the motion equations. While the elimination ofviolation of position constraints may require few iterations, theviolation of velocity constraints is removed in one step. The totalenergy of the system is sometimes treated as another measure of theintegration process inaccuracy. An improved scheme for one-stepelimination of the energy constraint violation is proposed as well. Theconclusion of this paper is, however, that the energy conservation is ofminor importance as concerns the improvement of accuracy of numericalsimulations. Some test calculations are reported. ABSTRACT ABSTRACT A new class of methods for solving the equations of motion of constrained mechanical system dynamics is presented. The tangent space local parametrization is used to form an index one system of mixed differential-algebraic equations (DAEs) that describes the constrained motion. Implicit numerical integration formulas are applied to the reduced ordinary differential equations (ODEs) in the tangent space. The resulting system of nonlinear equations is solved by Broyden's method. In the framework presented, the computational complexity of solving the implicit integration equations for the reduced ODE is about same as that of some explicit integration methods for the reduced ODE. In fact, in some cases the implicit method for solving the Euler-Lagrange equations can be implemented more efficiently than the explicit scheme. A hallmark of multibody dynamics is that most formulations involve a number of constraints. Typically, when redundant generalized coordinates are used, equations of motion are simpler to derive but constraint equations are present. While the dynamic behavior of constrained systems is well understood, the numerical solution of the resulting equations, potentially of differential-algebraic nature, remains problematic. Many different approaches have been proposed over the years, all presenting advantages and drawbacks: the sheer number and variety of methods that have been proposed indicate the difficulty of the problem. A cursory survey of the literature reveals that the various methods fall within broad categories sharing common theoretical foundations. This paper summarizes the theoretical foundations to the enforcement in constraints in multibody dynamics problems. Next, methods based on the use of Lagrange’s equation of the first kind, which are index-3 differential algebraic equations in the presence of holonomic constraints, are reviewed. Methods leading to a minimum set of equations are discussed; in view of the numerical difficulties associated with index-3 approaches, reduction to a minimum set is often performed, leading to a number of practical algorithms using methods developed for ordinary differential equations. The goal of this paper is to review the features of these methods, assess their accuracy and efficiency, underline the relationship among the methods, and recommend approaches that seem to perform better than others. A hallmark of multibody dynamics is that most formulations involve a number of constraints. Typically, when redundant generalized coordinates are used, equations of motion are simpler to derive but constraint equations are present. Approaches to dealing with high index differential algebraic equations, based on index reduction techniques, are reviewed and discussed. Constraint violation stabilization techniques that have been developed to control constraint drift are also reviewed. These techniques are used in conjunction with algorithms that do not exactly enforce the constraints. Control theory forms the basis for a number of these methods. Penalty based techniques have also been developed, but the augmented Lagrangian formulation presents a more solid theoretical foundation. In contrast to constraint violation stabilization techniques, constraint violation elimination techniques enforce exact satisfaction of the constraints, at least to machine accuracy. Finally, as the finite element method has gained popularity for the solution of multibody systems, new techniques for the enforcement of constraints have been developed in that framework. The goal of this paper is to review the features of these methods, assess their accuracy and efficiency, underline the relationship among the methods, and recommend approaches that seem to perform better than others. The situation arising in path planning under kinematic constraints, where the valid configurations define a manifold embedded in the joint ambient space, can be seen as a limit case of the well-known narrow corridor problem. With kinematic constraints, the probability of obtaining a valid configuration by sampling in the joint ambient space is not low but null, which complicates the direct application of sampling-based path planners. This paper presents the AtlasRRT algorithm, which is a planner especially tailored for such constrained systems that builds on recently developed tools for higher-dimensional continuation. These tools provide procedures to define charts that locally parametrize a manifold and to coordinate the charts, forming an atlas that fully covers it. AtlasRRT simultaneously builds an atlas and a bidirectional rapidly exploring random tree (RRT), using the atlas to sample configurations and to grow the branches of the RRTs, and the RRTs to devise directions of expansion for the atlas. The efficiency of AtlasRRT is evaluated in several benchmarks involving high-dimensional manifolds embedded in large ambient spaces. The results show that the combined use of the atlas and the RRTs produces a more rapid exploration of the configuration space manifolds than existing approaches. Abstract When a given system of differential equations is integrated by numerical and automatic integration it may occur that the solution at hand satisfies an analytical relation which is a corollary of the differential equations but which is unknown to the automatic computer. An example of such a relation is the energy relation in conservative systems or the analytical relation generated by an outer holonomic or non-holonomic constraint provided the Lagrange equations of the first kind are used. It is shown that, in general, the computed numerical values of the solution satisfy such analytic relations with poor accuracy. The aim of the paper is to show how the analytical relations can be satisfied in a stabilized manner in order to improve the numerical accuracy of the solution of the differential equations. The proposed method leads to a modified differential system which is often stable in the sense of Ljapunov, whereas the original system is unstable. Despite the significant advances in path planning methods, highly constrained problems are still challenging. In some situations, the presence of constraints defines a configuration space that is a non-parametrizable manifold embedded in a high-dimensional ambient space. In these cases, the use of sampling-based path planners is cumbersome since samples in the ambient space have low probability to lay on the configuration space manifold. In this paper, we present a new path planning algorithm specially tailored for highly constrained systems. The proposed planner builds on recently developed tools for higher-dimensional continuation, which provide numerical procedures to describe an implicitly defined manifold using a set of local charts. We propose to extend these methods focusing the generation of charts on the path between the two configurations to connect and randomizing the process to find alternative paths in the presence of obstacles. The advantage of this planner comes from the fact that it directly operates into the configuration space and not into the higher-dimensional ambient space, as most of the existing methods do.