Symbolic Lagrangian Multibody Dynamics

Brian Guenter∗ Sung-Hee Lee† Microsoft Research Honda Research Institute

Abstract in the Euler-Lagrange equations2 have prevented wide accep- tance of the approach [Herbert Goldstein 2001]. To over- Symbolic Lagrangian formulations of the equations of mo- come this difficulty, researchers have manually derived ef- tion of tree structured constrained mechanical systems have ficient differentiation methods of the Euler-Lagrange equa- the potential to be both more efficient and more numerically tions (see [Featherstone and Orin 2000] for an overview), but robust than formulations which use nonlinear kinematic con- most algorithms assume a small set of simple constraint types straint equations. We derive a simple recursive factorization such as revolute or prismatic joints and thus, not surprisingly, of the Lagrangian equations of motion which, along with our most of the current dynamics packages support only simple extended implementation of the D* symbolic differentiation types of constraints.3Notably, an improved version of the Ar- algorithm, yields empirically measured O(n) inverse dynam- ticulated Body Algorithm [Featherstone 1987] supports gen- ics and O(n3) forward dynamics. Complex kinematic con- eral scleronomic constraints, but the complexity of the algo- straints such as point on surface, point on curve, and surface rithm makes it difficult to understand and implement [Baraff on surface are easily handled by the new algorithm. Numer- 1996]. If arbitrary parametric kinematic constraints are al- ous examples demonstrate the wide range of mechanical sys- lowed then computing the derivatives is still problematic. tems the algorithm can be applied to. Computer graphics researchers have used symbolic or automatic differentiation to compute derivatives of complex func- CR Categories: I.3.7 [COMPUTER GRAPHICS ]: Three- tions [Kass 1992; Cohen 1992; Popovic´ et al. 2000; Grin- Dimensional Graphics and Realism—Animation spun et al. 2003]. In this paper, we create symbolic equations of motion by using symbolic differentiation. Specifi- 1 Introduction cally, we derive a simple recursive factorization of the La- grangian equations of motion which, along with our extended Lagrangian mechanics is a reformulation of classical me- implementation of the D* symbolic differentiation algorithm [Guenter 2007], yields empirically measured O(n) inverse chanics which is particularly useful for modeling constrained 3 mechanical systems. For tree structured systems, with holo- dynamics and O(n ) forward dynamics. (Note to review- nomic constraints that can be expressed as differentiable ers: D* symbolic differentiation software will be available to parametric functions of the generalized coordinates1, it is al- public in late 2009.) ways possible to find a set of symbolic differential equations In mechanics, symbolic or automatic differentiation have for the Lagrangian equations of motion. By contrast, non- been employed mostly to augment multibody dynamics such Lagrangian formulations require differential equations and as computing the derivatives of constraint equations or the nonlinear algebraic equations (abbreviated DAE) [Ascher mass matrix [Shi and McPhee 2000; Olsson et al. 2005; and Petzold 1998] to enforce kinematic constraints. Solv- Samin and Fisette 2003]. Not only is symbolic differenti- ing DAE systems is considerably more difficult than solving ation used for dynamics simulation, but it is also used for differential equations alone. linearizing the equations of motion and optimizing design parameters. However, for the core simulation of the dynam- However, the difficulty of computing the complex derivatives ics, they employed existing, manually derived dynamics algorithms that do not require symbolic differentiation. ∗email:[email protected] † email:[email protected] In contrast, we derive symbolic equations of motion by using symbolic differentiation. [Villard and Arnaldi 1996] applied symbolic differentiation to compute the equations for the constraint forces, which are numerically calculated using either the penalty method or the Lagrange mutltiplier tech-

