Inverse, Forward and Other Dynamic Computations Computationally Optimized with Sparse Matrix Factorizations

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 computational complexity of the underlying solution. This optimisation III presents a speciﬁc 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 ﬂoating 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.PREVIOUSWORKS II.NOTATION Real-time control of complex robots (such as humanoids) Let A and B be two arbitrary sets with cardinality |A| = asks for efficient ways of computing position, velocity, ac- m and |B| = n. For any element in a ∈ 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 ∈ A and b ∈ B, let’s associate a unique matrix Da,b. Let dynamic variables these quantities with the term even though p = (a1, . . . , am) ∈ SA and q = (b1, . . . , bn) ∈ 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]     have a computational complexity which scales linearly in da1 Da1,b1 ...Da1,bn the number of rigid links composing the robot. All these  .   . .. .  dp =  .  , Dp,q =  . . .  . (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 ∈ A, b ∈ B from Dp,q, p ∈ SA, q ∈ 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 ∈ SA and q ∈ 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: i ai = Xλi aλi + Siqï + ci, (3ai) ω ω× 0 ω ω× p˙× v× = × = , v×∗ = ×∗ = τ = S>f , (3τ ) p˙ p˙× ω× p˙ 0 ω× i i i i 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 j∈µ 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 = {ai, fi, τi, articulated chain). Links numbering is obtained by defining a x NB fi , qï }i=1. We have |D| = 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 ∈ 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 ∈ 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 ∈ 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 = {c c c }NB |C| = 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 ∈ 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 ∈ C and d ∈ D, let’s define ai: the link spatial accelerations, matrices Dc,d as follows (where j ∈ µi): qï: 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ï = 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.DYNAMICCONSTRAINTS 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˙.

The dynamics quantities instead (τ, q¨, f x, . . . , f x , IV. INVERSEDYNAMICS 1 NB a , . . . , a f , . . . , f 1 NB , 1 NB ) have to satisfy the Newton-Euler The inverse dynamics problem consists in finding τ1, ... , equations: τ which satisfies (3) given q¨ , ... , q¨ , f x, ... , f x . NB 1 NB 1 NB In [1] the problem is formulated as the computation of the 2The spatial inertia tensor of the rigid link i has the following form in the link reference frame: following function: > x x IC,i + mici × ci mici× τ = InvD(model, q, q,˙ q,¨ f1 , . . . , fN ). (6) Ii = > , B mici× miI3×3 In the above equations we grayed out some variables that where I is the spatial inertia tensor with respect to the body’s centre C,i will not play a role in the following sections, and can be of mass, mi is the total mass, ci is the relative displacement between the centre of mass and the origin of the link reference frame. assumed either to be contestant (model) or measured (q, q˙). A. Inverse dynamics solved with RNEA Let’s also choose a permutation pid of the elements in Cid : An efficient solution of inverse dynamics is given by h x x pid = c(7f ), c(7¨q1) . . . c(7f ), c(7¨qN ), c(3a1) . . . c(3aN ), the recursive Newton-Euler algorithm (RNEA), described 1 NB B B i hereafter. Equations (2) and (3ai) are propagated from 1 c , c , . . . , c , c . (11) (3fNB ) (3τNB ) (3f1) (3τ1) to NB with initial conditions v0 = 0 and a0 = −ag which corresponds to the gravitational spatial acceleration Property 1: The permuted matrix Dpid,q induced by the vector expressed in the body frame 0 (null in its first three permutations defined in (10) and (11) is lower triangular. components and equal to the gravitational acceleration in the Proof: Since Dpid,q is defined by blocks, we structure last three). Equations (3fi) and (3τi) are propagated from NB this proof by considering the blocks that constitute the matrix to 1. itself. First, we prove that the blocks in the main diagonal are diagonal matrices. These blocks are:

