Inverse, forward and other dynamic computations computationally optimized with sparse matrix factorizations Francesco Nori1 Abstract— We propose an algorithm to compute the dynamics the scope of the inverse, forward and hybrid dynamics of articulated rigid-bodies with different sensor distributions. even though they are of practical interest. The paper is Prior to the on-line computations, the proposed algorithm organized as follows. Section II present the notation. Section performs an off-line optimisation step to simplify the computa- tional complexity of the underlying solution. This optimisation III presents a specific formulation of the Newton-Euler step consists in formulating the dynamic computations as a equations and Section III-A discuss the matrix form of system of linear equations. The computational complexity of these constraints. Section IV presents the inverse dynamics computing the associated solution is reduced by performing problem and its solution with the recursive Newton-Euler a permuted LU-factorisation with off-line optimised permu- algorithm (Section IV-A). Section V presents the forward tations. We apply our algorithm to solve classical dynamic problems: inverse and forward dynamics. The computational dynamics problem and its solution with the articulated body complexity of the proposed solution is compared to ‘gold algorithm (Section V-A). An alternative but computationally standard’ algorithms: recursive Newton-Euler and articulated comparable solution of the inverse and forward dynamics body algorithm. It is shown that our algorithm reduces the is proposed in Section IV-D and Section V-D respectively. number of floating point operations with respect to previous Finally, Section VI extends this solution to dynamic problems approaches. We also evaluate the numerical complexity of our algorithm by performing tests on dynamic computations for which do not fall within the scope of the inverse, forward which no gold standard is available. and hybrid dynamics. I. PREVIOUS WORKS II. NOTATION Real-time control of complex robots (such as humanoids) Let A and B be two arbitrary sets with cardinality jAj = asks for efficient ways of computing position, velocity, ac- m and jBj = n. For any element in a 2 A let’s associate celeration and applied wrenches on all the bodies composing a vector da. Similarly, for any pair of elements a; b with the robot articulated chain. Within this paper, we will refer to a 2 A and b 2 B, let’s associate a unique matrix Da;b. Let dynamic variables these quantities with the term even though p = (a1; : : : ; am) 2 SA and q = (b1; : : : ; bn) 2 SB be two they include positions and velocities which are strictly speak- permutations of the elements in A and B respectively1. We ing kinematic. Efficiently computing the dynamic variables define the permuted vector dp and the permuted matrix Dp;q of a (possibly free-floating) robot is nowadays a solved induced by p and q as follows: theoretical problem. Several state of the art algorithms [1] 2 3 2 3 have a computational complexity which scales linearly in da1 Da1;b1 :::Da1;bn the number of rigid links composing the robot. All these 6 . 7 6 . .. 7 dp = 4 . 5 ; Dp;q = 4 . 5 : (1) efficient algorithms can be obtained by exploiting the prob- d D :::D lem sparsity which derives from the iterative propagation of am am;b1 am;bn forces, torques and accelerations across articulated structures. As for dynamic quantities, in this paper we follow the Classical problems have been extensively optimized from the same notation used in [1] but we avoid using the bold computational complexity point of view. As discussed in [1, symbols for matrices, since the bold symbols are reserved for arXiv:1705.04658v1 [cs.RO] 12 May 2017 Table 10.1] the recursive Newton-Euler algorithm as imple- distinguishing Da;b, a 2 A, b 2 B from Dp;q, p 2 SA, q 2 mented in [2, Algorithm 5.7] is the most computationally SB; v denotes the spatial velocity (a six dimensional vector efficient solution of the inverse dynamics problem. Similarly, including angular velocities in the first three components and the articulated body algorithm as implemented in [3, method the rest as linear velocities), a denotes the spatial acceleration 3] is the most computationally efficient solution of the (again angular and then linear), f denotes the spatial force forward dynamics problem. Yet another relevant problem is (couples in the first three components and forces in the B the hybrid dynamics solution, which can be efficiently solved remaining), XA is the motion vector transformation from A B ∗ with the articulated-body hybrid dynamics [1, Section 9.2]. to B coordinates and XA is the analogous transformation In this paper we consider the computationally efficient for a force vector. Given a vector v and its coordinates solution of dynamic problems which do not fall within Av in A we denote with Av_ its temporal derivative in the A coordinates. The Euclidean cross operator × (on 3) 1 R Francesco Nori is with iCub Facility, Istituto Italiano di Tecnologia, is defined as the usual vector product, while for a spatial Italy. [email protected]. This paper was supported by the FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems 1 and Robotics), and An.Dy funded by the European Union’s Horizon 2020 Equivalently, let p 2 SA and q 2 SB, i.e. let p and q be two elements Research and Innovation Programme under Grant Agreement No. 731540. of the symmetric group on A and B respectively. velocity v the cross spatial cross operator and its dual ×∗ are defined as follows: a = iX a + S q¨ + c ; (3a ) i λi λi i i i i ! !× 0 ∗ ! ∗ !× p_× > v× = × = ; v× = × = τi = Si fi; (3τi) p_ p_× !× p_ 0 !× X i f = I a + ν − f x + X∗f : (3f ) In the following sections, we restrict the analysis to robots i i i i i j j i j2µ described by kinematic trees (i.e. robots with no kinematic i ∗ where we defined νi = vi × Iivi and ci = vi × Siq_i. loops) composed of NB rigid links numbered from 0 to NB, 0 being the selected fixed base (world reference frame) and 1 A. Dynamic constraints in matrix form being the selected floating base (reference rigid body in the Let’s define the set of dynamic variables D = fai, fi, τi, articulated chain). Links numbering is obtained by defining a x NB fi , q¨i gi=1. We have jDj = 5NB. The elements in D can be suitable spanning tree where each rigid link is associated to thought as generic quantities, not necessarily expressed in a a unique node in the tree. Numbers can be always selected in specific reference frame. As described in Section II, for any a topological order so that each node i has a higher number a 2 D, let’s associate a vector da (e.g. dai might correspond than its unique parent λi and a smaller number than all the to the coordinates of ai expressed in a specific reference nodes in the set of its children µi. Links i and λi are coupled frame). Given a permutation q 2 SD, dq contains an ordered with the joint i whose joint motion constraints are modelled 6×1 sequence of the elements in D expressed in specific reference with Si 2 R (therefore without loss of generality we are frame(s). assuming that all joints have a single degree of freedom). For Equations (3) can be seen as a set of equations which the each rigid link i, the system is modelled also by supplying vector of dynamic variables d 2 p have to satisfy. Let’s put these the spatial inertia tensor Ii and the motion-vector transform constraints in matrix form by defining the set of constraints from the reference frame of the rigid link i to the reference C = fc c c gNB jCj = 3N j (3ai), (3fi), (3τi) i=1. We have B. Given frame of the rigid link j, denoted Xi. For each link i the c 2 C let’s define vectors bc as follows: considered kinematic variables are: vi: the link spatial velocity, i X0a0 + ci if λi = 0 qi: the joint i position, bc(3a ) = ; i vi × Siq_i if λi 6= 0 q_i: the joint i velocity, bc = νi; Similarly, the dynamic variables associated to the link i are: (3fi) and bc = 0 otherwise. Given c 2 C and d 2 D, let’s define ai: the link spatial accelerations, matrices Dc;d as follows (where j 2 µi): q¨i: the joint i acceleration, τi: the joint i torque, f i λ i i: the spatial force transmitted to body from i, Dc(3a );ai = −16;Dc(3a );aλ = Xλi ; (4a) x i i i fi : external forces acting on body i. Dc(3a );q¨i = Si; (4b) Remark 1: All these variables are expressed in body i i x coordinates including fi which is more often expressed in D = −1 ;D = I ; (4c) c(3fi);fi 6 c(3fi);ai i absolute (i.e. body 0) coordinates. i ∗ D x = −1 ;D = X ; (4d) c(3fi);fi 6 c(3fi);fj j III. DYNAMIC CONSTRAINTS D = −1 ;D = S> (4e) Let’s assume that all kinematic quantities, i.e. those de- c(3τi),τi ni c(3τi);fi i pending on q and q_ have been precomputed. In practice, these and Dc;d = 0 otherwise. j j ∗ quantities are the transformation matrices Xi, Xi and the Given two permutations p and q of the elements in C and linear/angular velocities vJi, vi. The latter can be efficiently D, the permuted matrix Dp;q and permuted vectors dq, bp computed with the following recursion, propagated from satisfy the following linear equation: i = 1 to NB: Dp;q(q; q_)dq + bp(q; q_) = 0; (5) i vi = Xλi vλi + Siq_i; (2) where we explicitly indicated the dependency on q and q_.