2For tree structured systems naïve direct approaches for evaluating these derivatives can have computational complexity as high as O(n4), where n is the number of degrees of freedom of the system. 3SD/FAST [Hollars et al. 1994], a multibody dynamics package based on Lagrangian formulation, supports complex, user-defined constraints only by 1This may sound restrictive but a wide variety of mechanisms can be using the Lagrange multipler technique, which is not related to our symbolic modeled this way, as we will show in Section 5. Lagrangian approach. nique. By contrast, our algorithm generates minimal coordinate motion equations that eliminate all constraint forces. p [Turner 2003] applied their automatic differentiation algo- w tcm rithm to Lagrange’s equation to generate motion equations r rigid body frame rcm for very simple mechanisms, but they did not generalize their t method to handle a broad scope of mechanisms. W One advantage of using symbolic differentiation for dynamics simulation is that one can easily incorporate complex world frame constraints. In general, one needs to compute derivatives of the constraint equations up to the second order. Com- Figure 1: rcm and tcm is the center of mass with respect to plex constraints have very complex derivatives, which are the world and body frame, respectively. rw and p refer to difficult to derive. By employing symbolic differentiation, a point of a rigid body with respect to the world and body one need only provide the constraint equations; the deriva- frame, respectively. tives are computed by the symbolic differentation. Section 5 demonstrates that some constraints whose derivatives are very complex to derive manually can be easily simulated by 2 Lagrangian Equations of Motion our method. Some of our example mechanical systems are similar to those presented in [Kry and Pai 2003] and [Lee We start by deriving the Langrangian equations of mo- and Terzopoulos 2008] but the dynamics algorithms to cre- tion of a multibody system. We assume a tree structured ate the examples are very different in that the former manu- multibody system in which n generalized coordinates, q = ally derived derivatives of the general surface constraints and (q0,...,qn−1), completely specify the configuration of every the latter employed easily differentiable constraint functions, rigid body in the system. Each local transformation for rigid both under the framework of existing dynamics algorithms. body i, Ai, with respect to its parent is determined by some number of generalized coordinates qi, and its transformation W in world coordinates is expressed as the product of local 1.1 Contributions i transformations of its ancestors, or in recursive form, as: To address the problem of applying symbolic Lagrangian me- R(qi) t(qi) Wi(q) = = Wp (q)Ai(qi) (1) chanics to tree structured systems we present in this paper a 0 1 i simple and efficient factorization of the symbolic Lagrangian where p denotes i’s parent. equations of motion which directly leads to an O(n) algo- i rithm for inverse dynamics. For the forward dynamics prob- If the inertia matrix is defined in the center of mass coordi- lem we have extended our implementation of the D∗symbolic nate frame then the Newton-Euler equations of motion for a differentiation algorithm [Guenter 2007] to give an O(n3) al- system of n rigid bodies are: gorithm. The new algorithm has several attractive properties: n n ∑ mir¨cmi = ∑ fi (2) i=1 i=1 ∙ Because the symbolic Lagrangian equations always n n ˙ maintain kinematic constraints it is possible, in some ∑ hcmi = ∑ τi (3) cases, to use very large time steps. These large time i=1 i=1