B. Inverse dynamics solved with matrix inversion x Dc x ,f = 16,Dc(7q¨ ),q¨i = 1ni ,Dc(3a ),ai = −16, (7fi ) i i i In this Section, we propose a way to solve the inverse Dc ,f = −16,Dc ,τ = −1n , dynamics problem with a matrix inversion. Remarkably, (5) (3fi) i (3τi) i i represents the set of linear constraints in dq. Additionally, which are indeed diagonal. We are left with proving that the x blocks in the upper triangular part of Dp ,q are identically certain components of dp are known since q¨1, ... , q¨NB , f1 , id ... , f x are given: null. We consider the non-null blocks in (4) and prove that NB each block Dc,d is positioned in the lower triangular part of

qï = yqï , (7qï) Dpid,q. This is equivalent to prove that if Dc,d 6= 0 then x x f = y x . (7f ) ic ≥ id, being ic and id the position of c and d in the 1 fi i permutations pid and q, respectively. In the definitions given These constraints, will extend the set of constraints C. In in (4), we can neglect the diagonal blocks, which have been N particular, we should define C = C ∪ {c , c x } B so previously considered. We are left with: id (7qï) (7fi ) i=1 that |Cid| = 5NB. To extend the definition of Dc,d and bc Dc ,a , ic = 2NB + i, ia = 2NB + λi we define: (3ai) λi (3ai) λi D , i = 2N + i, i = 2i c(3ai),qï c(3ai) B qï D = 1 , b = −y , (8a) c(7qï),qï ni c(7qï) qï D , i = 5N − 2i + 1, i = 2N + i c(3fi),ai c(3fi) B ai B

