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

Recursive mass matrix factorization and inversion: An operator approach to open- and closed-chain multibody dynamics

This report advances a linear operator approach for analyzing the dynamics of systems of joint-connected rigid bodies.It is established that the mass matrix M for such a system can be factored as M=(I+H phi L)D(I+H phi L) sup T. This yields an immediate inversion M sup -1=(I-H psi L) sup T D sup -1 (I-H psi L), where H and phi are given by known link geometric parameters, and L, psi and D are obtained recursively by a spatial discrete-step Kalman filter and by the corresponding Riccati equation associated with this filter. The factors (I+H phi L) and (I-H psi L) are lower triangular matrices which are inverses of each other, and D is a diagonal matrix. This factorization and inversion of the mass matrix leads to recursive algortihms for forward dynamics based on spatially recursive filtering and smoothing. The primary motivation for advancing the operator approach is to provide a better means to formulate, analyze and understand spatial recursions in multibody dynamics. This is achieved because the linear operator notation allows manipulation of the equations of motion using a very high-level analytical framework (a spatial operator algebra) that is easy to understand and use. Detailed lower-level recursive algorithms can readily be obtained for inspection from the expressions involving spatial operators. The report consists of two main sections. In Part 1, the problem of serial chain manipulators is analyzed and solved. Extensions to a closed-chain system formed by multiple manipulators moving a common task object are contained in Part 2. To retain ease of exposition in the report, only these two types of multibody systems are considered. However, the same methods can be easily applied to arbitrary multibody systems formed by a collection of joint-connected regid bodies

Recursive forward dynamics for multiple robot arms moving a common task object

Recursive forward dynamics algorithms are developed for an arbitrary number of robot arms moving a commonly held object. The multiarm forward dynamics problem is to find the angular accelerations at the joints and the contact forces that the arms impart to the task object. The problem also involves finding the acceleration of this object. The multiarm forward dynamics solutions provide a thorough physical and mathematical understanding of the way several arms behave in response to a set of applied joint moments. Such an understanding simplifies and guides the subsequent control design and experimentation process. The forward dynamics algorithms also provide the necessary analytical foundation for conducting analysis and simulation studies. The multiarm algorithms are based on the filtering and smoothing approach recently advanced for single-arm dynamics, and they can be built up modularly from the single-arm algorithms. The algorithms compute recursively the joint-angle accelerations, the contact forces, and the task-object accelerations. Algorithms are also developed to evaluate in closed form the linear transformations from the active joint moments to the joint-angle accelerations, to the task-object accelerations., and to the task-object contact forces. A possible computing architecture is presented as a precursor to a more complete investigation of the computational performance of the dynamics algorithms

Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation

Many algorithms for control, optimization and estimation in robotics depend on derivatives of the underlying system dynamics, e.g. to compute linearizations, sensitivities or gradient directions. However, we show that when dealing with Rigid Body Dynamics, these derivatives are difficult to derive analytically and to implement efficiently. To overcome this issue, we extend the modelling tool `RobCoGen' to be compatible with Automatic Differentiation. Additionally, we propose how to automatically obtain the derivatives and generate highly efficient source code. We highlight the flexibility and performance of the approach in two application examples. First, we show a Trajectory Optimization example for the quadrupedal robot HyQ, which employs auto-differentiation on the dynamics including a contact model. Second, we present a hardware experiment in which a 6 DoF robotic arm avoids a randomly moving obstacle in a go-to task by fast, dynamic replanning

Evolution of the DeNOC-based dynamic modelling for multibody systems

Dynamic modelling of a multibody system plays very essential role in its analyses. As a result, several methods for dynamic modelling have evolved over the years that allow one to analyse multibody systems in a very efficient manner. One such method of dynamic modelling is based on the concept of the Decoupled Natural Orthogonal Complement (DeNOC) matrices. The DeNOC-based methodology for dynamics modelling, since its introduction in 1995, has been applied to a variety of multibody systems such as serial, parallel, general closed-loop, flexible, legged, cam-follower, and space robots. The methodology has also proven useful for modelling of proteins and hyper-degree-of-freedom systems like ropes, chains, etc. This paper captures the evolution of the DeNOC-based dynamic modelling applied to different type of systems, and its benefits over other existing methodologies. It is shown that the DeNOC-based modelling provides deeper understanding of the dynamics of a multibody system. The power of the DeNOC-based modelling has been illustrated using several numerical examples

Differential-Algebraic Equations and Beyond: From Smooth to Nonsmooth Constrained Dynamical Systems

The present article presents a summarizing view at differential-algebraic equations (DAEs) and analyzes how new application fields and corresponding mathematical models lead to innovations both in theory and in numerical analysis for this problem class. Recent numerical methods for nonsmooth dynamical systems subject to unilateral contact and friction illustrate the topicality of this development.Comment: Preprint of Book Chapte