steps would cause convergence problems for nonlinear where mi is the mass of a rigid body i and rcmi = Witcmi is its constraint solvers. center of mass (Fig. 1). hcmi = Iiωi is the angular momentum about the center of mass where Ii is the rotational inertia and ^T ˙ 4 ∙ Mechanisms incorporating complex constraints which ωi = Ri Ri . The force fi and torque τi include external and are challenging to simulate using existing techniques, internal forces and torques, respectively; i.e., fi = fexti + finti such as point on surface, point on curve, surface on sur- and τi = τexti + τinti . By projecting the Newtonian motion face, are easily and robustly simulated. equations onto the kinematic constraint manifold we obtain the Lagrangian equations of motion, which completely elim- ∂r inate constraint forces ; i.e., we project (2) onto cmi . Con- The remainder of this paper is organized as follows: after de- ∂q j riving the Lagrangian equations of motion from the Newton- straint forces have zero projection on this vector so they all Euler dynamics equation (Section 2), we present a simple re- 4The ~ operator converts between vector and a skew-symmetric matrix cursive factorization of the Lagrangian equation (Section 3). representation; i.e., an angular velocity ω relates to the time derivative of the Then we show inverse and forward dynamics methods (Sec- rotation matrix R as follows: ⎡ ⎤ tion 4) followed by numerous examples (Section 5). Finally, 0 −ωz ωy ˜ = − , ]T ˙ = . we report the performance of our methods (Section 6) and ω ⎣ ωz 0 ωx ⎦ R R ω −ωy ωx 0 conclude the paper (Section 7). vanish per d’Alembert’s principle of virtual work: To simplify these equations start with two observations. First, because5 n n ∂Ai ∂rcm ∂rcm = 0 i ∕= j (7) (m r¨ − f )T i = fT i ∑ i cmi exti ∑ inti ∂q j i=1 ∂q j i=1 ∂q j n we can rewrite ∂Wi as T ∂rcmi ∂q j ∑ ai = Qf j (4) i=1 ∂q j ∂Wi ∂A j = W j−1 A j+1 ...Ai (8) ∂q j ∂q j where ai = mir¨cmi −fexti and Qf j is the generalized force, due to forces, associated with generalized coordinate q j. Like- Second ∂W wise, we project the torques onto the constraint manifold: j = 0 for k ≥ j ∂qk n n T where the expression k ≥ j means that the rigid body k is h˙ − τ ω = τT ω ∑ cmi exti q j ∑ inti q j further toward the leaves of the tree than the rigid body j. i=1 i=1 n Apply these two observations to our equations of motion to T get an upper triangular set of equations ∑ bi ωq j = Qτ j (5) i=1 T ∂A0 T ∂A0 T ∂A0 a0 tcm0 + a1 A1tcm1 + a2 A1A2tcm2 = Qf0 ∂q0 ∂q0 ∂q0 ^T ∂Ri where ωq = R , bi = h˙ cm −τext , and Qτ is the gener- j i ∂q j i i j T ∂A1 T ∂A1 a1 W0 tcm1 + a2 W0 A2tcm2 = Qf1 alized force due to torques. We get a single set of differential ∂q1 ∂q1 equations by adding the two generalized forces. T ∂A2 a2 W1 tcm2 = Qf2 ∂q2 n ∂r aT cmi + bT ω = Q (6) ∑ i q i q j j Notice that ∂Ai is included in every term in the LHS and i=1 ∂ j ∂qi T we can factor out theses common terms by moving the ai terms from the left to the right hand side. The products where Q j = Qf j + Qτ j . We have derived Lagrangain equa- T ∂Ai tions of motion (Eq. (6)) from Newton-Euler motion equa- a Wi−1 tcm are of the form i ∂q j i tions (Eqs. (2) and (3)) for a multibody system. However, this is not yet a computationally efficient form of the equa- T T T ∂Ai ∗ x y where x = ai and y = Wi−1 tcmi . tions; the inverse dynamics equations generated by the D ∂q j algorithm using (6) is O(n2), n times more than today’s manually optimized O(n) inverse dynamics algorithms. In Sec- Using the identity tion 3, we develop a novel factorization that yields an O(n) tr yxT = xT y, (9) inverse dynamics algorithms. our three equations of motion become 3 Factoring the Equations of Motion ∂A0 T ∂A0 T ∂A0 T tr tcm0 a0 + A1tcm1 a1 + A1A2tcm2 a2 = Qf0 ∂q0 ∂q0 ∂q0 The factorization of the motion equations is most easily ap- ∂A1 T ∂A1 T tr W0 tcm1 a1 + W0 A2tcm2 a2 = Qf1 proached by considering the force and torque equations sep- ∂q1 ∂q1 ∂A2 T arately. Substituting rcmi = Witcmi (Fig. 1) into (4) gives us tr W1 tcm2 a2 = Qf2 ∂q2 this set of equations for Qf j . Factoring out common terms, we get n T ∂Wi a t = Q . ∂A0 ∑ i cmi f j tr t aT + A t aT + A t aT = Q i=1 ∂q j cm0 0 1 cm1 1 2 cm2 2 f0 ∂q0 ∂A1 T T For n = 3 we get this system of equations: tr W0 tcm1 a1 + A2tcm2 a2 = Qf1 ∂q1 ∂A2 T T ∂W0 T ∂W1 T ∂W2 tr W t a = Q + + = Q 1 cm2 2 f2 a0 tcm0 a1 tcm1 a2 tcm2 f0 ∂q2 ∂q0 ∂q0 ∂q0 ∂W ∂W ∂W 5We are ignoring a technical detail that would unnecessarily complicate aT 0 t + aT 1 t + aT 2 t = Q 0 cm0 1 cm1 2 cm2 f1 the equations at this point: the matrix A j may be a function of up to six gen- ∂q1 ∂q1 ∂q1 eralized coordinates qk so the indices of the matrix A j and the generalized ∂W ∂W ∂W T 0 + T 1 + T 2 = Q coordinate may not be the same. This does not materially change the form a0 tcm0 a1 tcm1 a2 tcm2 f2 of the equations or the factorization. ∂q2 ∂q2 ∂q2 Because of common subexpression elimination in D* it takes Algorithm 1 coefficient algorithm O(n) time to compute the equations in this form. nd: node to find coefficient of Let us turn now to the torque equations (5): lc: true if left child graph of this contains nd rc: true if right child graph of this contains nd T coeff(function nd) n ^R T ∂ i if this is leaf return this Qτ j = ∑ Ri bi. i=1 ∂q j else foreach (node c in this.children){c.coeff(nd)} We will show how we can move the term R / q in front ∂ i ∂ j if (this.type not one of {+,-,*,/}) ERROR for effecient factorization. To this end, we introduce the fol- if (this is * && lc && rc) ERROR lowing identity if (this is / && rc) ERROR ⎧⎡ ⎤⎡ ⎤⎫ 0 −az ay 0 −bz by if (!(this is + || this is -)) return this ⎨ ⎬ tr a˜b˜ = tr ⎣ az 0 −ax ⎦⎣ bz 0 −bx ⎦ else if (lc && !rc){return this.leftchild} ⎩ −ay ax 0 −by bx 0 ⎭ else if (rc && !lc){return this.rightchild} else return this = −2aT b. T Using this relation, we can rewrite the ωq j bi terms Because they share common factors we can combine the gen- T = −. tr ˜ ˜ ωq j bi 5 ωq j bi eralized force and torque equations to get an equation for each of the generalized forces Q0,Q1,Q2 illustrated in Fig. 2. 1 T ∂Ri ˜ = tr − Ri bi Finally, we get the following efficient form of the Lagrangain 2 ∂q j equations of motion: 1 ∂Ri ˜ T = tr − biRi (10) 2 ∂q j ∂A j tr{W j−1 φ j} = Q j (13) where we used the relation tr (AB) = tr (BA) for (10). Now ∂q j ∂Ri we can rewrite the torque equations with ∂q on the left-hand j T 1 T side where everything will factor nicely: φ j = tcm a − B jU + A j+1φ j+1 j j 2 j n Q = T b (11) τ j ∑ ωq j i i=1 4 Forward and Inverse Dynamics ( n ) ∂Ri 1 ˜ T For the inverse dynamics problem, we can use the mo- = tr ∑ − biRi (12) i=1 ∂q j 2 tion specification, q(t), and (13) to compute the generalized forces. Q is a function of q(t), q˙(t), q¨(t), all of which are Define two new matrices, j computed from the motion specification. Computing the gen- R 0 b˜ 0 U = i , B = i eralized forces takes O(n) time. i 0 0 i 0 0 The general dynamics formula of (13) is not in the proper to rewrite the torque equations using the full transformation form to directly solve the forward dynamics problem. How- Wi instead of Ri: ever, the equations of motion have the following structure: ∂Ri 1 ˜ T ∂Wi 1 T − biRi = − BiUi . M(q)q¨ + c(q,q˙) = Q. (14) ∂q j 2 ∂q j 2