x x Dc(3f ),f , ic(3f ) = 5NB − 2i + 1, if = 2i − 1 D x x = 1 , b x = −y x , (8b) i i i i c(7f ),fi 6 c(7f ) fi i i D , i = 5N − 2i + 1, i = 5N − 2j + 1 c(3fi),fj c(3fi) B fj B and Dc,d = 0, bc = 0 otherwise. Again, given two permuta- D , i = 5N − 2i + 2, i = 5N − 2i + 1. c(3τi),fi c(3τi) B fi B tions pid and q of the elements in Cid and D, the permuted Easy computations can show that in these case i ≥ i matrix Dpid,q and permuted vectors dq, bpid satisfy the c d following linear equation: considering a numbering scheme with λi < i and j > i, ∀j ∈ µi as described in [1]. D (q)d + b (q, q˙) = 0, (9) pid,q q pid D. Inverse dynamics solved with LU factorization which is representation of the inverse dynamics problem. In In Section IV-C, suitable permutations led to a lower particular, inverting the matrix Dpid,q we can compute the triangular structure for the matrix D in (8). After obtaining solution dq of the inverse dynamics. A more computation- the lower triangular structure, the underlying linear system ally efficient solution can be obtained as described in the can be solved with a forward substitution which is a compu- following section. tationally efficient algorithm to solve a linear system. In this section we are interested in computing these permutations C. Inverse dynamics solved with forward substitution by solving the following problem. In this section we prove that with suitable permutations Problem 1: Given arbitrary permutations p and q of the elements in Cid and D, compute permutation matrices P and we can compute dq which solves (9) with a computationally efficient algorithm, which is the forward substitution as Q to obtain a triangular matrix P Dp,qQ. defined in [4]. The idea is to build two find permutations In consideration of Property 1, Problem 1 has necessarily a solution. In other terms, Dp,q is triangularizable with pid and q of the elements in Cid and D which lead to a lower triangular matrix D and then to solve (9) with a permutations. pid,q Definition 1 (triangularizable with permutations): forward substitution. The permutations are inspired by the A A RNEA described in Section IV-A. Let’s first consider the q square matrix is triangularizable with permutations if permutation of the elements in D. We choose: there exists a permutation of the rows and a permutation of the columns which result in a triangular matrix. x x To solve Problem 1, we resort to a classical problem in q = f , q¨1, . . . , f , q¨N , a1, . . . , aN , 1 NB B B matrix analysis: the sparse LU factorization with minimum f , τ , . . . , f , τ ] . NB NB 1 1 (10) filling-in [4, Section 11.1.9]. i Problem 2: Given a square matrix A find the permutation Xλi which depend on q. These sub-matrices do have a state- matrices P and Q, a lower triangular matrix L and an upper dependent sparsity structure but for the purpose of this paper, triangular matrix U such that P AQ = LU and the number we can consider the associated worst-case sparsity structure, of filling-in (new nonzeros in L and U that are not present i.e. if an element is non-zero for at least one value of q, then it in A) is minimum. is considered as a non-zero element in the associated sparsity Remark 2: If A is triangularizable with permutations, i.e. pattern. For classical joint types (revolute, prismatic, helical, A = P¯L¯Q¯ with L triangular, then the solution to problem cylindrical, planar, spherical and free-motion) the worst-case 2 is given by P = P¯−1, Q = Q¯−1, L = L¯ and U equal sparsity pattern can be easily computed by observing that to the identity matrix. In this case in fact, we can obtain the only q dependent elements are either sines of cosines zero filling-in which by definition is the minimum number (see [1, Table 4.1, page 79]). These functions are zero only achievable. Under this considerations, we can apply Problem on a countable number of configurations (and therefore on a 2 to solve Problem 1 subset whose Lebesgue measure is zero) which are easy to Remark 3: Deciding if a matrix is triangularizable accord- enumerate. ing to definition 1 is NP-complete [5]. Similarly, solving problem 2 is also NP-complete [6]3. Available numeri- V. FORWARD DYNAMICS cal tools for finding their solution are not guaranteed to The inverse dynamics problem consists in finding q¨1, ... , reach the minimum. In solving Problem 2 we will use the q¨ which satisfies (3) given τ , ... , τ , f x, ... , f x . NB 1 NB 1 NB unsymmetric-pattern multiFrontal method, as implemented In [1] the problem is formulated as the computation of the in UMFPACK [9]. following function: Going back to Problem 1, we can take advantage of q¨ = FwdD(model, q, q˙,τ,f x, . . . , f x ). (12) Property 1 to guarantee that Dp,q is always triangularizable. 1 NB Therefore, remark 2 applied to Dp,q guarantees that solving Again, in the above equations we grayed out some variables Problem 2 will give us also a solution to Problem 1. Given that will not play a role in the following sections, and can the NP-completeness of the underlying problem, we are not be assumed either to be contestant (model) or measured (q, 4 guaranteed to find a solution but numerical experiments q˙). conducted so far with UMFPACK shows that a solution is always found if NB ≤ 100. As to this concern, one- A. Forward dynamics solved with the ABA hundred can be considered a a practical upper-bound for The articulated-body algorithm [1, ABA] solves the for- robotic applications. ward dynamics problem in O(NB) computational complex- Remark 4: Even though Problem 2 is NP-complete, in ity. The algorithm consists in the following steps. First, the practice the problem is solved once in a preliminary op- A articulated body bias forces pi and the articulated body timization phase and its benefits can be exploited in the A inertias Ii are recursively computed iterating with i = NB, runtime computations. The preliminary optimization consists ... , 1 the following equations: in solving Problem 2 for a worst-case sparsity pattern.

The runtime computations instead consist in solving (9) for A x X i ∗n A a pi = νi − fi + Xj pj + Ij cj + different positions q and velocities q˙ exploiting the optimized j∈µ i (12pi) −1 permutations computed previously. A > A > A o A solution of Problem 2 can be used to compute permu- + Ij Sj Sj Ij Sj τj − Sj pj , −1 tations that allow to solve (9) efficiently. The solution uses a A A > A > A a Ij = Ij − Ij Sj Sj Ij Sj Sj Ij (12Ii ) the sparsity pattern of Dp,q(q), i.e. the pattern of non-zero X IA = I + iX∗Ia j X (12IA) elements in the matrix. However, in practical applications we i i j j i i j∈µi are interested in a sparsity pattern which somehow represents Then the following two equations are iterated with i = 1, the sparsity of Dp,q(q) for all possible values of q. As to this concern, we define the worst case sparsity pattern. ... , NB and initial condition a0 = −ag. The basic observation is that the robot structure (e.g. −1 > A n > h A i Aio number of degrees of freedom, joint types, joint positions, qï = Si Ii Si τi − Si Ii Xλi aλi + ci + pi (13qï) tree structure of the robot) does not change. As a conse- i ai = Xλ aλ + Siqï + ci. quence, the underlying sparsity structure of (9), i.e. the non- i i zero elements in D, changes only for the state-dependent B. Forward dynamics solved with matrix inversion q q˙ elements, i.e. those that depend on and . Looking at (4), In solving the inverse dynamics, the fact that Dp ,q i ∗ id the only state-dependent blocks are the transformations Xj , is lower triangular follows from the specific structure of the measurement equations (7). Changing the measurement 3Even though [6] is cited several times [7], [8] as a proof of the NP- completeness of Problem 2, it has to be observed that it is not clear to the equations would compromise the lower triangularity of authors of the present paper how to extend the results in [6] to the case of Dpid,q. As a consequence, the associated linear system non-symmetric positive definite matrices would not have the suitable structure to apply the forward 4Experiments are avaialble here https://github.com/ iron76/bnt_time_varying/tree/master/experiments/ substitution. Within this context, forward dynamics give a computationalComplexity/RNEA. useful example. The measured variables for the forward x 104 dynamic case are fi and τi. The latter in matrix notation 2.5 can be expressed as follows: 2

