Efficient and accurate partial derivatives of rigid-body dynamics for trajectory optimization

In the past few years, trajectory optimization has increasingly been used for motion planning in dynamic-legged robots, like humanoids, quadrupeds, and manipulators. Optimization-based closed-loop controllers like Model-Predictive Control solve trajectory optimization problems sequentially in a receding time horizon fashion. However, the partial derivatives of the rigid-body dynamics used for optimization take almost 90% of the run-time for the optimization problem. Hence, efficiency and accuracy for computing the derivatives determine how fast the optimization algorithms can converge. Conventionally, numerical methods like Finite-Difference and automated Chain-Rule methods called Automatic Differentiation (AD) have been used to compute the derivatives. However, numerical difference results in approximate derivatives, and hence compromises the accuracy, while AD methods often lead to increased run times due to the large amount of matrix-vector products in the algorithms. This work provides analytical recursive expressions and algorithms to compute the first and second-order partial derivatives of rigid-body dynamics, for robotic models with open-loop connectivity trees, multi-DoF joints, and external forces in the form of contacts. Featherstone's Spatial Vector Algebra (SVA) has been extensively used to derive the underlying analytical expressions. Firstly, the first-order partial derivatives of Inverse Dynamics are presented. These derivatives are then extended to derive the second-order derivatives of Inverse Dynamics by extending SVA for tensor objects. Then, efficient tricks to compute the first/second-order derivatives of Forward Dynamics using all the previously derived derivatives are presented. Then, related model-based recursive algorithms are developed and compared in run-time with Finite-Difference, Complex-Step, Automatic Differentiation, and manual chain-rule methods. For first-order Inverse-Dynamics derivatives, run-time analysis with the state-of-the-art library Pinocchio (in C++) shows improvements up to 2x for the ATLAS humanoid. Run-time comparison of the Inverse-Dynamics and Forward-Dynamics second-order derivatives with the AD approach shows speed-ups up to 11x, and 4x respectively. Open-source implementation in Pinocchio and Featherstone's library are also provided for all the developed algorithms. The resulting derivatives show machine precision accuracy for the first-order, and up to 10⁻¹⁰ for the second-order derivatives when compared with the Complex-Step method. The derivatives of the impact dynamics used to model the impulsive interaction of the robot with a rigid surface are also presented. Finally, a Multi-Shooting Differential Dynamic Programming (DDP) optimizer is used for solving a bounding gait optimization problem for a 2D 7-DoF planar quadruped model of the MIT Mini-Cheetah. Here, for the first time, both the first and the second-order derivatives are computed using analytical methods for the different modes -stance, impact, and flight for the quadruped. The Quasi-Newton (QN) method used to approximate the State Transition Matrix shows speed-ups up to 10x over the full DDP approach.Aerospace Engineerin

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

Discontinuous Molecular Dynamics for Semi-Flexible and Rigid Bodies

A general framework for performing event-driven simulations of systems with semi-flexible or rigid bodies interacting under impulsive torques and forces is outlined. Two different approaches are presented. In the first, the dynamics and interaction rules are derived from Lagrangian mechanics in the presence of constraints. This approach is most suitable when the body is composed of relatively few point masses or is semi-flexible. In the second method, the equations of rigid bodies are used to derive explicit analytical expressions for the free evolution of arbitrary rigid molecules and to construct a simple scheme for computing interaction rules. Efficient algorithms for the search for the times of interaction events are designed in this context, and the handling of missed interaction events is discussed.Comment: 16 pages, double column revte

Helicopter mathematical models and control law development for handling qualities research

Progress made in joint NASA/Army research concerning rotorcraft flight-dynamics modeling, design methodologies for rotorcraft flight-control laws, and rotorcraft parameter identification is reviewed. Research into these interactive disciplines is needed to develop the analytical tools necessary to conduct flying qualities investigations using both the ground-based and in-flight simulators, and to permit an efficient means of performing flight test evaluation of rotorcraft flying qualities for specification compliance. The need for the research is particularly acute for rotorcraft because of their mathematical complexity, high order dynamic characteristics, and demanding mission requirements. The research in rotorcraft flight-dynamics modeling is pursued along two general directions: generic nonlinear models and nonlinear models for specific rotorcraft. In addition, linear models are generated that extend their utilization from 1-g flight to high-g maneuvers and expand their frequency range of validity for the design analysis of high-gain flight control systems. A variety of methods ranging from classical frequency-domain approaches to modern time-domain control methodology that are used in the design of rotorcraft flight control laws is reviewed. Also reviewed is a study conducted to investigate the design details associated with high-gain, digital flight control systems for combat rotorcraft. Parameter identification techniques developed for rotorcraft applications are reviewed

Geometric integration on spheres and some interesting applications

Geometric integration theory can be employed when numerically solving ODEs or PDEs with constraints. In this paper, we present several one-step algorithms of various orders for ODEs on a collection of spheres. To demonstrate the versatility of these algorithms, we present representative calculations for reduced free rigid body motion (a conservative ODE) and a discretization of micromagnetics (a dissipative PDE). We emphasize the role of isotropy in geometric integration and link numerical integration schemes to modern differential geometry through the use of partial connection forms; this theoretical framework generalizes moving frames and connections on principal bundles to manifolds with nonfree actions.Comment: This paper appeared in prin

Efficient minimization of multipole electrostatic potentials in torsion space

The development of models of macromolecular electrostatics capable of delivering improved fidelity to quantum mechanical calculations is an active field of research in computational chemistry. Most molecular force field development takes place in the context of models with full Cartesian coordinate degrees of freedom. Nevertheless, a number of macromolecular modeling programs use a reduced set of conformational variables limited to rotatable bonds. Efficient algorithms for minimizing the energies of macromolecular systems with torsional degrees of freedom have been developed with the assumption that all atom-atom interaction potentials are isotropic. We describe novel modifications to address the anisotropy of higher order multipole terms while retaining the efficiency of these approaches. In addition, we present a treatment for obtaining derivatives of atom-centered tensors with respect to torsional degrees of freedom. We apply these results to enable minimization of the Amoeba multipole electrostatics potential in a system with torsional degrees of freedom, and validate the correctness of the gradients by comparison to finite difference approximations. In the interest of enabling a complete model of electrostatics with implicit treatment of solvent-mediated effects, we also derive expressions for the derivative of solvent accessible surface area with respect to torsional degrees of freedom