Using the same 3 degree of freedom system that we used for We can see that Mi j, the (i, j) entry of M is the coefficient of the force equations we get the following three torque equa- q i ¨ = tions ¨ j of the -th equation. Also, if we set q 0, then the LHS of (13) becomes c(q,q˙). 1 ∂W0 T ∂W1 T ∂W2 T − tr B0U0 + B1U1 + B2U2 = Qτ0 2 ∂q0 ∂q0 ∂q0 We derive a symbolic expression for M and c using D* . For 1 ∂W ∂W ∂W − tr 0 B UT + 1 B UT + 2 B UT = Q this, we have added two new functions, coefficient and q 0 0 q 1 1 q 2 2 τ1 2 ∂ 1 ∂ 1 ∂ 1 substitute, to our implementation of D*. These make 1 ∂W ∂W ∂W − tr 0 B UT + 1 B UT + 2 B UT = Q it trivial to reorganize these equations into an easily solved 2 ∂q 0 0 ∂q 1 1 ∂q 2 2 τ2 2 2 2 form. As in the force relation, we transform the equations into upper triangular form using (7) and (8), and factor out common Given a D* function, fi, which computes f (q,q˙,q¨) and a terms i D* variable, qiddt, which corresponds toq ï, the function 1 ∂A0 T T T − tr B0U0 + A1 B1U1 + A2 B2U2 = Qτ0 fi.coefficient(qiddt)returns the symbolic expression 2 ∂q0 which represents the coefficient ofq ï. Since M is a sym- 1 ∂A1 T T − tr W0 B1U1 + A2 B2U2 = Qτ1 metric matrix, we can create a symbolic expression of M by 2 ∂q1 1 ∂A calling this function forq ¨ j in each fi for i ≥ j. The ci(q,q˙) − tr 2 T = Q W1 B2U2 τ2 are computed by calling 2 ∂q2 ∂A0 T 1 T T 1 T T 1 T tr tcm0 a0 − B0U0 + A1 tcm1 a1 − B1U1 + A2 tcm2 a2 − B2U2 = Q0 ∂q0 2 2 2 ∂A1 T 1 T T 1 T tr W0 tcm1 a1 − B1U1 + A2 tcm2 a2 − B2U2 = Q1 ∂q1 2 2 ∂A2 T 1 T tr W1 tcm2 a2 − B2U2 = Q2 ∂q2 2

