Inverse Dynamics Vs. Forward Dynamics in Direct Transcription Formulations for Trajectory Optimization

Inverse Dynamics vs. Forward Dynamics in Direct Transcription Formulations for Trajectory Optimization Henrique Ferrolho1, Vladimir Ivan1, Wolfgang Merkt2, Ioannis Havoutis2, Sethu Vijayakumar1 Abstract— Benchmarks of state-of-the-art rigid-body dynamics libraries report better performance solving the inverse dynamics problem than the forward alternative. Those benchmarks encouraged us to question whether that computational advantage would translate to direct transcription, where cal- culating rigid-body dynamics and their derivatives accounts for a significant share of computation time. In this work, we implement an optimization framework where both approaches for enforcing the system dynamics are available. We evaluate the performance of each approach for systems of varying complexity, for domains with rigid contacts. Our tests reveal that formulations using inverse dynamics converge faster, require less iterations, and are more robust to coarse problem discretization. These results indicate that inverse dynamics should be preferred to enforce the nonlinear system dynamics in simultaneous methods, such as direct transcription. I. INTRODUCTION Fig. 1: Snapshots of the humanoid TALOS [2] jumping. Direct transcription [1] is an effective approach to formulate and solve trajectory optimization problems. It works constraints are usually at the very core of optimal control by converting the original trajectory optimization problem formulations, and require computing rigid-body dynamics (which is continuous in time) into a numerical optimization and their derivatives—which account for a significant portion problem that is discrete in time, and which in turn can be of the optimization computation time. Therefore, it is of solved using an off-the-shelf nonlinear programming (NLP) utmost importance to use an algorithm that allows to compute solver. First, the trajectory is divided into segments and the dynamics of the system reliably, while achieving low then, at the beginning of each segment, the system state computational time. and control inputs are explicitly discretized—these are the In the study of the dynamics of open-chain robots, the decision variables of the optimization problem. Due to this forward dynamics problem determines the joint accelerations discretization approach, direct transcription falls under the resultant from a given set of joint forces and torques applied class of simultaneous methods. Finally, a set of mathemat- at a given state. On the other hand, the inverse dynamics ical constraints is defined to enforce boundary and path problem determines the joint torques and forces required to constraints, e.g., initial and final conditions, or intermediate meet some desired joint accelerations at a given state. In goals. In dynamic trajectory optimization, there exists a trajectory optimization, most direct formulations use forward specific set of constraints dedicated to enforce the equations dynamics to enforce dynamical consistency [3]. However, of motion of the system, the so-called defect constraints. This benchmarks have shown that most dynamics libraries solve arXiv:2010.05359v2 [cs.RO] 11 Mar 2021 paper discusses different ways of defining these constraints, the inverse dynamics problem (e.g., with the Recursive as well as their implications. Newton-Euler Algorithm) faster than the forward dynamics The dynamics defects are one of the most important problem (e.g., with the Articulated Body Algorithm) [4], constraints in optimization problems when planning highly [5]. For example, for the humanoid robot TALOS [2], the dynamic motions for complex systems, such as legged library Pinocchio [6] solves the inverse dynamics problem in robots. Satisfaction of these constraints ensures that the just 4 µs, while the forward dynamics problem takes 10 µs. computed motion is reliable and physically consistent with These differences in performance motivated us to question the nonlinear dynamics of the system. The dynamics defect whether the computational advantage of inverse dynamics would translate to direct transcription—where the dynamics 1 School of Informatics, University of Edinburgh, United Kingdom. problem needs to be solved several times while computing 2Oxford Robotics Institute, University of Oxford, United Kingdom. This research is supported by EPSRC UK RAI Hub for Offshore Robotics the defect constraints. Moreover, there is biological evidence for Certification of Assets (ORCA, EP/R026173/1), EU H2020 project suggesting that inverse dynamics is employed by the nervous Memory of Motion (MEMMO, 780684), and EPSRC as part of the Centre system to generate feedforward commands [7], while other for Doctoral Training in Robotics and Autonomous Systems at Heriot-Watt University and The University of Edinburgh (EP/L016834/1). studies support the existence of a forward model [8]—which Email address: [email protected] increased our interest in this topic. In this work, we present a trajectory optimization frame- Finally, to the best of our knowledge, there is no current work for domains with rigid contacts, using a direct tran- work directly comparing inverse dynamics against forward scription approach. Particularly, our formulation allows to dynamics in the context of direct methods. Posa et al. [15] define dynamics defect constraints employing either forward also identified that a formal comparison is important, but dynamics or inverse dynamics. We defined a set of evaluation missing so far. They argued that one of the reasons for tasks across different classes of robot platforms, including this was that the field had not yet agreed upon a set of fixed- and floating-base systems, with point and surface canonical and hard problems. In this paper, we tackle this contacts. Our results showed that inverse dynamics leads issue, and compare the two approaches on robots of different to significant improvements in computational performance complexity on a set of dynamic tasks. when compared to forward dynamics—supporting our initial The main contributions of this work are: hypothesis. 1) A direct transcription formulation that uses inverse dynamics to enforce physical consistency, for constrained II. RELATED WORK trajectory optimization in domains with rigid contacts. RigidBodyDynamics.jl (RBD.jl) [9], RBDL [10], Pinoc- 2) Evaluation of the performance of direct transcription chio [6], and RobCoGen [11] are all state-of-the-art software formulations using either forward or inverse dynamics, implementations of key rigid-body dynamics algorithms. for different classes of robot platforms: a fixed-base Recently, Neuman et al. [5] benchmarked these libraries and manipulator, a quadruped, and a humanoid. revealed interesting trends. One such trend is that implemen- 3) Comparison of performance for different linear solvers, tations of inverse dynamics algorithms have faster runtimes 1 and across strategies to handle the barrier parameter of than forward dynamics. Koolen and Deits [4] also compared the interior point optimization algorithm. RBD.jl with RBDL, and their results showed that solving We validated our trajectories in full-physics simulation and inverse dynamics was at least two times faster than solving with hardware experiments. We also open-sourced a version forward dynamics for the humanoid robot Atlas. Both of of our framework for fixed-base robots, TORA.jl [16]. these studies only consider computation time of rigid-body dynamics; they do not provide insight into how these algo- III. TRAJECTORY OPTIMIZATION rithms perform when used in trajectory optimization. Lee et al. [12] have proposed Newton and quasi-Newton A. Robot Model Formulation algorithms to optimize motions for serial-chain and closed- We formulate the model of a legged robot as a free-floating chain mechanisms using inverse dynamics. However, they base B to which limbs are attached. The motion of the used relatively simple mechanisms for which analytic deriva- system can be described with respect to (w.r.t.) a fixed inertial tives can be obtained. In this work, we are interested in frame I. We represent the position of the free-floating base dynamic motions of complex mechanisms in domains with w.r.t. the inertial frame, and expressed in the inertial frame, 3 3 contact, for which the derivation of analytic derivatives is an as I rIB 2 R ; and the orientation of the base as IB 2 R , error-prone process, involving significant effort. using modified Rodrigues parameters (MRP) [17], [18]. The In the same spirit, Erez and Todorov [13] generated a joint angles describing the configuration of the limbs of the nj running gait for a humanoid based on inverse dynamics under robot (legs or arms) are stacked in a vector qj 2 R , external contacts. This method allowed them to formulate where nj is the number of actuated joints. The generalized an unconstrained optimization where all contact states can coordinates vector q and the generalized velocities vector v be considered equally, contact timings and locations are of this floating-base system may therefore be written as optimized, and reaction forces are computed using a smooth 2 3 I rIB and invertible contact model [14] with convex optimization. νB q = 2 3 × 3 × nj ; v = 2 nv ; (1) 4 IB 5 R R R q_ R However, their approach requires “helper forces”, as well as q j tuning of contact smoothness and of the penalty parameters j > 6 on the helper forces to achieve reasonable-looking behavior. where the twist νB = [I vBB!IB] 2 R encodes the In contrast, our approach does not require helper forces or linear and angular velocities of the base B w.r.t. the inertial any tuning whatsoever; we consider contact forces as deci- frame expressed in the I and B frames, and nv = 6 + nj. sion variables and model contacts rigidly. Another difference For fixed-base manipulators, the generalized vectors of is that we formulate a constrained optimization problem and nj coordinates and velocities can be simplified to q = qj 2 R enforce the nonlinear system dynamics with hard constraints, nj and v = q_j 2 R , due to the absence of a free-floating base. which results in high-fidelity motions. This is especially important for deployment on real hardware, where dynamic B. Problem Formulation consistency and realism are imperative.

Load more