τi = yτi , (14τi) 1.5 These constraints, will extend the set of constraints C. In N particular, we should define C = C∪{c , c x } B and fd (14τi) (7fi ) i=1 1 extend the definition of Dc,d and bc as follows: D = 1 , b = −y , 0.5 c(14τi),τi ni c(14τi) τi and Dc,d = 0, bc = 0 otherwise. Given two permutations 0 0 10 20 30 pfd and q of the elements in Cfd and D, a solution of the forward dynamics can be computed as the unique solution dq of the following liner system: Fig. 1. Comparison of the different proposed algorithms for solving the forward dynamics of a serial chain with NB links. Dpfd,q(q)dq + bpfd (q, q˙) = 0, (15) C. Forward dynamics solved with forward substitution Similarly to what observed in the inverse dynamic case, algorithms proposed in Section V-A (ABA) and Section V- we might try to find permutations for p of the elements in C (matrix reformulation of the ABA). The computational fd cost of ABA is the one reported in [1, Page 202] and Cfd and q of the elements in D to obtain a matrix Dpfd,q somehow simple to invert. Specifically, we might think that corresponds to the algorithmic solution proposed in [10]. The other computational costs are numerically computed with the articulated-body algorithm [1, the ABA] presented in 5 Section (V-A), could be translated into suitable permutations software which is available open source . that transform Dp ,q into a lower triangular matrix. Unfor- fd VI.DYNAMIC EQUATIONS AND LU-FACTORIZATION tunately, the ABA algorithm is instead something more than a permutation as discussed in the following. In the previous sections, we have seen how the RNEA Property 2: There exist: and the ABA are computationally efficient solutions of the - a permutation pfd of the elements in Cfd; inverse and forward dynamics problems. The reduction of - a permutation q of the elements in D; the computational costs is obtained by suitable permutations R W q , q ∈ D of the matrices {Dc,d}c∈C ,d∈D and {Dc,d}c∈C ,d∈D. In - matrices q1,q2 defined for 1 2 ; id fd W L p , p ∈ C this section we consider the problem of finding similar - matrices p1,p2 defined for 1 2 fd; such that: permutations for a wider class of problems. Inspired by L R the matrix representations (9) and (15) of the inverse and Wp ,p Dpfd,qWq,q fd fd forward dynamics respectively, we consider a generic esti- is lower triangular. These quantities lead to the following mation problem consisting in computing the solution d of matrix reformulation of the ABA algorithm: the following linear system: L R W Dp ,qW dq + bp (q, q˙) = 0. pfd,pfd fd q,q fd D (q) b (q, q˙) The proof of the preposition above is constructive and can p,q d + p = 0, (16) D (q) b (q, q˙) be found in Appendix B. Y Y | {z } | {z } D. Forward dynamics solved with LU factorization ,D(q) ,b(q,q˙) Again, part of the computational optimizations of the obtained by combining (5) with a generic measurement equa- ABA proposed in Section V-A and revisited in Section V- tion DY (q)d+bY (q, q˙) = 0. In Section IV-C and Section V- C consists in row and column permutations. In this section C respectively, suitable permutations led to a lower triangular we will suggest an algorithm for simplifying the forward structure for the matrix D. The underlying linear system dynamics computations by leveraging the intrinsic sparsity could then be solved with a forward substitution. However, of the underlying matrices (15) and pre-computing suitable the proposed solutions rely on the specific structure of the row and column permutations that will reduce the underlying problem and therefore extending them to solve (16) is non computational cost. The idea consists in computing the trivial. The algorithm proposed in Section IV-D and Section V-D seems more suitable to simplify the computational worse case sparsity pattern for Dpfd,q(q) in (15). Solving Problem 2 with this sparsity pattern will give the row-column complexity of solving (16) in its generic form (i.e. beyond permutations P and Q suitable for solving (15). At run-time the inverse and forward dynamics cases). regardless of the specific q the idea is to perform the sparse LU factorization on P D (q)Q followed by a backward 5The software for these computations are available here: pfd,q https://github.com/iron76/bnt_time_varying/tree/ substitution on U and a forward substitution on L. Figure master/experiments/computationalComplexity/ABA/ 1 shows the computational cost of this solution against the serial/compare. As an example, we consider a relevant case study: the inverse dynamic computation of a humanoid robot standing 0 NB "0 ># τ J > X J on two feet. The specificity of this problem is that the robot M(q)¨q + h(q, ˙ q) = + 1 f x + j f x. (20) 0 1 1 0X∗ j has two external wrenches applied at the feet. Differently 6 j=2 j from the inverse dynamic computations presented in Section Using (18) in (20) and grouping the unobservable/observable IV-A, we assume that these external wrenches are unknown. quantities as in problem (19), we obtain: Using the notation used in previous sections, these external wrenches can be denoted f x and f x by labelling with 1 1 NB x x and NB the right and the left foot respectively. We therefore Y (q, q,˙ q,¨ f , . . . , f , y , y ) = 2 NB −1 lf rf consider the following problem: " # 0 > 0 > τ J Hh J Hhrf = + 1 lf + NB . (21) 0 Hh 0X∗ Hh lf NB rf τ = InvD(model, q, q,˙ q,¨ f x, . . . , f x ), (17) 2 NB −1 Understanding if the inverse dynamic problem with these measurements is well-posed boils down to understanding if where we explicitly indicated that f x and f x are unknown 1 NB the following matrix is invertible: even if they act on the system. The considered mechani- cal system is a free-floating articulated rigid body subject " 0 > 0 > # to constraints. Unfortunately solving (17) is ill-posed, i.e. 1n J H J H 1 NB . (22) given q, q,˙ q,¨ f x, . . . , f x there exist multiple τ, f x, f x 0 H 0X∗ H 2 NB −1 1 NB 6×6 NB satisfying (3). To estimate τ we could try using additional Given the upper triangular form of this matrix, we are measurements. We hereafter consider quite a common sensor 0 ∗ 6 left with proving the invertibility of [ H, XN H ]. We are distribution, nominally four load cells located on each foot . B These sensors are available in the NAO robot [11], in the interested in guaranteeing the invertibility of this matrix for any 0X∗ , i.e. regardless of the feet relative pose. QRIO robot [12] and the Atlas [13]. These sensors corre- NB > H> H> 3×3 spond to a three-axes force-torque sensor, i.e. they measure Property 3: Let H = [ f µ ] with Hf ∈ R , H ∈ 3×3. If H is singular, then there always exists 0X∗ the net force orthogonal to the plane of the each foot and the µ R f NB H, 0X∗ H projection of the torque on the same plane. As it is often the which makes [ NB ] singular. Proof: Using the structure of 0X∗ and H we obtain: case, if we choose the left and right feet reference frames NB with the x and y axis aligned to the foot plane we have: 0 ∗ Hf RHf H XN H = B Hµ p × RHf + RHµ x x f1 = Y ylf + Hhlf , fN = Y yrf + Hhrf , (18a) B R=13 Hf 03×3 13 13 0 0 0 1 0 0 = , Hµ p × Hf 03×3 13 0 0 0 0 1 0 0 0 1 0 0 0 Y   ,H   , (18b) and the results follows by observing that the multiplicand , 1 0 0 , 0 0 0     matrix on the left is singular (the first three lines are linearly 0 1 0 0 0 0 0 0 0 0 0 1 dependent) and by using the fact that the rank of a product is always less or equal the rank of the multiplied matrices. 3 with ylf , yrf ∈ R being the measured three-axes force- 3 torque and hlf , hrf ∈ R being the non-directly measured Remark 5: Using Property 3 with definitions (18b) we can x components of the contact wrench f1 . We are now consid- conclude that (19) is an ill-posed problem. In other terms, ering the following problem: the inverse dynamics problem cannot be solved in the case of a humanoid robot standing on the two feet with four load cells on each foot. τ = (model, q, q,˙ q,¨ f x, . . . , f x , y , y ). InvD 2 NB −1 lf rf (19) Additional assumptions are needed to compute the inverse dynamics. A first realistic assumption in certain applications To understand if this problem is well-posed, it is convenient is to assume that the robot is standing in a very slippery to resort to a specific form of equation (3). This formulation surface and therefore the tangential forces along the x and is detailed in [14, eq. (41)] and it combines the floating- y axis are negligible. In this case: base dynamics with the joint dynamics. We consider here a simplified version obtained by choosing the left foot as the f x = Y y + Hh , f x = Y y + Hh , base frame and choosing q a local parametrization of the 1 lf lf NB rf rf (23a) robot pose (composed by the free-floating six-dimensional 1 0 0 0 0 0 configuration and the joint n-dimensional configuration). We 0 1 0 0 0 0     have: 0 0 1 0 0 0 Y   ,H   , (23b) , 0 0 0 1 0 , 0 6     We first prove that these sensors are not yet sufficient to solve the inverse 0 0 0 0 1 0 dynamics. Then we propose a different set of sensors which results in a well posed inverse dynamics problem. 0 0 0 0 0 1 > A j A τj = Sj Ij Xλj aλj + Sjq¨j + cj + pj . (25)