Figure 2: Combined force and torque equations

p seen in some of our examples.6 f(q) fqq(q) The Frenet frame along a curve could be used, for example, to constrain beads to slide along a wire, or to make a roller

b fq(q) coaster or train roll along a track. If the curve f(q) has a con- n tinuous, nonzero second derivative everywhere in a region, then it is easy to deﬁne a coordinate frame that travels along Figure 3: Traveling coordinate frame deﬁned by the tangent, the curve b p n f fq(q), and second derivative, fqq(q) of the curve f(q) C(q) = , 0 0 0 1

where b = fq , n = b×fqq , and p = n × b. ∥fq∥ ∥b×fqq∥ Assume we want to constrain an object to move along a space curve, f(q ) : 1 → 3, so that the x axis in object model S2 p ℝ ℝ space always points in the direction of the tangent, fqp , to the

S1 curve, and the object can rotate by qr about the axis of the tangent vector. The combined constraint will use two transformation matrices: the curve orientation and translation constraint matrix, and a rotation about the x axis Figure 4: Surface-surface coordinate frame constraints Apath = C(qp)RrotX (qr). fi.substitute(new[]{q0ddt,...qnddt}, An example of several systems constrained this way is shown new[]{0,...,0}) in Fig. 5(a). Moving up one dimension from curves we can deﬁne a sur- This will substitute zero for all theq ¨i leaving just ci(q,q˙) . Forward dynamics is performed by solving face frame whose origin is on the surface with two axes tangent to the surface. For the parametric surface

Mq¨ = Q − c T f(q1,q2) = [ fx(q1,q2), fy(q1,q2), fz(q1,q2)] using standard linear algebra algorithms. The Cholesky de- we use the two tangent vectors of unit lengths composition method is a good choice in this case since M is positive deﬁnite. ∂f ∂f ∂f ∂f p0 = /∣∣ ∣∣ p1 = /∣∣ ∣∣ ∂q1 ∂q1 ∂q2 ∂q2 5 Examples of Complex Constraints and the unit normal vector n = p0 × p1 to create a surface frame Systems are built by connecting rigid bodies with motion p n × p n f S = 0 0 . constraints, represented as a transformation matrix which is 0 0 0 1 a function of up to 6 generalized coordinates. To create and simulate a mechanism the user has only to deﬁne these trans- We can force the surface frames of two surfaces, shown in formation matrices of rigid bodies with respect to their par- Fig. 4, to align by using this transformation ents as functions of generalized coordinates. This is in con- S = S S−1 (15) trast to conventional, numerical dynamics packages in which 1,2 1 2 one must provide the derivatives of the transformations up to 6Note that some of the example constraints have singularity problems, the second order. This can be very complicated as will be but this is a separate issue from the main subject of this paper. (a) Path constraint (b) Surface surface constraint (c) Rolling constraint

(d) Tong-like mechanism (e) Ellipsoidal ball rolling over ellip- (f) Multiple systems soidal surface

Figure 5: Example mechanisms

where S1 and S2 define the two surface frames. The coupled with 31 holonomic closed loop constraints. Newto- matrix S1,2 is a function of four generalized coordinates nian dynamics algorithms will generate a 33×6 dimensional q1, q2, q3, q4. We can make a chain of torii, shown in ODE for this type of mechanism. Fig. 5(b), by applying (15) to the sequence of torus objects. Fig. 5(e) shows an ellipsoidal ball rolling over an ellipsoidal It is also possible to simulate certain types of rolling contact. surface with the rolling constraint enforced using the La- In Fig. 5(c) the wheels are constrained to roll without slid- grange multipliers. To establish a rolling constraint between ing. Assume the wheels lie in the xy plane and rotate about two surfaces let (R1,t1) and (R2,t2) denote the orientation the z axis. The relative transformation of the bigger wheel and position of the surface frames of the two surfaces. When (colored yellow and green) can be parameterized by a single the two surfaces do not slip, the relative velocity of the two T ˙ T ˙ generalized coordinate q; the position of the body frame is surface frames must be same, i.e., R1 t1 = R2 t2 which yields [cosq,sinq,0]T , and, due to the non-slip constraint, the ori- this constraint equation: entation is also defined as a function of q, T dt1 T dt2 r1 Pq˙ = 0, P = R1 − R2 . (16) R = RrotZ( q) dq dq r2 ˙ where r1 is the radius of the half circle and r2 is the radius of Note that one must derive P, the time derivative of P, in or- the wheel. The relative transformation of the smallest wheel der to use Lagrange multipliers. D* computes P˙ as well as can be defined similarly. the derivatives in (16) so one need not worry about manually computing them. Fig. 5(d) shows an example in which the generalized coordinates can be defined for a closed loop mechanism. A total of 32 rigid bodies is constrained such that each joint angle 6 Results of the link can be defined as a function of a single general- Because the symbolic Lagrangian algorithm performs alge- ized coordinate, i.e., qi = qi(q) for all i. A “fish” object is connected to a tong-like chain via a universal joint. In this braic simplification and common subexpression elimination case, our algorithm creates an ODE with only three indepen- the operations count is not just a function of the number of dent variables. In contrast, non-symbolic, conventional La- degrees of freedom in the system, as it is for previously pub- grangian dynamics algorithms such as the Articulated-Body lished forward and inverse dynamics algorithms. We mea- Algorithm [Featherstone 1987] cannot achieve this level of sured the time complexity of the new algorithm on two sys- reduction for the closed loop system because they endow in- tems and present those results here. dependent coordinates to each mobile rigid body followed Results for the inverse dynamics problem are shown in Fig. by enforcing the loop constraints using Lagrange multipli- 6. Fig. 6(a) shows the total number of operations as a func- ers. For this example, they will obtain a 34 dimensional ODE tion of the number of degrees of freedom for a simple linear 1800 10000 algorithm ± * algorithm ± * 1600 1400 8000 1200 Symb. best 328 365 Symb. best 818 943 6000 1000 Balafoutis 386 450 Balafoutis 790 956 800 4000

operations 600 operations Symb. worst 509 971 Symb. worst 1875 2389 400 2000 200 0 0 (a) Inverse dynamics (b) Forward dynamics 2 3 4 5 6 7 8 9 10 11 12 6 10 14 18 22 26 30 34 38 42 46 Degrees of freedom Degrees of freedom Table 1: Operations count comparison for a 6 degree of free- (a) (b) dom chain of revolute joints: symbolic Lagrangian vs. Bal- afoutis. Figure 6: Inverse dynamics, arithmetic operations versus number of generalized coordinates: (a) linear revolute chain (not illustrated). (b) torus chain (Fig. 5(b)). Growth is well In practice the new algorithm generates efﬁcient dynamics approximated by a best ﬁt line. code. For example, Fig. 5(b), which has the largest number of generalized coordinates (14) of the examples shown in Fig. 5, requires only 0.02 miliseconds to evaluate the motion equa- 18000 90000 16000 80000 tion. Solving the matrix motion equation using Cholesky 70000 −3 14000 decomposition takes an additional 1.3 × 10 miliseconds 12000 60000 10000 50000 on a 2.66Hz Intel Core 2 Quad CPU. Our code is not mul-

8000 40000