Step 3 - S3. Multiply the previous equation by the inverse 10000 > A of Sj Ij Sj to obtain (13q¨i) for q¨j:

−1 > A n > h A j Aio q¨j = Sj Ij Sj τj − Sj Ij Xλj aλj + cj + pj 5000 −1 j∈µi > A n > h A j Aio = Sj Ij Sj τj − Sj Ij Xiai + cj + pj ,

Step 4 - S4. Substitute the last equation in (24ai): 0 8 10 12 14 16 18 20 i > A −1 ai = Xλi aλi + ci + Si Si Ii Si > A i A Fig. 2. Comparison of the different proposed algorithms for solving the τi − Si Ii Xλi aλi + ci + pi . inverse dynamics (i.e. joint torque estimation) of a serial chain with NB links. Considered measurements are load-cells at extremal links. which evaluated with the substitution i → j ∈ µi leads to:

y , y ∈ 3 j > A −1 with lf rf R being the measured contact forces and the aj = Xiai + cj + Sj Sj Ij Sj measured torques on the x-y plane and h , h ∈ being lf rf R τ − S> IA jX a + c + pA . (26) the torques on the z axis. In this case it was numerically j j j i i j j 7 observed that the associated inverse dynamics problem is Step 5 - S5. Substitute aj in (24fi) with its expression in solvable. Fig. 2 shows a comparison of the number of (26): floating point operations necessary to solve this specific inverse dynamics problem with or without a sparse LU i ∗ A j x factorization. Remarkably this case cannot be solved with fi = Ii + Xj Ij Xi ai + νi − fi + classical algorithms (e.g. hybrid dynamics [1]). X i ∗ A > A −1 + Xj Ij Sj Sj Ij Sj τj APPENDIX j∈µi i ∗ h A > A −1 >i A A. The articulated body equation of motion + Xj 1 − Ij Sj Sj Ij Sj Sj pj The idea consists in recursively computing (i = NB, ... , hi ∗ A > A −1 > A j i A A − Xj Ij Sj Sj Ij Sj Sj Ij Xi ai 1) the quantities pi (articulated body bias forces) and Ii (articulated body inertias) which satisfy the articulated body i ∗ h A A > A −1 > Ai + Xj Ij − Ij Sj Sj Ij Sj Sj Ij cj. (27) equation of motion: A A and enforcing fi = Ii ai + pi leads to the definitions in a A A A ab (12pi), (12Ii ) and (12Ii ). fi = Ii ai + pi . (23fi ) B. Proof of Property 2 For defining these quantities, let’s start by considering i such x We hereafter assume that the articulated body inertias that µi = ∅. In this case (3fi) gives fi = Iiai + νi − fi and A A x IA (i = 1, ... , N ) have been computed. The idea is to therefore Ii = Ii and pi = νi − fi . Recursively, let’s i B A A assume that I and p have been defined for every j ∈ µi follow the steps presented in Section A to compute the j j L R (this time non-empty) and let’s find suitable expressions for sub-blocks of the matrices W and W . The latter is A A defined as a matrix equivalent of Step 1. The former as Ii and pi . This is achieved by a five step procedure. ab W L = W L,4W L,3W L,2W L,1 with W L,1 representing Step Step 1 - S1. Replace (23fi ) in (3): 2, W L,2 representing Step 3, W L,3 representing Step 4 and W L,4 representing Step 5. i a = X a + S q¨ + c , (24a ) R i λi λi i i i i Step 1 - S1. We compute a matrix W which multiplied by > A A A A R τi = Si Ii ai + pi (24τi) dq replaces fi with pi = fi −Ii ai. As usual we define W W R q , q ∈ D x X i ∗ A A by its blocks q1,q2 with 1 2 . We have: fi = Iiai + νi − fi + Xj Ij aj + pj . (24fi) j∈µi W R = IA,W R = 1 ∀q ∈ D, (28) The latter is almost the equation we need for the recursive fi,ai i q,q A A definition of Ii and pi if only we could write aj as a W R = 0 and q1,q2 otherwise. function of ai. This is achieved with the following steps. Remark 6: Given two permutations p and q of the ele- Step 2 - S2. Substitute (24ai) in (24τi): R ments in C and D, if Dp,q represents (3), Dp,qWq,q represents NB 7https://github.com/iron76/bnt_time_varying/tree/ (24). Therefore, in the following {c(3ai), c(3τi), c(3fi), }i=1 can master/experiments/computationalComplexity/SIE NB be read as {c(24ai), c(24τi), c(24fi)}i=1. L,1 R Step 2 - S2. We define W which left multiplies Dp,qWq,q REFERENCES a τ W L,1 = 0 to substitute (24 i) in (24 i). The exceptions to p1,p2 are: [1] R. Featherstone, Rigid Body Dynamics Algorithms. Springer, 2008. [2] C. Balafoutis and R. Patel, Dynamic Analysis of Robot Manipulators: A Cartesian Tensor Approach, ser. The Springer International Series L,1 > A L,1 in Engineering and Computer Science. Springer US, 1991. [Online]. W = S I ,W = 1 ∀p ∈ Cfd, (29) c(3τi),c(3ai) i i p,p Available: https://books.google.it/books?id=7BcpyUjmLpUC L,2 [3] D. Orin and M. Walker, “Efficient dynamic computer simulation of Step 3 - S3. We define W which left multiplies robotic mechanisms,” ASME Journal of Dynamic Systems, Measure- L,1 R > A Wp,p Dp,qWq,q to multiply (25) by the inverse of Sj Ij Sj. ment and Control, 1982. The exceptions to W L,2 = 0 are: [4] G. Golub and C. Van Loan, “Matrix computations 4th ed,” 2013. p1,p2 [5] G. Fertin, I. Rusu, and S. Vialette, “Obtaining a Triangular Matrix by Independent Row-Column Permutations,” in 26th L,2 > A −1 L,2 International Symposium on Algorithms and Computation, Nagoya, W = S I Sj ,W = 1 ∀p ∈ Cfd, q 6= τi c(3τi),c(3τi) j j p,p France, Dec. 2015. [Online]. Available: https://hal.archives-ouvertes. (30) fr/hal-01189621 [6] M. Yannakakis, “Computing the minimum fill-in is np-complete,” L,3 Step 4 - S4. We define W which left multiplies SIAM Journal on Algebraic Discrete Methods, vol. 2, no. 1, pp. 77–79, W L,2W L,1D W R to replace the occurrences of q¨ in 1981. p,p p,p p,q q,q j [7] J. Dongarra, V. Eijkhout, and P. Luszczek, “Recursive approach in (24ai) with their expression obtained in the previous step. sparse matrix lu factorization,” Sci. Program., vol. 9, no. 1, pp. 51–60, L,3 The exceptions to Wp ,p = 0 are: Jan. 2001. [Online]. Available: http://dx.doi.org/10.1155/2001/569670 1 2 [8] L. Grigori, E. G. Boman, S. Donfack, and T. A. Davis, “Hypergraph-based unsymmetric nested dissection ordering for sparse L,3 L,3 lu factorization,” SIAM J. Sci. Comput., vol. 32, no. 6, pp. 3426–3446, W = −Si,W = 1 ∀p ∈ Cfd. (31) c(3ai),c(3τi) p,p Nov. 2010. [Online]. Available: http://dx.doi.org/10.1137/080720395 L,4 [9] T. A. Davis, “Algorithm 832: Umfpack v4.3—an unsymmetric-pattern Step 5 - S5. We define W which left multiplies multifrontal method,” ACM Trans. Math. Softw., vol. 30, no. 2, pp. L,3 L,2 L,1 R Wp,p Wp,p Wp,p Dp,qWq,q to replace the occurrences of aj 196–199, June 2004. [Online]. Available: http://doi.acm.org/10.1145/ 992200.992206 in (24fi) with their expression obtained in the previous step. [10] H. Brandl, R. Johanni, and M. Otter, “A very efficient algorithm for the W L,4 = 0 The exceptions to p1,p2 are: simulation of robots and similar multibody systems without inversion of the mass matrix.” in IFAC/IFIP/IMACS Symposium on Theory of Robots, 1986, pp. 95–100. i W L,4 = X∗IA,W L,4 = 1 ∀p ∈ C . (32) [11] D. Gouaillier, V. Hugel, P. Blazevic, C. Kilner, J. Monceaux, P. Lafour- c(3f ),c(3a ) j j p,p fd i j cade, B. Marnier, J. Serre, and B. Maisonnier, “Mechatronic design of We are left with the definition of the permutations. Let’s nao humanoid,” in 2009 IEEE International Conference on Robotics first consider the q permutation of the elements in D. We and Automation, May 2009, pp. 769–774. [12] T. Ishida, “Development of a small biped entertainment robot qrio,” in choose: Micro-Nanomechatronics and Human Science, 2004 and The Fourth Symposium Micro-Nanomechatronics for Information-Based Society, x x 2004., Oct 2004, pp. 23–28. q = f1 , τ1, . . . , fN , τNB , fNB , . . . , f1, B [13] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, a1, . . . , aNB , q¨1,..., q¨NB ] . (33) F. Permenter, T. Koolen, P. Marion, and R. Tedrake, “Optimization- based locomotion planning, estimation, and control design for Let’s also choose a permutation pfd of the elements in Cfd the atlas humanoid robot,” Autonomous Robots, vol. 40, no. 3, : pp. 429–455, 2016. [Online]. Available: http://dx.doi.org/10.1007/ s10514-015-9479-3 h [14] S. Traversaro, D. Pucci, and F. Nori, “A unified view of the equations x x pfd = c(7f ), c(3τN ) . . . c(7f ), c(3τ1), c(3fN ) . . . c(3f1), 1 B NB B of motion used for control design of humanoid robots,” Submitted i to Multibody System Dynamics - Springer, 2017. [Online]. Available: c , . . . , c , c , . . . , c . (34) https://traversaro.github.io/preprints/changebase.pdf (3a1) (3aNB ) (14¨q1) (14¨qNB )