operations operations 6000 30000 tithreaded so only a single processor is used for the mo- 4000 20000 2000 10000 tion equation computation. Comparing the compute time of 0 2 0 the motion equation (O(n )) and that of the matrix solution 2 3 4 5 6 7 8 9 10 11 12 6 10 14 18 22 26 30 Degrees of freedom Degrees of freedom (O(n3)) suggests that the two procedures will take the same (a) (b) amount of time when the DOFs of the system are about 200. For models with ≪ 200 DOFs, the algorithm is dominated Figure 7: Forward dynamics, arithmetic operations versus by the evaluation of the O(n2) motion equation procedure. number of generalized coordinates: (a) linear revolute chain (not illustrated). (b) torus chain (Fig. 5(b)). Growth is well approximated by a best fit quadratic curve. 7 Discussion and Conclusion chain of revolute joints. Fig. 6(b) shows the total number of Because the Lagrangian formulation always maintains kine- operations for a linear chain of torii, of the type shown in Fig. matic constraints, it is possible to take very large time steps 5(b). Both systems are well approximated by a best fit line. when precise modeling of the true dynamics is unnecessary. For the forward dynamics problem Fig. 7 shows the mea- In computer games and animation for film or television it is sured number of operations in the equations of motion for rarely necessary to model dynamics to high precision, since the same systems of Fig. 6. The operation count is increas- human viewers don’t notice small dynamic errors. By com- ing quadratically in both cases. Because eventually the O(n3) parison, kinematic constraint errors are much more notice- matrix solution time will dominate the overall complexity of able, since they result in parts of the mechanism failing to forward dynamics will be O(n3). connect properly. Existing forward and inverse dynamics algorithms are usu- Fig. 8 shows two frames, (a), (b), from a dynamic simula- ally limited to a very restricted set of joint types, often just tion of a bead constrained to move along a wire: (a) shows revolute or translational joints – other joint types require sig- a step along the tangent to the constraint manifold, as would nificant effort on the part of the end-user to implement. Both be done when using nonlinear constraint equations, and (b) of these facts make direct comparison of our algorithm to ex- shows the step taken by our symbolic Lagriangian formula- isting algorithms problematic; the capabilities of the two sets tion. The tangent step moves the bead far off the constraint of algorithms are very different so it is hard to make an apples manifold; convergence of a nonlinear constraint solver to the to apples comparison. correct point on the constraint manifold is highly unlikely. To guarantee convergence a much smaller time step would We created a nonbranching system consisting of six 1 DOF be necessary to correctly maintain the kinematic constraints. revolute joints, connected in a linear chain, and used this as a We have taken advantage of this robustness property of sym- comparison test case. Roughly best and worst case results for bolic Lagrangian dynamics in all the animations, included the symbolic Lagrangian algorithm are compared in Table 1 in the supplemental materials for this paper, which have a to the most efficient hand derived algorithms [Balafoutis and timestep of one frame time (either 1 th or 1 th second). Patel 1991] that requires manual differentiation of the con- 60 30 straints. Our algorithm is competitive even in this restricted We do not deal with simulating contact and collision in this class, and, of course, it can easily be applied to a much wider paper since it is a separate problem that is resolved after com- range of joint types. puting the dynamics of a system. Any collision and contact puter Methods for Ordinary Differential Equations and Differential-Algebraic Equations. SIAM.

BALAFOUTIS,C.A., AND PATEL, R. V. 1991. Dynamic Analysis of Robot Manipulators: A Cartesion Tensor Ap- proach. Kluwer Academic Publishers.

BARAFF, D. 1996. Linear-time dynamics using lagrange multipliers. In Proceedings of SIGGRAPH 96, Computer (a) (b) Graphics Proceedings, Annual Conference Series, 137– 146. Figure 8: (a) Yellow arrow shows step along tangent to constraint manifold. (b) Step taken by our algorithm. COHEN, M. F. 1992. Interactive spacetime control for animation. Computer Graphics (Proceedings of SIGGRAPH 92 (July), 293–302. handling techniques for minimal coordinate approaches can be used with the new algorithm. For example, [Kokkevis and FEATHERSTONE,R., AND ORIN, D. 2000. Robot dynamics: Metaxas 1998] can be used for simulating collision response, equations and algorithms. IEEE Int’l Conf. on Robotics enforcing unilateral and bilateral constraints as well as joint and Automation (IROS) 2000, 826–834. limits, and computing contact or friction forces when the sys- FEATHERSTONE, R. 1987. Robot Dynamics Algorithms. tem is in contact with environment. Kluwer Adademic Publishers, Boston. The most significant limitation of the new algorithm is that it GRINSPUN,E.,HIRANI,A.N.,DESBRUN,M., AND can only be applied to systems with parameterizable kinemat- SCHRCDER´ , P. 2003. Discrete shells. 2003 ACM SIG- ics. In general, closed loop systems do not have a closed form GRAPH / Eurographics Symposium on Computer Anima- parametric description, although simple closed loop systems, tion (aug), 62–67. such as 4 bar linkages, do. In some cases even open loop systems do not have closed form parametric kinematics. How- GUENTER, B. 2007. Efficient symbolic differentiation for ever, in practice parameterizability is not generally a limi- graphics applications. ACM Transactions on Graphics 26, tation since the universe of mechanisms which can be de- 3 (July), 108:1–108:12. scribed this way is very large. HERBERT GOLDSTEIN,CHARLES P. POOLE, J. L. S. 2001. Another issue is handling topology change. If we can pre- Classical Mechanics. define a set of sub-systems that a system can break into, then we can compute the motion equations of the sub-systems off- HOLLARS,M.G.,ROSENTHAL,D.E., AND SHERMAN, line and the switch between the equations in real time. How- M. A. 1994. SD/FAST User’s Manual. ever, if one wants to simulate completely general topological KASS, M. 1992. Condor: Constraint-based dataflow. Com- changes, such as breaking a system into every single rigid puter Graphics (Proceedings of SIGGRAPH 92) (July), bodies, a maximal coordinate approach will probably be a 321–330. better choice. The great strength of the new algorithm is that complex joints KOKKEVIS,E., AND METAXAS, D. 1998. Efficient dy- can be easily specified and robustly and rapidly simulated. namic constraints for animating articulated figures. Multi- For example, a common weakness of human figure anima- body System Dynamics, 89–114. tions is that the knee joint is modeled as a hinge when it is KRY, P. G., AND PAI, D. K. 2003. Continuous contact simu- more accurately modeled as a four bar linkage. This gives lation for smooth surfaces. ACM Transactions on Graphics natural human knees a significantly larger range of motion 22, 1 (Jan.), 106–129. than artificial knee joints, which are simple hinges. Similarly, the human scapula is a complex joint which can be modeled LEE,S.-H., AND TERZOPOULOS, D. 2008. Spline joints as a surface-surface joint type. Incorporating these complex for multibody dynamics. ACM Transactions on Graphics joints in a human body simulation would be straightforward 27, 3 (aug), 22:1–22:8. with our algorithm but considerably more problematic using OLSSON,H.,TUMMESCHEIT,H., AND ELMQVIST,H. existing algorithms. In addition, the robustness, efficiency, 2005. Using automatic differentiation for partial deriva- and simplicity of the algorithm make it an attractive choice tives of functions in modelica. Proceedings of the 4th In- for real-time applications, such as computer simulations or ternational Modelica Conference (Mar.), 105–112. games, where numerical simulation failure is not an option. POPOVIC´ ,J.,SEITZ,S.M.,ERDMANN,M.,POPOVIC´ ,Z., References AND WITKIN, A. P. 2000. Interactive manipulation of rigid body simulations. Proceedings of ACM SIGGRAPH ASCHER,U.M., AND PETZOLD, L. R. 1998. Com- 2000 (July), 209:1–209:9. SAMIN,J.-C., AND FISETTE, P. 2003. Symbolic Modeling of Multibody Systems. Springer.

SHI, P., AND MCPHEE, J. 2000. Dynamics of ﬂexible multibody systems using virtual work and linear graph theory. Multibody System Dynamics 4, 4, 355–381.

TURNER, J. D. 2003. Automated generation of high-order partial derivative models. AIAA Journal 41, 8, 1590–1598.

VILLARD,D., AND ARNALDI, B. 1996. Symbolic differentiation library for simulation of multibody rigid systems. Mathematics and Computers in Simulation 42, 4-6, 659– 673.