Rigid Body Simulation with Contact and Constraints by Michael Bradley Cline B.S., The University of Texas at Austin, 1999 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF Master of Science in THE FACULTY OF GRADUATE STUDIES (Department of Computer Science) We accept this thesis as conforming to the required standard The University of British Columbia July 2002 © Michael Bradley Cline, 2002 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of C o H p o t g g 3 c . \ E - t J c € . The University of British Columbia Vancouver, Canada Date T ^ L V \ 2 * U 2 * * 2 DE-6 (2/88) Abstract We present techniques for constructing an interactive rigid body simulation sys-tem, and describe our implementation such a system. We use a constraint-based simulation approach, incorporating a time stepping method which handles multiple contacts with friction in a general way. We describe a method for constraint stabilization for simulations with con-tact. The method is based on the post-stabilization methods of Ascher and Chin, but is formulated as a linear complementarity problem, allowing us to solve for the stabilization step when there are redundant inequality (contact) constraints. As an application of our simulation software, we present a method for hu-man character animation where motion capture data is used to drive a rigid body simulation. This allows physical constraints and forces acting on the character to be taken into account. n Contents Abstract ii Contents iii List of Figures vii Acknowledgements ix 1 Introduction 1 1.1 Related Work 2 1.2 Thesis Organization 4 2 Background 6 2.1 Position and Rotation of a Rigid Body 6 2.1.1 Rotation Matrices 6 2.1.2 Euler Angles / Roll, Pitch, Yaw 7 2.1.3 Unit Quaternions 8 2.2 Velocity of a Rigid Body 11 2.2.1 The Velocity of a Point on a Rigid Body 12 2.2.2 The Relationship Between Angular Velocity and Quaternions 12 2.3 Equations of Motion 12 iii 2.4 Twist/Wrench/Adjoint Notation 14 2.4.1 Twists 15 2.4.2 Wrenches 16 2.4.3 Transforming Twists and Wrenches Between Coordinate Frames 16 2.4.4 Newton-Euler Equations 17 2.5 Constrained Dynamics 18 2.5.1 Reduced Coordinates vs. Full Coordinates 19 2.5.2 Expressing Constraints as Equations 20 2.5.3 Solving Constrained Dynamics Equations using Lagrange Mul-tipliers 23 2.6 Incorporating Contact Constraints 24 2.7 Linear Complementarity Problems 28 2.7.1 Lemke's Algorithm 28 2.7.2 Converting a Mixed LCP to a pure LCP 33 2.7.3 Solvable LCPs for Contact with Friction 34 2.7.4 Implicit Method for Stiff Systems 38 3 Stabilization For Simulation With Contact 41 3.1 Overview of Stabilization 41 3.2 Baumgarte Stabilization 44 3.3 Post-Stabilization 44 3.4 Fitting Post-Stabilization into Our Dynamics LCP Framework . . . 46 4 System Architecture 49 4.1 System Overview 50 4.2 Accommodating both 2D and 3D Simulations 53 iv 4.3 Keeping Track of Simulation State 54 4.4 Implementing Constraints 55 4.4.1 Joint Constraints 55 4.4.2 Contact Constraints 60 4.5 Collision Detection 65 4.6 External Forces 66 4.7 Interacting with the Simulation 67 4.8 LCP Solver Implementation 68 4.8.1 Exploiting Coherence Between Basis Matrix of Consecutive Iterations 68 4.8.2 A Generalization of Baraff 's Technique for Incorporating the Auxiliary Constraints into a Linear Time Lagrange Multiplier Approach 69 4.8.3 Numerical Difficulties We Encountered with Lemke's Algorithm 70 5 Results and Discussion 73 5.1 An Application: Transforming Motion-Capture Data 73 5.1.1 Implementation Details 74 5.1.2 Experiments and Discussion 81 5.2 Experiments to Test The Stabilization Algorithm 86 5.2.1 6-Link Chain 86 5.2.2 Stabilization of Contact Constraints 91 5.2.3 Advantages and Disadvantages of Post-Stabilization 92 6 Conclusions and Future Work 95 6.1 Future Work 96 v Bibliography 98 VI List of Figures 3.1 An unstabilized simulation of a swinging pendulum 41 4.1 Overview of the simulator 52 4.2 An example showing the relationship between an instance of RigidBodySet and several of RigidBody objects and Constraint objects 72 5.1 The skeleton model for our animated character 82 5.2 A sequence of a ball hitting a simulated arm 83 5.3 A sequence of a ball hitting a simulated body 84 5.4 A simulation of a 6-link chain falling freely under gravity, with and without constraint stabilization 86 5.5 Unstabilized chain simulation: constraint error has grown very large. 87 5.6 Constraint error graph for chain with no stabilization 88 5.7 Constraint error graph for Baumgarte stabilization on 6-link chain, for reasonable values of constant 89 5.8 Constraint error graph for Baumgarte stabilization on 6-link chain, for unreasonable value of constant 89 5.9 Constraint error graph for post-stabilization on 6-link chain 90 5.10 Smaller-scale version of previous constraint error graph 90 vii 5.11 Contact constraint error over time for a rigid rectangle undergoing user interaction. No stabilization is used 91 5.12 Two screen captures from a contact simulation with a single moving rigid rectangle 92 5.13 Two screen captures from a contact simulation with five moving rigid bodies 92 vm Acknowledgements "J* c\ju ve.ru ^rqxcftAX t o i^<-\ ^ l Y t r v i ^ o r , t>Lne.-=jk Fqj . . V - ' . xko iCL t>Lne.«jk •* o O d I aUJAcince., T-Kifi "Lke.'ji.s? w o ' - w L HOT. kqve. be.e.n po-^^iJpXe.. ^pe.c.cqx x k q n W q£«so ^ o " t o Ore. r<-=tck«.r " for q_<ace.e_i>vi t o be. ^ - j w o i i i i . ce.qcLe.c, qnA , O O O d p rov icu ru . A ? w i x k fuqn^ kexpfux co/wue,vvx.<!. V^qn^V^qn.* V i n . cLe.-^e.rve.'S q_ I O O o o o r <?re.qx Ae.qjc o f cceoux " for xKe. k i v u q n «suuiAxqt.Lon exqA*pXe..-=; un <i-kqi?xe.<- / , qneL"foc p<^Xizy\xX<-\ kexp.*V| " t o we.e.eL O K X xke. ntv-VoiA.-? 1>KC.-=J en xke. I a I c O ^ ' - ^ ^Xqx ' i on coke.. "X" x k q n V Fc^nx Vic^ " for /uqni^ \yvce.ce."*x'iyv^ - A l s c ' v W L o n ^ o1 d O t n xke. Kqb , qyvcL " for ^e.x"Civ\^ A*e. OI\X=SLOU<L " for «;o(u«. /u<\ck ne_e.okje.cL- uy\ucncXir\ci O O d O t>ce.qW cxKTLn^ xke. <;uAA*e.r A o n x k - ? wke.n "T wq_-=; k q r i - q x ioorV o n x k u * O "cke.'^'u?;. Ov\ q_ pe.r<jonqx. xevex., "T I-JOIAX<L JciW t o x k q n V <^-krL«T.i-| -For l>e.'tn^ I | a O xkere . x k c o u ^ k xke. ^oooL XL<^ e«? qnoL xke. kqcoL xi^e.-? o? "tke. Xq.<sx xkcee. O O O ^e.qc-=j, pCovu\ln=? A o t L v q t i o n qn<A, s ^ o t i o n ^ x l <ii\pvorT.. VtH pqjre.nX'=J, ' F o b ;KpporX. I n c qn<L "TW.W, qje-^o cLe.«je.rve. x k q n W - for xke.'ur < j t w o r x . MICHAEL BRADLEY CLINE The University of British Columbia July 2002 IX Chapter 1 Introduction Predicting the motion of real-world objects using computers has a history almost as old as computers themselves. Some of the very first electronic computers were designed in secrecy, at great expense, with the sole purpose of calculating tables to predict the motion of the U.S. Navy's projectile weapons. The past couple of decades have seen an explosion in amount of research on physics-based animation, especially rigid body motion, by robotics and computer graphics researchers. Applications of this technology range from virtual reality and games to simulating and planning robot motion. This thesis makes two main contributions to the area of rigid body simu-lation. The first contribution is a description of a constraint stabilization method specifically designed to handle both contact constraints and equality constraints. Our stabilization is based on the post-stabilization methods described by Ascher, Chin, et al. [Asc97, ACR94], but is phrased as a linear complementarity problem to allow stabilization of contact constraints. This fits nicely with our dynamics frame-work, which is based on the time-stepping schemes of Anitescu et al. [AP97]. We describe our stabilization method in Chapter 3. 1 The second contribution of this thesis is a detailed description of the archi-tecture of our rigid body simulation software. Although there is a large amount of published work relating to rigid body physics, implementing a general purpose rigid body simulation system remains a challenging task. Most papers related to the subject describe only a small component, and figuring out how all of the pieces fit together is non-trivial. Our system architecture is explained in Chapter 4. We present an application of our simulation software in Chapter 5 where we use some novel techniques for human animation. We show how to extend and use the simulator for dynamic simulation of human motions from motion capture data. The simulated humans are driven using motor controls which are based on concepts from biomechanics and neuroscience research. 1.1 Related Work Dynamic simulation of rigid bodies has a long history, going back over two decades. We cannot hope to list all of the work that has been published on this topic. We mention here the work that was most influential to this thesis. Some of the first published work in this area was on simulating the dynamics of articulated rigid bodies (sets of rigid bodies connected by joints), using a reduced coordinate approach. Vereshchagin [Ver74], in 1974, and Armstrong and Green [AG85], published efficient algorithms for articulated body dynamics. In the late eighties, Featherstone's [Fea87] book laid down a mathematical foundation for rigid multibody simulation. Papers by Barzel and Barr [BB88] and Witkin et al. [WGW90] were earlier works that introduced constraint-based systems using Lagrange multipliers. These systems used full coordinates for each rigid body, and implemented constraints 2 through constraint forces. However, these works dealt primarily with bilateral con-straints, mostly ignoring the issues of contact. Originally, these constraint-based methods also suffered from slow computation algorithms. For instance, [BB88] re-ported using singular value decomposition to solve the constraint force equations, a technique which was relatively slow and did not take advantage of sparsity in the constrained motion equations. Lubich [LNPE92] later demonstrated techniques to solve such systems in linear time using sparse matrix techniques. Around the same time as [BB88], Moore and Wilhelms [MW88], and Hahn [Hah88] published methods for simulating contact. Neither addressed contact in the general case, where there can be multiple bodies in contact, and multiple contacts on the same body. Modelling contact, and in a general and robust way, has remained a difficult problem. Mirtich and Canny [MC95] continued work along the lines of [MW88] and [Hah88], using impulse based methods which were well suited to temporary contacts and single contact points, but would have difficulty simulating, for instance, large stacks of objects statically resting on each other. Many others have worked on extending the constraint-based methods to han-dle contact. This is typically done by formulating the constrained dynamics equa-tions as a linear complementarity problem (LCP), which is essentially a problem of finding some solution to a linear system that satisfies certain inequality constraints. The first paper to pose the contact problem as an LCP was published by Lotstedt [L82] in 1982. Baraff later presented a method [Bar94] that used an LCP algorithm by Cottle and Dantzig [CD68] to solve contact and static friction forces at interactive rates. One of the difficulties with earlier LCP methods was that there is no guaran-tee of the existence of a solution, in the presence of contact with Coulomb friction. 3 Indeed, there are known configurations of rigid bodies for which no set of contact and friction forces satisfies all of the constraints (such as the Painleve paradox). Us-ing a model that allows impulsive forces turned out to be the key to avoiding these problems. Stewart and Trinkle [ST96] introduced a time-stepping scheme that com-bines the acceleration-level LCP with a time step, to obtain an LCP where the variables are velocities and impulses. Impulsive forces are incorporated naturally in such a model. Their method guaranteed solvability for a larger class of problems, and was later modified by Anitescu and Potra [AP97], leading to an algorithm that guaranteed solvability regardless of the configuration or number of contacts. Our work is based heavily on [AP97]. The stabilization method that we will present is based on work by Ascher and Chin [Asc97] [ACR94]. These papers discuss stabilization methods in general and post-stabilization in particular as the favoured method. We will draw on this work in Chapter 3, and compare post-stabilization and Baumgarte stabilization [Bau72] for counteracting constraint drift in our rigid body simulations. 1.2 Thesis Organization The layout of this thesis is as follows: The next chapter will present background material on rigid body physics laws and simulation algorithms. We will begin by explaining the various ways of parameterizing the state of a rigid body (the position, orientation, and velocity). We will give the equations of motion. We will explain how to mathematically repre-sent constrained motion, and then discuss how to solve the motion equations with constraints. Contact constraints will be given special attention, and we will describe how the problem of dynamics with contact is modelled as a linear complementarity 4 problem (LCP). This is followed by a description of Lemke's algorithm, which is the method we use to solve the LCP. We finish the chapter by presenting the time-stepping method given by Anitescu and Potra [AP97], which our implementation is based on. In Chapter 3, we explain the need for constraint stabilization in a system like ours. We will present Baumgarte's method as one example of a stabilization technique, then we will present our version of the Ascher-Chin post-stabilization, and explain how and why we use an LCP similar to the dynamics LCP to accomplish the post-stabilization. Chapter 4 gives the details of our system's architecture. We explain how we have implemented the methods discussed in the previous chapters. Chapter 5 presents the results of our work. Two of the goals of our simulation software were to be extensible, and to run at interactive rates. To demonstrate that these goals were met, we show an application of our software to human character animation. By combining motion capture data with a rigid body simulation, we are able to generate perturbed animations of characters reacting to external physical constraints. We can do this at interactive frame rates (>30 fps) on a modern personal computer. In this chapter we also present some results from experiments that test our post-stabilization method. In Chapter 6, we will conclude the thesis and discuss opportunities for future work. 5 Chapter 2 Background 2.1 Position and Rotation of a Rigid Body An unconstrained rigid body has six degrees of freedom: three degrees of transla-tional freedom, and three degrees rotational freedom. There are many possible ways to parameterize the position and rotation of a body. The translational position is usually given as a vector from the origin to some fixed point on the body. The centre of mass of the body is a convenient choice for this fixed point, but any point can be used. Parameterizing rotation of a rigid body in 3D is more tricky. There are several methods commonly used, each with its own advantages and disadvantages. 2.1.1 Rotation Matrices Within computer graphics, rotation matrices are the most well known method of representing rotation. Any rotation in 3D can be parameterized by a 3 x 3 orthogo-nal matrix. Rotation matrices are convenient because in addition to parameterizing the rotation, they can also be used to transform vectors from one coordinate frame 6 to another. Together, the rotation matrix R and position vector p define a rigid transfor-mation g. Using homogeneous coordinates, we write this transformation as a 4 x 4 matrix (n p\ 9= • (2-1) V° V A drawback of using rotation matrices is that they use nine numbers to pa-rameterize three degrees of freedom. In terms of memory requirements, this means that rotation matrices take up more space than they really need to. Another dif-ficulty is that in the presence of numerical error, it is possible to arrive at a non-orthogonal matrix. Thus it is sometimes necessary to do a costly re-normalization to ensure that the matrix actually corresponds to a valid rotation. 2.1.2 Euler Angles / Roll , P i tch , Yaw According to Euler's rotation theorem (see [BR79]), any rotation can be described by three angles. Thus any rotation matrix R can be parameterized by three angles (a, /?,7): R = rot(v\, a) x rot(v2, (3) x rot{vz, 7), (2.2) where v\,V2 and 1*3 are a specific set of axes, and rot(y,0) is the rotation matrix corresponding to a rotation of angle 0 about the axis v. There are many different conventions dictating which axes are actually used, and in which order. The most common convention may be the ZXZ convention, which means rotations are parameterized by a rotation about the Z-axis, followed by the X-axis, and then 7 about the Z-axis again. Another common convention is the ZYX convention, which is also known as roll, pitch and yaw1. Although the Euler angle representation avoids the problems of over-parameterization that rotation matrices suffer from, they have problems of their own. All 3-angle pa-rameterizations suffer from singularities where two of the rotation axes line up and a single rotation can be parameterized by an infinite number of 3-angle combinations. For example, using the ZXZ convention, if the second angle is zero, then the first and third axes line up. Thus a rotation of 6 about the Z-axis can be parameterized by any triplet (a, 0, 7) such that a + 7 = 6. Singularities are undesirable because near the singularity small changes in position can result in extremely large changes in the Euler angles, leading to numerical problems. Another drawback of using Euler angles is that they cannot be used to trans-form vectors directly from one frame to another the way rotation matrices can, nor can rotations be easily combined. The Euler angles have to be converted to matrices first, as in Equation 2.2. 2.1.3 Unit Quaternions Shoemake [Sho85] introduced unit length quaternions to the computer graphics community as a means of representing rotation. Quaternions are often preferable to using rotation matrices or Euler angles. They avoid the main problems of the other methods, and offer some additional benefits such as providing an easy way to correctly interpolate between rotations. 1In the engineering and robotics communities, the term Euler Angles is usually taken to mean ZXZ, and other terminology (e.g., Fick angles, Helmholtz angles, roll-pitch-yaw) are used to name the other conventions[MLS94]. Computer graphics papers tend to confuse the matter by either defining Euler Angles to mean XYZ angles [Mat, GOM98], or by using the term Euler Angles more loosely, to describe any 3-angle parameterization. 8 Quaternions are a generalization of complex numbers. However, their rela-tionship to complex numbers is not of much importance to this thesis, and it is more convenient to think of a quaternion as a pair q = (v,s) containing a 3-vector and a scalar (sometimes we will treat it as a 4-vector). A rotation of angle 9 about the axis v (a unit vector) is given as the quaternion / L\\ sin(0/2) (2.3) Jy w \ cos(0/2) J Like rotation matrices, quaternions must sometimes be re-normalized to make sure they correspond to valid rotations. However, the cost of normalizing a quaternion is much less than the cost of making a 3 x 3 matrix orthogonal. Quaternions do not suffer from the singularity problems of Euler Angles. The mapping between quaternions and rotations is two-to-one because every quaternion represents the same rotation as its negative. Rotations can be easily combined by multiplying two quaternions together. The rule for multiplying quaternions together is <?1®<?2 S1V2 + S2V1 + Vi X V2 ' V SlS 2 - Vl • V2 (2.4) / The inverse of a quaternion is also easy to find, simply by negating either the vector or scalar part of the quaternion. M_1 / "^ qx -qx \QwJ -Qz or / \ qx qy qz \-QwJ (2.5) Quaternions provide an easy way to transform a vector between coordinate frames. If rotation matrix R and quaternion q both represent the same rotation, then R/u = unquat(q <g) quat{v) <g) q -U (2.6) It the equation above, the quat function constructs a quaternion from a vector by appending a zero as the scalar part, and the unquat returns the vector part of the quaternion. In the rest of this thesis, when we talk about the position of a rigid body, we are referring implicitly to both the position and orientation of the body. We will concatenate the position vector pi and the quaternion qi into a single vector (v\ Pi = . Often we concatenate the positions of many rigid bodies into a single V* vector p = \PnJ 10 2.2 Velocity of a Rigid Body Rigid body velocity is usually represented by a combination of an angular velocity vector, u>, and a linear velocity vector v. The angular velocity vector's direction is the axis of rotation, and the magnitude is the rate at which the body is spinning (radians per unit time). The linear velocity vector corresponds to the velocity of some fixed point on the rigid body. As with representing the position of a rigid body, there is a choice of which point is used. There are two different choices that are commonly used. In the graphics literature, v is usually the velocity of the body's centre of mass (e.g., see [Hah88], [Bar97]). This is convenient because the centre of mass has the same equations of motion as a particle. The other option (which is common in the robotics literature) is to let v stand for the velocity of the point attached to the rigid body that is at the origin of the coordinate frame. In Section 2.4, we will discuss twist coordinates, which are used in the robotics literature to describe rigid body velocity. For any vector x that is fixed to a rigid body with angular velocity u, the rate of change of x is given by u> x x. Because we would like to use this fact later in matrix equations, it is useful to define an operator that gives the cross-product matrix of a vector. We will put brackets around a vector to denote that vector's cross product matrix: U) x x = u x = ' 0 -wz wy » W7 U — Wy \-Wy Wr x. (2.7) 11 2.2.1 T h e Velocity of a Point on a Rigid B o d y Given the linear velocity v and angular velocity w o f a rigid body, we can determine the velocity of a point attached to the body. Assume that v is the velocity of a point p0 attached to the body, then any other point p has the velocity p = v + [u>](p-p0). (2.8) 2.2.2 T h e Relat ionship Be tween Angular Velocity and Quaternions If u is the angular velocity vector for a rigid body, and q is the quaternion repre-senting that body's orientation, then the rate of change of q is given by q = - quat(u) ® q. (2-9) Rearranging this equation, we can determine the angular velocity from q. 2q = quat(u>) <g> q, 2q <g> q~x = quat(u>) <g) q <g) g_ 1 , (2.10) 2q®q~~ =quat(u), u> = unQuat(2q <g> q" ). 2.3 Equations of Motion The equation of motion for a rigid body can be derived by thinking of the body as a collection of particles, then integrating over the entire mass [MasOl]. The first result of this calculation is a proof that the centre of mass of any rigid body behaves like a particle. Thus, if v is the velocity of the centre of mass, / is the sum of all 12 forces acting on the body, and m is the total mass of the body, the acceleration obeys Newton's second law: dv J = rn—-. J dt (2.11) Another way of saying the same thing is to say tha t force is the rate of change of linear momentum (f = | ( m t ) ) ) . Whereas linear momentum is related to linear velocity with a scalar (the mass), angular momentum is related to angular velocity with a matrix I , called the angular inertia matrix. The reason for this is that objects generally have different angular inertias around different axes of rotation. Angular momentum is defined as lu. The total torque r applied to the body is equal to the rate of change of the angular momentum: " 5 ^ - (2.12) When the body's distribution is approximated by a collection of k particles, the angular inertia matrix is given by 1 = ^J'rni{rjril - rtrj), (2.13) i = l where m-i is the mass of the z'th particle, and r ; is the vector from the origin to the z'th particle. I here is the 3 x 3 identity matrix. If r ; = (a:;, yi,Zi) then k i = 0 ' 2 2 2 2 \ (2.14) y -XiZi -mzi x\ + ViJ 13 In body coordinates, the angular inertia matrix X is constant. The world coordinates of the angular inertia matrix are given by R I R T , where R is the rotation matrix of the body. So, we can evaluate Equation 2.12 as follows: T = | ( ^ » = lib + Tui = lu + jt(RlbodyKT)u (2.15) = XUJ + (RXbodyR + RZbo^R )u> = Jw + ([u]RlbodyRT + BJbodyRTu)u; = 16J + [a;]Iu; — Z[a>]u;. The final term cancels out since u x u; is zero. This leaves us with a rela-tionship known as Euler's Equation: T = 1U+[U]1U. (2.16) 2.4 Twist/Wrench/Adjoint Notation In the robotics literature we find a more general notation and language for describ-ing rigid body velocities and forces that act on rigid bodies. We will now introduce vectors called twists, which describe velocities, and wrenches, which describe forces, and explain how these objects transform from one coordinate frame to another. Other sources [MLS94] derive these quantities from exponential equations of rota-tion. Here we will concentrate instead on the relationship of these quantities to the "centre-of-mass" parameterizations mentioned earlier. 14 2.4.1 T w i s t s A twist is a vector that expresses rigid motion or velocity. In Section 2.2, we saw how to parameterize the velocity of a rigid body as a linear velocity vector and an M angular velocity vector. The coordinates of a twist are given as a 6-vector v = containing a linear velocity vector v and an angular velocity vector u. According to the way twists are denned, the velocity vector v is not the velocity of the body's centre of mass, but the velocity of a point attached to the body, located at the origin of the coordinate frame. Since our implementation assumes that the velocity vector v is the velocity of the body's centre of mass, and many computer graphics papers give their equations in terms of the motion of the centre of mass, we feel it is important to discuss how our parameterization relates to twists. If v is the velocity of the body's centre of mass, given in world coordinates, and u> is the angular velocity in world coordinates, M then can be seen as a twist in a coordinate frame with its origin at the centre w of mass, but aligned with the axes of the world coordinate frame. Earlier, we gave an equation for the velocity of a point p attached to a rigid body as p = v + [u)(p-pQ). (2.17) If is a twist, then p0 above is the origin, and Equation 2.17 simplifies w to p = v + [u>]p. (2.18) If we use homogeneous coordinates for p, we can write Equation 2.18 as 15 \LJ V P = (2.19) V 0 0 Accordingly, we define a new bracket operator that acts on a twist v = / V v = (2.20) so that Equation 2.19 becomes p = MP- (2.21) 2.4.2 Wrenches A wrench is a vector that expresses force and torque acting on a body. A wrench f = ) contains an angular component r and a linear component / , which are w applied at the origin of the coordinate frame they are specified in. 2.4.3 Transforming Twis ts and Wrenches B e t w e e n Coordinate Frames We can transform twists and wrenches between coordinate frames by multiplying them with a matrix called the adjoint transformation. If the 4 x 4 matrix that transforms points and vectors from frame A to frame B is (2.22) then the 6 x 6 adjoint that transforms twists from frame A to frame B is 16 tAd = | ] . (2.23) [p]R Ry Throughout this thesis, twists will often have a leading superscript to denote their frame. Twists transform from frame to frame according to the following rule: "v = ' A d V (2.24) The inverse of a Ad is ab Ad, which transforms a twist from frame B to frame A: "v = ' A d " 1 "v = °Ad Y (2.25) Wrenches transform differently than twists, and will be written with a leading subscript. The rule for transform a wrench f from frame A to frame B is bf = abAdTJ. (2.26) 2.4.4 Newton-Euler Equations The Newton-Euler equations for a rigid body can now be written in terms of the M (A body's acceleration twist and the wrench acting on the body. To write w W these equations in a world-aligned frame at the center of mass, we simply rewrite the Newton and Euler equations (given in Section 2.3 into a matrix equation: 1 T + H0 r ° j i •- **> 0 ml \v \ 0 0 / \0 ml / \ v 17 1 0 We shall use the symbol M. for the mass-inertia matrix | . Matrix I 0 ml R here is the body's rotation matrix, and I is the angular inertia matrix in world coordinates (equal to R2t0^yRT) . In a frame "J3" that is at the centre of mass and rotating with the body, the equation changes slightly. Ibody 0 \ t (\"u:} 0 6. 6 f = l " | V + 0 ml rb (2.28) V o [°«]y (See [MLS94] for a derivation of this.) Throughout the rest of the thesis, we will write the Newton Euler equations as simply f = Mv (2.29) The coriolis term (the second term on the right hand side in Equations 2.27 and 2.28) will be implicitly included in the wrench f. 2.5 Constrained Dynamics Most interesting simulations of rigid bodies involve some kind of constraints. Usually we want to model systems of bodies that are interacting in some way. Some bodies may be in contact with each other, or attached together by some type of joint. For example, we may wish to simulate a walking robot. We would model the robot as a system of rigid bodies connected by joints between the legs and the body, and there would be contact constraints between the legs and the ground. In this section we explain how to incorporate such constraints into a dynamic simulation. 18 2.5.1 Reduced Coordinates vs. Full Coordinates There are two main approaches to incorporating constraints into a simulation. The first is the reduced coordinate approach, where the number of parameters describing the system's configuration is equal to the number of degrees of freedom in the system. In contrast to this, the full coordinate approach uses a full 6 degree of freedom parameterization of each body's position and velocity. We then represent the constraints by constraint equations, which restrict the position and velocity to a subspace of the possible values. Constraint forces are added to the system to keep the bodies on a trajectory that satisfies the constraint equations. Reduced coordinates have certain advantages over full coordinates. In terms of computational efficiency, the reduced coordinate approach can be slightly faster for highly constrained systems where the remaining number of degrees of freedom is small. (Computation time for algorithms that use a full-coordinate approach tend to depend on the number of DOF that the constraints remove from the system, whereas algorithms for reduced coordinates depend on the number of DOF remaining in the system. However, this could be addressed by arranging the linear algebra carefully [LNPE92], [APC97]). Also, reduced coordinates have the advantage that the entire coordinate space corresponds to valid configurations. When using full coordinates, only some lower-dimensional subspace of the configuration space is consistent with the constraints. Numerical integration errors will lead to invalid states, making stabilization necessary (see Chapter 3). When using reduced coordinates, numerical error can make the simulation less accurate, but at least the resulting configuration will always be a plausible one2. On the other hand, reduced coordinates also have their drawbacks. In certain 2There is some danger in this too, of wrong results looking right 19 situations it may be unclear how to parameterize the configuration space. Kry [KP02] shows how a 5 degree of freedom parameterization can be used to model single point contact between curved surfaces. However, he points out that extending the reduced coordinate approach to more general, multiple contact situations is not so easy. In general, the number of degrees of freedom changes when contacts are made and broken. Using full coordinates, we can handle these changes easily by introducing or removing constraint equations when necessary. This flexibility is one of our main motivations for choosing the full coordinate approach. 2.5.2 Expressing Constraints as Equations We express constraints mathematically as algebraic matrix equations with position, velocity, or acceleration vectors as the unknowns. In general, the configuration space of a set of n rigid bodies has dimension 6n. Adding constraint equations restricts the position (or velocity, or acceleration) to a subspace of smaller dimension. The constraints discussed in this thesis will all be holonomic constraints, which means the constraint on the velocity can be found by taking the derivative of a position constraint. Constraints that do not fit this description are called nonholonomic. An example of a nonholonomic constraint is a ball rolling on a table without slipping. The position of the ball has five degrees of freedom, so there is only one degree of constraint (only the height is constrained). Taking the derivative of the position constraint would only give a velocity constraint of degree one. Additional constraints on the velocity are needed to prevent the ball from slipping. We will describe position constraints with a constraint function g(p), which is a function from the space of possible positions of the rigid bodies, to Rd, where 20 d is the number of degrees of freedom that the constraint removes from the system. If the constraint function returns a zero vector, then the position p satisfies the constraint. Constraint functions for specific kinds of constraints will be described in more detail in Chapter 4. Position constraints can be divided into equality constraints (e.g., joint con-straints), where the constraint is g(p) = 0, and inequality constraints (e.g., contact constraints), where gr(p) > 0. For the rest of this section, we will just be talking about equality constraints. We will talk about contact constraints separately in the next section. Constraining the position of an object also constrains its velocity. The veloc-ity constraint function can be found by taking the time derivative of the constraint function. We want the velocity constraint function to be a linear function of the twists representing the velocities of all of the rigid bodies in the system. We can . Velocity constraints /v,A concatenate all these twists into a vector of twists v = are of the form w dt = J;v = J HI J*2 /7«-A\ Vl V2 = 0, (2.30) \\vnJ J where the index i here is unique for each constraint. The matrix J is called the constraint's Jacobian matrix, which we refer to simply as the Jacobian3. The 3In Physics the Jacobian is the determinant of a matrix. 21 Jacobian is a function of the current position of the bodies involved in the constraint. We will go into detail on how to compute the Jacobians for various constraints in Chapter 4. In practice, most constraints will only involve one or two bodies, so most of the matrices J i . . . J n will be zero matrices. A constraint on the acceleration can be found by taking the derivative again. Doing so results in an equation of similar form to Equation 2.30: Jv + k = 0. The acceleration and velocity constraints use the same Jacobian matrix, but the acceleration constraint has an additional term k that depends on the velocities as well. When there are many constraints, we will often write all constraints simul-taneously. For a system with n bodies, and m constraints, our velocity constraint equation looks like dg dt 92 \9m) = J\I = ^ vv ^ w /W\ J l l J l2 J21 J22 J i n J2n J m l Jm2 • • • " r vi V2 = 0. \\Vnl j (2.31) Note that we refer to all of the matrices J, J;, and J ^ as Jacobians, but we use different fonts to distinguish between the different levels in this hierarchy: • J is used for Jacobians that relate the velocity of a single body to a single constraint. • J is used for Jacobians that relate the velocity of many bodies to a single constraint. 22 • J is used for Jacobians that relate the velocity of many bodies to many con-straints. 2.5.3 Solving Constrained Dynamics Equations using Lagrange Mul-tipliers A constraint Jacobian matrix J has a dual use. In addition to relating the velocities to the rate of change of the constraint function g, the rows of J act as basis vectors for constraint forces. Thus, when we solve for the constraint forces, we actually just need to solve for the coefficient vector A (whose components are called the Lagrange multipliers) that contains the magnitudes of the forces that correspond to each of these basis vectors. Solving Dynamics at the Force-Acceleration Level The total force acting on the system is the sum of the external forces fext (in which we include Coriolis forces) and the constraint forces JTX. Combining the Newton-Euler equations, f = JT\ + fext = Mil, with the constraint equations J\i + k — 0, we get the following system M -JT\ (\i\ (fext\ (2.32) 7 J 0 / \XJ \-k which we can solve for the acceleration v and the Lagrange multipliers A. Many authors have shown how to solve this system in linear time by exploiting the sparse structure of the matrix on the left hand side of Equation 2.32 [Ver74, LNPE92, APC97, Bar96]. 23 There is no guarantee that the matrix in Equation 2.32 will be nonsingular. Singularity here is usually a result of over-constraining the bodies. In our hand made examples, we usually chose constraints carefully enough to avoid over-constraining. In general though, it would be nice to have a system that can handle singularities gracefully. 2.6 Incorporating Contact Constraints Contact constraints are different from joint constraints, and require special treat-ment. There are a few important features that set contact constraints apart from other constraints: • Contact forces can push bodies apart, but cannot pull bodies towards each other. This leads to inequalities in the constraint equations, whereas other types of constraints are equalities. • If there are many points of contact between a pair of bodies, there may be many solutions for the contact forces that are plausible. • In the presence of Coulomb friction, a solution to the dynamics equations may not exist. • Contacts are transient - they come and go. Contacts can appear suddenly when bodies are moving towards each other, resulting in an impact. Impacts between rigid bodies in reality result in large forces applied in over a very short time period, leading to sudden changes in velocity. Since the time scale of the impact is much smaller than the time scale of the overall simulation, it is 24 not practical to simulate the forces and accelerations involved. Instead, we simulate impacts with impulses (more on this in sections 2.7.3 and 4.4.2). There are several methods for handling contact in rigid body simulations. Mirtich [Mir98] gives a summary of several different methods, and explains the ad-vantages and disadvantages of each. We prefer the Linear Complementarity Prob-lem approach, first introduced in the context of constrained mechanical systems by Lotstedt [L82], and later made popular in the computer graphics community by Baraff [Bar94]. To describe the contact model we use, we introduce the following terminology. We use the index i on symbols below to denote that the variable belongs to contact i. • A contact consists of a pair of contact points, one point attached to one rigid body, and the other point attached to another rigid body. The contact points are sufficiently close together for our collision detection algorithm to report a collision. • A contact normal is a unit vector that is normal to one or both of the surfaces at the contact points. • The contact constraint Jacobian for constraint i, denoted by JCj, is a 1 x 6n matrix, where n is the number of bodies (the subscript "c" here stands for "contact"). • A contact force J^ACi is a force acting in the direction of the contact normal which prevents the two rigid bodies from interpenetrating. • The separation distance of a contact is the normal component of the distance 25 between the two contact points. It is negative when the bodies are interpene-trating at the contact. The constraint function c?i(p) for a contact constraint returns the separation distance. The constraint is satisfied for gi > 0. • The relative normal acceleration ai of a contact is the second derivative of the separation distance with respect to time (aj = g'i). The acceleration constraint is satisfied when a* = JCiv + ki > 0. For the purposes of this section, assume that we have some separate method of dealing with impact. This section presents a method for finding the contact forces at more permanent contacts, which we will call resting contacts. We will define a resting contact as one where g and g are both zero (or nearly zero). For contacts where g or g are positive, no acceleration constraint is needed (these con-ditions indicate that the bodies are not touching, or moving apart at that contact, respectively). Situations where g is negative indicate impacts, which are resolved separately. The assumptions that at each resting contact g = 0 and g = 0 are critical to the constraints that we enumerate below. Let a be a vector containing the relative normal accelerations {ai,...,an} for all contacts, and Ac be a vector of contact force multipliers {ACl,..., A^}. The vectors a and Ac are linearly related at a given p. Additionally, we have three constraints: 1. The relative normal accelerations must be nonnegative: a = Jc\i + k > 0. Since g and g are zero, a negative acceleration would cause interpenetration. 2. The contact force magnitudes must be nonnegative (so as to push the bodies apart): Ac > 0 26 3. For each contact i, at least one of a,, A^ must be zero. We write this as aTXc = 0. This may not be obvious at first, but follows if we consider that there can only be a contact force if the bodies are actually touching (giX^ = 0). Differentiating this twice using the chain rule, we get giXCi + 2<7iACi +piA; = 0. Since we assume gi = 0 and gi = 0, the second and third term cancel out leaving simply giXCi = 0, and therefore one of g\, ACi must be zero. The problem of finding a solution to a linear equation given such constraints is called a Linear Complementarity Problem (LCP), a general class of problems that we discuss in Section 2.7. If we let Je be the Jacobian of equality constraints, and Jc be the Jacobian of contact constraints, we can extend Equation 2.32 to include the contact constraints as follows: •j?\ M ft. x ext \~kcJ T^ - o . (2.33) /o\ (M -J? 0 - J e 0 0 \a) \JC 0 0 / \XCJ a> 0,AC > 0,aA Xc In this form, we have a mixed LCP, meaning that only some of the rows have complementarity constraints on them. To obtain a pure LCP, we must eliminate v and Ae by solving for those in terms of Ac. In Section 2.7.2 we explain how to convert a mixed LCP into a pure LCP. 27 2.7 Linear Complementarity Problems A Linear Complementarity Problem (LCP) is, given &nxn matrix M and a n-vector q, the problem of finding values for the variables {21,22, • -,£„} and {u>i,W2, •••,wn\ such that / \ = M / \ Z<l + q, (2.34) \Wnl \Znl and, for all i from 1 to n, z; > 0, wi > 0 and Z{Wi — 0. (These three constraints are called the complementarity constraints). 2.7.1 Lemke's Algor i thm Lemke's algorithm (also known as the Complementary Pivot Algorithm) is one of the most commonly used methods for solving LCPs. We describe the algorithm briefly here. Lemke's method is an iterative method that works by introducing an "artificial variable" ZQ so that Equation 2.34 becomes / \ W2 (A (7\ = M Z2 + Z0 + <?• (2.35) \WnJ \ZnJ yz0J Introducing this artificial variable makes it easy to find an initial solution (we will explain why in a moment). After finding the initial solution, we iterate until we find a solution where ZQ = 0, and the artificial variable is no longer needed. 28 Rewriting Equation 2.35 so that all of the unknowns are grouped into a single vector, we have ^ -M I = q. (2.36) w Here we use en to represent a vector containing n ones. At any given iteration of the algorithm, only n of the variables from the set {ZQ, Z\, ..., zn, w\, ...,wn} are active (i.e., nonzero). The other variables are inactive and are assumed to have a value of zero. The active variables are contained in a vector called the basic vector, y. The basis, denoted by B, is a matrix containing only the columns of the matrix I _ e n _ M I J that correspond to variables in the basic vector. The invariant of the algorithm is that the basic vector always satisfies these properties: 1. There are always n variables in the basic vector. 2. There can be at most one variable from each complementary pair in the basic vector. A complementary pair is a pair {wi,Zi}. This restriction steers us away from solutions where the complementarity constraint ziwi = 0 is not satisfied for all i. 3. The basis B corresponding to the basic vector is feasible, meaning that y = B _ 1 g > 0 (i.e., if we set all the inactive variables to zero and we solve Equation 2.36 for the active variables, we have a solution where all active variables have a nonnegative value). This restriction steers us away from solutions where the constraints wi > 0 and Z{ > 0 are not satisfied for all i. 29 The idea of the algorithm is to swap variables in and out of the basic vector in way that ensures that the above properties always hold. For every iteration, except possibly the last one, the basic vector contains ZQ, plus one variable from each of n — 1 of the n complementary pairs. Hopefully, ZQ is eventually dropped out and another variable comes in, so that the basic vector contains one variable from each of the complementary pairs. When this happens, y = B - 1 q is a solution to the LCP. Init ial iz ing t h e Bas ic Vector If all elements of q are non-negative, then the LCP is trivially solved with w = q, z = 0. Otherwise, the algorithm is initialized with ZQ equal to the negative of the minimum element of q. This way all elements of z§en + q are nonnegative. Let t be the index of the minimum element in q. To find the initial basic vector, we start with {w\, ...,wn} and replace wt with ZQ. For example, if n = 4, and q% is the minimum element of q, then our initial basic vector is {w\,W2, ZQ, 104}. Since all of the inactive variables are assumed to be zero, Equation 2.36 reduces to By V (2.37) ' 1 0 - 1 0 0 1 - 1 0 0 0 - 1 0 0 0 - 1 1 By solving this system, we see that initializing the basic vector this way always leads to a feasible basis. Solving the third row first, we get ZQ — —q^. We assumed that some the elements of q were nonnegative, and since 93 is the minimum element, it must be negative. Thus ZQ is positive. Solving for the other variables, U>2 ZQ w <?1 12 13 W 30 we have w\ = q\ — q%, wi = qi — 53, and w^ = 94 — q%. Since q$ is less than q\, q%, and 94, we are guaranteed a positive solution for w\, W2, and 104. Iterating the Algorithm Each step of Lemke's algorithm begins by choosing an entering variable that will be inserted into the basic vector. In the initial iteration, we always choose zt as this entering variable, where t is the index of the smallest element of q. We choose zt because {u)t, zt} is the only complementary pair that is left out of the basic vector. (We assume that as an initialization step, we have checked for the trivial case where all elements of q are positive. Thus at this stage we know that choosing wt as the entering variable will not lead to a solution.) The basic vector must always contain n variables, so in every iteration we must also have a dropping variable. We will choose the dropping variable so that the algorithm's invariant (number 3 in the list above) is preserved: if the dropping variable becomes inactive and the entering variable becomes active, there must still be a feasible (nonnegative) solution. We will temporarily imagine that the basic vector contains n + 1 variables so we can see how to choose the dropping variable. Let B and y be the basis and basic vector from the previous iteration. Let x be the entering variable and c be the column of ( _ e n _ M I I corresponding to the variable x. By introducing x, Equation 2.36 becomes B c ) | =q- (2-38) If x is given a nonzero value, how must the values of the variables in y change so that we still have a solution to our matrix equation? ? 31 B(B-lq) = q, B(B _ 1 q) — ex + ex = q, B{{B-lq)-(B-1c)x)+cx = q, (2-39) / \ ((B'iq) - (B^e)x Imagine the value of x is increasing from zero. As we do this, some elements of (B-1<jr) — (B~1c)x will be increasing and some will be decreasing. The dropping variable is the first variable to reach zero. So, what is the smallest value of x such that some element of (B_ 1q) — (B~lc)x is zero? This is equivalent to finding the smallest positive element of B - 1 q -=- B - 1 c , where " V is componentwise division. This is called the minimum ratio test. Note that if some element of B _ 1 c is nonpositive, then the corresponding element of (B_ 1g) — (B~1c)x will either increase or stay the same as we increase x. It will never reach zero, and is therefore not a candidate for the dropping variable. If all variables in y can be ruled out this way, then Lemke's algorithm fails to solve the LCP and we terminate the algorithm at this iteration. If we can continue, we remove the dropping variable from the basic vector, and adjust the basis matrix accordingly. If ZQ is the dropping variable, then the we have found a solution to the LCP. Otherwise, we continue to the next iteration. The next iteration's entering variable is the complement of this iteration's dropping variable. In degenerate situations, there may be a tie in the minimum ratio test. Ar-bitrarily breaking this tie may lead to infinite looping in Lemke's algorithm. To break the ties in a way that prevents looping, we use the lexico-minimum ratio test 3 2 = <?• [Mur88]. This can be summarized as follows: • When there is a tie for minimum ratio in B _ 1 q-^B _ 1 c , we look for the smallest positive element in v\ T B _ 1 C , where v\ is the first column in B - 1 . • When there is a tie for minimum ratio in v\ 4- B - 1 c , look for the smallest positive element in v% 4 B _ 1 c , where vi is the second column in B _ 1 . • And so on... A more complete discussion of the Linear Complementarity Problem and Lemke's algorithm, refer to Cottle, Pang and Stone's book on LCP's [CPS92] (the definitive LCP reference), or Murty's book [Mur88], which provides examples and intuition and a good discussion of degeneracy and maintaining lexico-feasibility 4. 2.7.2 Convert ing a Mixed L C P t o a pure L C P Sometimes a linear complementarity problem must be solved simultaneously with other equations that do not have complementarity constraints. Such systems are called Mixed LCPs. In general, a mixed LCP has the form: u J \VJ (2.40) w > 0,z > 0,wTz = 0. In this case, the variables in the vector x are unconstrained (they can be positive or negative), but the variables in z and w have complementarity constraints 4Murty's book is available online from h t t p : / / i o e . engin. umich. edu/books/murty/linear_complementarity_webbook/ 33 on them. To obtain a pure LCP, as in Equation 2.34, we must eliminate x. We assume that P is nonsingular. Rearranging the first row of Equation 2.40, we obtain X = P-\-U-QZ). (2.41) Then, substituting this into the second row, we have w + KP-1u + KP-1Qz-Sz = v. (2.42) Reorganizing the terms gives us a pure LCP in the form of Equation 2.34. w-(S-KP-1Q)z = v-KA-1u. (2.43) V v ' ' - ' M Q After solving the LCP Equation 2.43, we can use Equation 2.41 to find x. Alternatively, rather than rearranging the mixed LCP into a pure LCP, it is fairly straightforward to modify Lemke's algorithm to deal directly with the mixed LCP. The modifications are: 1. All of the unconstrained variables are in the basic vector, and can never be swapped out for another variable. 2. The unconstrained variables are allowed to have negative values. 3. The unconstrained variables do not take part in the minimum ratio test. 2.7.3 Solvable L C P s for Contact wi th Friction It is well known that that when Coulomb friction is added to the acceleration-level dynamics equations (Equation 2.33), the equations fail to have a solution in certain configurations, even for situations where there is only one contact involved. 34 Anitescu and Potra [AP97] present a time-stepping method that combines the acceleration-level LCP with an integration step for the velocities, arriving at a method having velocities and impulses as unknowns, rather than accelerations and forces. Their method is guaranteed to have a solution, regardless of the configuration and number of contacts. To discretize the system 2.33, Anitescu and Potra approximate the acceler-ation as: (vt+h ~ Vt) h (2.44) where vt and Vt+h a r e the velocities at the beginning of the current time step, and the next time step, respectively, and h is the time step size. We then arrive at the mixed LCP (°) 0 w -fM Je \Jc -Jl 0 0 -JI\ 0 0 J H Ae \ K ) — Mvt + /if (pt 0 { ° Vt ,* )^ / (2.45) a> 0,AC > 0 , a i A c = 0. Friction Constraints Coulomb friction is introduced by adding some additional forces and constraints to the LCP 2.45. In true Coulomb friction, the magnitude of the friction force must be less than fi times the magnitude of the normal force: | | / / | | < / i | | / n | | . Geometrically speaking, the contact force is restricted to lie inside of a cone. However, to fit friction into our linear algebraic framework, it helps to approximate the friction 35 cone with a polyhedral cone. We do this by choosing a set of direction vectors da-, di2 • • • din tangent to contact i. We express the frictional impulse for a contact i as: J / i A / i - ( d j i d i 2 . . . din) A / i 2 (2.46) \ A / i n / where dn,di2 • •. djn contain wrenches corresponding to equal and opposite forces on the contact points of the two contacting bodies, in directions dn,di2 • • • din . The more directions we use for each contact, the better approximation to the friction cone we can get, at the expense of having to solve a larger LCP. Note that the set dn, d;2 • •. d{n will contain vectors that are negatives of each other. This may seem redundant, but is necessary because of the nonnegativity constraints that we will place on the force multipliers when formulating the LCP. The set of contact impulses lying within this polyhedral friction cone is then {j£ACi + l \ \ u | ACi > 0, \ h > 0, ef\fi < ^AcJ , where e* = [1,1,..., 1]T, and yn is the coefficient of friction. The complementarity constraints for each contact are as follows: (2.47) (2.48) 7;ej + Jjvv > 0 complementary to Ay\ > 0, Hi\Ci — e.{ \ft > 0 complementary to 7i > 0. If the normal impulse magnitude XCi is zero, then the friction impulse must also be zero (A^ = 0), or the constraint /iiA^ — efXf{ > 0 would be violated. 36 Whenever the normal impulse magnitude is positive, the variable 7^ ties the two friction constraints to each other. When there is no relative tangential motion at the contact, we have ji = 0, which allows the sum of the magnitudes of the friction impulses (efXfJ to be less than fii\Ci- However, if there is some relative tangential motion at the contact, then 7; will be positive (it will be equal to the negative of the minimum element in J/ ;v). Having 7; > 0 forces m\Ci = efX^, meaning that the friction impulse is at its maximum possible magnitude. To summarize, • if the contact impulse lies inside the friction cone (but not on its surface), then the bodies are not exhibiting relative tangential motion. • If the bodies are in relative tangential motion, then the contact impulse lies on the surface of the friction cone, and exhibits negative work. • As the approximation of the friction cone becomes closer to the ideal circu-lar friction cone, the friction becomes closer to directly opposing the relative tangential motion. These friction conditions are described more fully in [Ani97].5 Extending Equation 2.45 to include these friction constraints, we get the following linear complementarity problem 5However, we use different variable names here than Anitescu does 37 (°) 0 a a w / -V M Je Jc Jf 0 -Jl 0 0 0 0 -Jl 0 0 0 ^ -J] o 0 0 0 0 0 E -ET 0 Ac A/ V 7 / M\it + hf 0 0 0 0 (2.49) L\ V) Ml > 0 , where fj, /A C \ A/ and E > 0 , L\ \(J /A C \ A/ \ 7 / 0, Mn/ e i 2.7.4 Implicit M e t h o d for Stiff Sys tems In a recent technical report, Anitescu and Potra [APOO] published an extension to the above, which we implemented in our system. Rather than using an explicit time-step in Equation 2.49, they suggest a linearly implicit method. This required only small modifications, yet the human animations we present in Chapter 5 would not have been possible without making this change (the feedback forces we used make the differential equations stiff, necessitating implicit integration) . The main requirement of this implicit method is that we must calculate the gradients of the stiff forces with respect to changes in the position and velocity of the rigid bodies. To derive the linearly implicit method, we start with a backward Euler dis-38 cretization of rigid body dynamics equation: M(vt+h)(Vt+h~Vt)=f(pt+h,vt+h,t + h). (2.50) The difficulty in solving this is that the force vector f is not known unless the position and velocity vectors p t + / l and Vt+h are known. In general, to apply backward Euler method to a nonlinear system, one must apply several Newton-Raphson iterations until a solution is found. We make a linear approximation using only the first Newton-Raphson iteration (hence the term "linearly implicit") that f(Pt+h,*t+h,t + h) « (2.51) f (Pt, vt, t) + Vpf h vt+h + V„f (vt+h - v t), where Vpf and V„f are the gradients of the function f with respect to change in position and velocity, respectively, and evaluated at (pt,vt,t). If we substitute this into Equation 2.50 and move all of the terms with \it+h to the left hand side, we obtain (M - h2Vpf - hVvf)vt+h = (2.52) Mvt - hVJvt + h f (p t, vt, t). It is convenient to use the notation M = M-h2Vpf-hVvf, (2.53) and f = f ( p t , v t ) t ) - V „ f v t , (2.54) so that the linearly implicit equation for forward dynamics closely resembles the time-stepping dynamics equations we have seen already, such as Equation 2.49. The implicit version is: 39 (2.55) vt+h | _ f -Mvt - hf x)~{ 0 In our implementation, we estimate the force gradients Vpf and V„f numer-ically, by evaluating f for several position and velocity values in the neighborhood of the current state of the system. 40 Chapter 3 Stabilization For Simulation With Contact 3.1 Overview of Stabilization Figure 3.1: An unstabilized simulation of a swinging pendulum. Figure 3.1 is an example situation that motivates the need for stabilization. 11 The figure shows a simple two-dimensional simulation of a swinging pendulum: one rigid body with one constraint. The only external force is that of gravity. The constraint is a hinge joint constraint that anchors point A to a fixed point. Over time, however, point A actually drifts, due to numerical integration errors. The centre of mass, rather than staying on the circular path that it is supposedly constrained to (shown with a dashed line), continues to fall further from that path as time progresses. We can lessen this problem by using higher order, more accurate integration scheme (the ones used here are only first-order accurate1), or by using smaller time steps. Even with first-order methods, the local error is proportional to the square of the step size, so decreasing the step size to one tenth effectively decreases the local error to one hundredth. However, even using the best integration methods and small time steps, numerical drift can never be completely eliminated. Furthermore, it may not always be feasible to use the more complicated integrators or decrease the step size and still achieve acceptable performance. Even if we cannot achieve very high numerical accuracy, we still wish to have simulations that are plausible - ones where the constraints are always met. This section describes methods for counteracting constraint drift using stabilization. Before continuing, we need to define our problem more clearly. Stability is a word that is used with a variety of meanings in different contexts in the differential equation literature. By the word stabilization here, we mean stabilization of an ODE with respect to an invariant set. We shall now explain what this means. The position constraint equation coupled with the constrained Newton-Euler equations (see Equation 2.32) together are an example of a differential-algebraic 1see [AP98] for a precise definition of the accuracy of an integration scheme. 42 equation (DAE). This is a term for a system of equations containing both differential equations and algebraic equations. DAEs are a relatively new area of research, and the methods for solving them directly are somewhat difficult. The usual approach is to convert the DAE into an equivalent ordinary differential equation (ODE) tha t can be solved by conventional techniques. In our case, we can do this by differen-tiating the position constraint equations twice, to arrive at a constraint in terms of accelerations, then substituting this acceleration constraint into the Newton-Euler equations. In doing this, we arrive at an ODE. The approach we use is actually slightly different, as we have described in Section 2.7.3.2 The penalty of this approach is that we lose the constraints on the position and velocity. Upon discretization of the ODE, we introduce numerical errors, and we will experience drift away from the constraint manifold. The constraint manifold (also known as the invariant set) is the subset of the state space in which the constraints are satisfied. To counteract this, we must stabilize the ODE with respect to the invariant set. In other words, we must alter the ODE so tha t it has the same solutions as the original whenever g(p) = 0, but whenever g(p) ^ 0 the solutions is at t racted towards the invariant set. In this chapter we will introduce two methods of stabilization, in the con-text of rigid body simulation. First we will discuss Baumgarte stabilization, a method that is very popular because of its simplicity. We will then introduce Post-Stabilization, which is based on the work of Ascher et al. [Asc97] [ACR94] 2We use time-stepping methods, where a numerical integration step is built into the system of equations we solve, and the equations are given in terms of velocities and impulses, rather than forces and accelerations. 43 3.2 Baumgarte Stabilization Baumgarte's stabilization technique [Bau72] is one of the most familiar and com-monly used methods, because of its simplicity. The idea here is to replace the acceleration constraint equation g = J\i + k = 0 with some a linear combination of the acceleration, velocity and position constraint equations: 0 = g + ag + (3g, (3.1) which creates a more stable ODE. If the velocity and position constraints are satisfied, the last two terms on the right hand side vanish, and we are left with the original acceleration constraint equation. A physical interpretation of this method is that we are adding in additional correction forces, proportional to the error in the velocity and position constraints, to counteract drift. Because our implementation uses only velocity-level constraints rather than acceleration constraints (see Section 2.7.3), we use an even simpler version of Baum-garte stabilization where we replace the velocity constraint g — J\i = 0 with g + ag = JM + ag = 0. The main difficulty of using Baumgarte stabilization is that it is not always easy to find an appropriate value for the constants a and (3. 3.3 Post-Stabilization Another approach to stabilization is to follow each integration step with a stabiliza-tion step. The stabilization step takes the result of the integration step as input, and gives a correction so that the end result is closer to the constraint manifold. Post-stabilization methods are discussed in detail and compared to other stabi-44 lization methods by Ascher et al. [ACR94]. Here we give one interpretation of post-stabilization that fits into Ascher's broader definition. Let p be the position of a set of rigid bodies after the integration step. Let g be the constraint function (see Section 2.5.2). In general, due to numerical drift, gi(p) 7^ 0. Let G = -M. In our stabilization step, we wish to find some dp such that g(p + dp) = 0. Assuming dp will be small, we can make the approximation that g (p + dp)«<7(p) + G(p)dp. (3.2) Rearranging this, we see that the stabilization term dp should satisfy Gdp = - f f ( p ) . (3.3) In general, G is not square, so G _ 1 does not exist. One way to solve for dp is to use the pseudoinverse of G: dp = - ( G r ( G G T ) - 1 ) f f ( P ) . (3.4) This idea works fine as long as G G T is nonsingular, which is usually the case for a system that contains only equality constraints. However, we often run into singularities when contact constraints are involved. This is because the colli-sion detector may find many contact points between a pair of objects, leading to constraints that are redundant. One approach to deal with singularities is to use a pseudoinverse formula based on singular value decomposition of G. By truncating the small (nearly zero) singular values, we can find a pseudoinverse of G even when G G T is singular. Using a singular value decomposition in each time step, however, would be prohibitively expensive, and it does not take the inequality constraints involved 45 with contact into account. Instead, we find it natural to pose the post-stabilization problem as an LCP, just as we do with the dynamics equations. We explain this method in the next section. 3.4 Fitting Post-Stabilization into Our Dynamics LCP Framework As mentioned above, in the absence of contact constraints, we can find the stabiliza-tion term using the pseudoinverse of G(p) (Equation 3.4). Doing so is equivalent to solving the system -f1 -G("1H = H . p., \G(p) 0 J \XJ \g(p)) In other words, we can express the problem of finding the post-stabilization step in terms as a problem of finding Lagrange multipliers, just as we did with the dynamics equations (see Equation 2.32). We can make 3.5 look more like Equation 2.32 by doing two things: 1. Replace G(p) with J(p). These two matrices are both constraint Jacobians. The difference is that G multiplies with changes in position (7-vectors con-sisting of a translation, plus a quaternion), whereas J multiplies with twists, which are 6-vectors. It makes more sense to use J because we would already have code to compute it from our implementation of the dynamics compu-tations. As a result of using J, the post-stabilization step dp would be a twist and needs to be converted back into our position representation, using Equation 2.9.3 3Note that in two-dimensional rigid body simulations, G and J are identical, so this 46 2. Replace the identity matrix with the mass matrix M.. By doing so, we are no longer using the pseudoinverse G T ( G G T ) _ 1 , but a weighted pseudoinverse, M~1GT(GM~lGT)~1. This corresponds to favouring the position change that requires the least amount of energy. This way, for example, a rotation around an axis with low angular inertia would be favoured over a rotation around an axis with high angular inertia. Making these two changes gives us M -J(„f\ M ( 0 , p 6 ) J(p) 0 J \XJ \g(p)' Similar to our dynamics equations for systems with contact, it is natural to place complementarity constraints on the variables having to do with the contact constraints. • The post-step should never pull contacting bodies towards each other at the contact points, only push them apart: Ac > 0. • If contact constraint functions were evaluated after adding the post step, the result must not be negative, but may be positive: g+ = (g~ + J^dp) > 0 (we use superscripts "-" and "+" to denote "before" and "after" the post-step. That is, g~ = g(p), and g+ approximates g(p + dp)). • ^c9c = Q- This constraint roughly means that for each contact i, either we are pushing the bodies apart at i or contact i's constraint will be satisfied in the absence of any push at i. somewhat confusing distinction can be ignored. 47 Adding the stabilization for contact constraints, we have the LCP 0 \9tj ( M Je \Jc -JJ 0 0 0 0 JT\ /A„\ J dp A, 9'e \9c) (3.7) 9 + > 0 , A c > 0 , A ^ 0. 48 Chapter 4 System Architecture We have implemented a rigid body simulation system using Java. We chose Java for its extensive freely available libraries (including the linear algebra package Colt [Hos], which we used heavily), its support for multi-threading, its cross-platform compatibility, support for object-oriented programming, and ease of use. Our im-plementation goal was to build a general purpose, extensible, interactive simulation system that could handle simulations of constrained rigid bodies with contact. Writ-ing this software was no easy task. Similar systems have been developed by others, both for research and for commercial products in the entertainment industry. How-ever, source codes are generally not available to the public, and it is difficult to find material that goes into the specifics of implementing a general purpose rigid body simulator. A SIGGRAPH tutorial by Baraff [Bar97] provides some details, as does Cremer's thesis [Cre89]. In this chapter we will try to explain the important im-plementation details of our simulator, so that others will not have to feel as though they are reinventing the wheel, as the author sometimes did. 49 4.1 System Overview Figure 4.1 shows a flow chart of the main loop of the simulator. We describe the steps here in more detail. 1. The time step begins with the rigid bodies in position p(t) and moving with velocity v(i). We have a set of equality constraints, and a set of contact constraints. 2. We total up the external forces by calling the apply method of each ExternalForce object. 3. We calculate the Jacobian matrices and the mass matrix based on the current position p(t). 4. We solve the forward dynamics LCP to determine the velocity for the next time step, v(t + h). 5. We use a numerical integrator to find the next position p(t + h). (Higher-order numerical methods may actually solve the LCP in step 4 several times). 6. We pass the position vector p(t + h) to the collision detector. This position vector may represent a state where there are interpenetrations between bodies. We assume that at the beginning of the time step there were no interpene-trations, and therefore if we interpolate the positions between the beginning and end of the time step, we will find a position free of interpenetration. We repeatedly cut the step size in half until we find such a state. The revised step size is dt. 50 7. The collision detector returns a set of contacts. We create new contact con-straints from these contacts. 8. We update the Jacobians based on this new position vector p(i + dt). 9. We calculate the constraint functions for all of the constraints. This is input to the post-stabilization. After taking our post-step, we have some revised position vector p r , for which the constraints are met. We then progress on to the next time step. 51 numerical integrator (e.g. Euler's) X new position with possible interpenetration p(t+h) contact: - pointers to contacting bodies - coordinates of contact points on both bodies - normal vector "new position withou interpentration P(t+dt) constraint functions are evaluated stabilization post-step Figure 4.1: Overview of the simulator. 52 4.2 Accommodating both 2D and 3D Simulations Although three dimensional simulations are clearly better than two dimensional ones in terms of realism, there are good reasons for wanting to simulate 2D rigid body motion. In certain situations, the motion in one dimension may be unimportant. For instance, in a head-on car crash simulation, we may choose to ignore the left-right motion of the crash dummy, and just simulate the crash in 2D using a side-view of the dummy. In a simulation of an air-hockey game, we could probably neglect the height dimension and just use a 2D bird's-eye view of the table. Two dimensional simulations are very useful from a software development point of view. We often found it much easier to understand the bugs and problems of our system by testing it on 2D examples. There are a couple of reasons for this. The first reason is that it is 2D simulations are much more easily visualized than 3D simulations. Debugging 3D simulations involves moving the viewpoint around a lot so that the area of interest becomes clearer. For example, in 3D it is difficult to visually tell whether two objects are separated, in contact, or in interpenetration. If two objects are close to each other, the only way to get an idea of how far apart they are is to manoeuver the camera so that the crack between them is visible. In 2D, there is no such problem. The other benefit of 2D simulations for debugging is that the matrices and vectors involved are much smaller, and therefore easier to verify by hand, if necessary. One of the challenges of writing a piece of software like this is information overload on the programmer during debugging sessions, and anything that can be done to alleviate this is usually welcome. Fortunately, it is easy to allow both 2D and 3D rigid body simulations within the same code, as long as care is taken not to hard-code any assumptions about the length of position and velocity vectors, or the width of mass and Jacobian matrices. 53 4.3 Keeping Track of Simulation State Most of the state information needed during the simulation is contained within a single RigidBodySet object, which keeps track of the following values: • The positions and velocities of all rigid bodies in the set • The mass-inertia matrix M for the set of rigid bodies • The Jacobian Je for the equality constraints affecting the system • The Jacobian Jc for the contact constraints, and Jf for the friction constraints. • The matrices E and /x, also used in friction computations • Pointers to all Constraint objects • Pointers to all RigidBody objects The RigidBodySet class encapsulates all of the matrix and vector values needed to solve the forward dynamics problem (see Equation 2.49). Although we could have distributed this information among many objects (for instance, keeping each body's position in a separate RigidBody object), this centralized approach is more efficient because it reduces the amount of copying needed when formulating the dynamics problem. Conveniently, our matrix package allows us to create sub-range views of matrices and vectors, so that the information pertaining to individual RigidBody and Constraint objects can be mirrored inside those objects without creating a separate copy of the data. Figure 4.2 is a visual representation of some of our data structures. In this instance, there are three rigid bodies connected into a chain. The arrows in this diagram represent pointers, and the boxes are vectors and matrices. 54 4.4 Implementing Constraints In Section 2.5.2, we described how constraints are represented mathematically. Re-call that a constraint on the positions of rigid bodies is represented by a constraint function g(p), and velocity constraints are given as linear equations of the form J(p) v = 0. Thus, every constraint object must implement methods to calculate g(p) and J(p). 4.4.1 Joint Constraints Constraint Equations for a Ball Joint A ball joint (sometimes called a "ball-and-socket" joint) constrains a point pa on body A and a point pb on body B to lie at the same location. The constraint function g{p) should then return zero if and only if pa and pb are at the same place. The obvious choice then, is to have g return the vector from one point to the other: g(p) = pa-Pb- (4.1) Recall (from Equation 2.21) that the velocity of a point p attached to a rigid body is given by p = [V]p = M Px Py Pz v 1 / (4.2) where V = is the body's velocity given in twist coordinates. The easiest w frame to calculate the Jacobian in is a frame having its origin at the location of the 55 joint. Let us call this frame the "joint frame". If 3Va is the velocity of body A given as twist coordinates in the joint frame, and 3Vb is the velocity of body B, then the constraint is simply: j J a Va+j^b Vb /o 0 0 1 o o\ 0 0 0 0 1 0 \0 0 0 0 0 \J U>n V Vaj + A) 0 0 - 1 0 0 ^ 0 0 0 0 - 1 0 \0 0 0 0 0 - 1 / Ub Vb = 0. (4.3) Since our implementation uses a velocity representation where the linear velocity component is the velocity of the centre of mass, we must transform the Jacobians so that they are expressed in frames with their origins at the bodies' centres of mass. We do this by expressing the above equation in terms of these frames. Let frame A be a frame at the centre of mass of body A, and frame B be a frame at the centre of mass of body B. Both of these frames are rotated to align with the world frame, and for simplicity we will assume that the joint frame J is also aligned with the world frame (the rotation of frame J does not affect Equation 4.3). Equation 4.3 becomes: 3 J a J a A d X + 3 - J i i A d V 6 = 0. (4.4) From this, we can see that the coordinates of Jacobian J a in frame A are aJa = j J a i A d = ( o I 3 ) V J \{Va] I3/ [ra] Is \ r°-y fay rax 0 1 0 0 1 0 0 (4.5) 0 0 1 56 The vector ra is the vector from the joint to the centre of mass of body A. Doing a similar transformation to b J;, gives ( 0 -rhz rby - 1 0 0 ^ bJfc = >Jfc (,Ad \~rby 0 -nx o - l 0 0 0 (4.6) V Hinge Joints A hinge joint is a one degree of freedom joint, meaning that it constrains five degrees of freedom. It can be seen as an extension of the ball joint, just with two extra constraints added to ensure that the bodies can only rotate relative to each other around the axis of rotation. For example, if the axis of rotation of the joint is the X axis of frame J, and 3ua and Jcjb are the angular velocities of bodies A and B given in world coordinates, then the constraints are W - W = 0, Jujaz - °ubz = 0. (4.7) Appending these constraints on to the ball joint constraint equation (4.3), gives us the hinge joint constraint equation: jJa Va+j2b Vb = A) 1 0 0 0 0^ 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 \ 0 0 0 0 0 1/ un + vaJ 0 - 1 0 0 0 0 0 0 - 1 0 0 0 0 0 0 - 1 0 0 0 0 0 0 - 1 0 \0 0 0 0 0 -I) OJn Vn (4, 57 In terms of our twists aVa and Vj, that we use in our implementation, the above constraint is equivalent to the constraint / « Jx 3y 3z 0 0 o\ akx aky akz 0 0 0 0 rn. 0 \ r " y ray 1 0 0 0 1 0 0 0 0 1/ / K fcT Un + h 0 0 \ -bky -bkz 0 0 0 0 nz o \-ny n. nz n. - 1 0 0 -rbx 0 - 1 0 o o o - i y Ub Vb (4.9) where j and k are two mutually orthogonal vectors that are also orthogonal to the hinge joint's axis of rotation. We will call these vectors the forbidden axes, because the bodies are forbidden to rotate with respect to each other around these axes. Universal Joint A joint with two degrees of rotational freedom is sometimes called a universal joint. Universal joints have two axes of rotation, one which is fixed to a first body, and another which is fixed to a second body. The two axes of rotation are usually or-thogonal to each other. The Jacobian of a universal joint is very similar to the hinge joint's Jacobian, except it has only four rows because there is only one forbidden axis about which the relative rotation of the two bodies is disallowed. Constraint Functions for Hinge Joints and Universal Joints Since hinge joints and universal joints are extensions of ball joints, the first three rows of the Jacobian are the same between these three types of joints. Correspondingly, 58 three of the elements in the vector returned by the constraint function g(p) should be the same as the ball joint's constraint function, given in Equation 4.1. The remaining elements of the constraint function are measures of rotational error. In the universal joint, there is just one forbidden axis, so there is one remaining element of g(p) that returns the relative rotation (in radians) around that forbidden axis. We calculate this by decomposing the relative rotation R ^ R ; , (where R a is the rotation matrix for body A, and R(, is the rotation matrix for body B) into three rotations: R ^ R b = rot(v3,j) x rot(v2,(3) x rot(vi,a), (4-10) where v\ and vi are the rotation axes, and v% is the forbidden axis (the coordinates of these axes are given in body A's coordinate frame here). Then 7 is our rotational error measure that we return as the one remaining element of g(p). We use a similar method for hinge joints, except that there are two forbidden axes and only one rotation axis. Then in Equation 4.10, v\ is the rotation axis, and we return (3 and 7 as the two rotational elements of the constraint function g(p). When defining the Jacobian of a constraint, we said that -^ = Jv. Note that when using the constraint function described here and the constraint Jacobians described above for the hinge joint and universal joint, it is not exactly true in general that -^ = Jv, but it is a good approximation when the constraint's rotational error is small. We could derive a more exact J, but the derivation is rather complicated to go into here, and we find that the Jacobians we gave work fine in practice. One question that comes up when implementing this is how to deal with constraint error in the initial configuration of the bodies. For example, let's say that bodies A and B are constrained by a hinge joint so that they can only rotate relative 59 to each other about the Z-axis of body A. What if, in the initial configuration given by the user, the relative rotation R ^ R ; , is some rotation about the F-axis? There are two ways we can deal with this: 1. We can decompose R " 1 ^ as in Equation 4.10. Doing this, the initial configu-ration will in violation of the constraint, and the post-stabilization will correct for it so that the relative rotation between the two bodies is purely around the Z-axis. 2. We can assume that the initial rotations of the bodies (which we will call Ra o and Rt0) do not violate the rotational constraints, and interpret the rotation constraint to mean that the relative rotation R ^ R j , must be of the form rotz(9) x ( R ^ R ^ ) , where rotz{9) is the matrix for a rotation of theta about the Z-axis. In this method, rather than decomposing R~1R;) into three angles, we decompose (R~1R(,)(R~01Rfo0)_1. This matrix can be seen as the change in relative rotation between the initial configuration and the current configuration. We have implemented the constraint functions using the latter method, be-cause it allows the user to have more flexibility in specifying the initial rotations of the objects. 4.4.2 Contact Constraints In Section 2.6, we defined a contact as a pair of contact points, one point attached to one rigid body, and the other point attached to another rigid body, and the separation distance of a contact is the normal component of the distance between the two contact points. The constraint function g(p) for a contact constraint returns 60 the separation distance, minus e: g(p) = n-(xa- xb) - e, (4.11) where n is the contact normal, xa and xb are the contact points on bodies A and B, respectively, and e is the contact tolerance. Thus, if the separation distance is less than the contact tolerance, the constraint function returns a negative number, indicating that something is wrong. To derive the Jacobian of the contact constraint, we will make use of three coordinate frames. W is the "world" coordinate frame. A and B are world-aligned frames with origins at the centres of mass of the two bodies. Because the axes of all three of these coordinate frames are aligned, a 3-vector can be transformed from frame to frame without changing the coordinates: w . a . *Ea ~ 3Ca: (4.12) w . b . xb = xb. The contact point locations are given by xa = Pa i fai (4.13) Xb = Pb + rb-By assuming the contact points stay fixed with respect to the bodies, and that the contact normal stays fixed, we can approximate the rate of change of the separation distance g (and thus derive the Jacobian of the contact constraint) by applying the Equation 2.8 to both of the contact points1: 1The normal, in general, also changes . We currently do not take this into account. See [Can86] for a derivation of n 61 —g(p) fan-(xa- xb), = n • ( xa - xb) w /a . b . \ = n • ( xa - xb), = wn • ((ava + [au>a]axa) - ( vb + [ujb] xb)), = wn • ((ava - [axa]aua) -(vb-[xb] u>b)), a w T a w Tro na \ /w T* b w T fb -ib \\ n va- n [xa] ua ) - ( n vb - n [ xb\ u>b)), Xa\ n) U>a = (- xax ny u>„ ( xa x n ) J n J \ . \ wn • \ v " / + I — ( xb x n)^ — n1 b b vb a** a- b^b (4.14) The Jacobians of the individual contact constraints are each only one row, representing the fact that each contact constraint removes only one degree of free-dom. In each time step of the simulation, after running collisions detection, we create a single matrix Jc, where each row of Jc is the Jacobian matrix of one contact. Resting Contact vs. Impact Resting contacts and impact contacts differ in that with resting contact, the relative normal velocity at the contact is close to zero, while in the case of impacts, the bodies are moving towards each other, so the relative normal velocity will be negative. Most objects have a certain amount of bounciness, so we expect to see the objects move apart after an impact, whereas there is no bounce involved in resting contact. Simulating impact in an acceleration-level dynamics system (see Equation 2.33) is 62 tricky because simpler models of impacts usually involve impulsive forces - infinite accelerations over an infinitesimal amount of time. However, since we use a time-stepping method (Equation 2.49) where velocities and impulses are the variables, our implementation can naturally handle both resting contact and impact using the same model. We use the very simple Newton impact model: v — ~^restitution v > V*-*-") where v~ is the relative velocity of the two bodies before impact, v+ is the relative velocity after impact, and kresutution is the coefficient of restitution. Although this model is typically used to describe single contact collisions between particles, we apply it to rigid body dynamics by insisting that for each contact, the separation velocity of the contact after impact is greater than or equal to —krestitution times the separation velocity of the contact before impact, leading to the following velocity constraint equation: estitution I J a + Jfc "• > 0 . (4.16) JV c Admittedly, this impact model is a bit of an abuse of physics. The Newton impact model is not intended to model collisions involving multiple contacts, and the impact law we use here has been proven to be physically incorrect. In particular, the Newton impact model has been shown to generate energy under certain types of frictional collisions [Str91]. More theoretically sound impact models have been proposed (see Stronge [Str91], and Ullrich [U1198]), but even very complicated models are still inaccurate at predicting real impacts. 63 Despite the simplicity of our impact model, we were satisfied that it produced plausible-looking impacts and we did not feel there was much to gain by exploring other models. However, better impact models would certainly be necessary if the simulator was being used in an application where the computed impact responses were expected to be a good prediction of real-world behaviour (e.g., simulating crash tests of cars to improve their design). Friction Jacobians The friction Jacobian Jy of a contact is similar in nature to the contact constraint Jacobian Jc: • Jcv gives the relative normal velocities at each of the contacts. J/V gives the relative tangential velocities at each of the contacts. • The column of J;T is a wrench that corresponds to a force being applied at the contact, in the normal direction. The columns of JT give wrenches for forces applied tangentially at the contact. Thus, Jc and Jj have a mathematically similar form. As we saw above, for an individual contact constraint, the two Jacobians are Jca = ((axa x wn)T wnT) , Jab = ((bxb x wn)T v ) • (4.17) The friction Jacobians are 64 aJ/a — ( xa x di ) J df l ( x a x da) d\ \{ xa x d^)-1 d n y ( xb x d i ) J df » ( xb x da)-1 d^ fcJ/6 W J ^T » J T , (4.18) where {di, d2, ...dn} is a set of directions that are tangential to the surface. 4.5 Collision Detection Although collision detection is a very important part of any rigid body simulation, our research has not focused on the collision detection problem. Instead, we have essentially viewed the collision detector as a black box, so our software is not tied to any particular collision detection system. However, some collision detectors are more well-suited to our purposes than others. In particular, many collision detection packages only return the closest pair of points between the two objects, or just the distance between them. This is insufficient for modelling contact between two planes that are resting on each other. Ideally, we want the collision detector to return enough contacts so that plane-plane and edge-plane contacts can be modelled properly. The collision detector that we use (a sphere-tree based collision detector, like [Qui94]) returns contacts for all of the following situations: • The distance between a vertex on object A and a face on object B is less than e. The distance between a vertex on object B and a face on object A is less than e. 65 • The distance between an edge on object A and an edge on object B is less than e. • The distance between a vertex on object A and an edge on object B is less than e. The variable e is the contact tolerance. Each one of these situations results in a Contact object being created. The contact object contains pointers to the two rigid bodies that are in contact, coordinates of the contact points on body A and B in body-relative coordinates, a normal vector, and the separation distance at that contact. This information is used to generate a contact constraint. One problem with returning contacts for all of the situations mentioned above is that we may get far more contacts than we need. For example, imagine a simu-lation of a small, highly tesselated spherical ball being hit by a bat. Since the ball and bat are both convex shapes, we really only need one contact point to simulate the contact event, but it is likely that many contacts will be detected. The problem is worsened if the contact tolerance e is large. Having a large number of contacts will cause the LCP solver to run slowly and the simulation to bog down. This is a difficult problem to deal with, and one that remains unsolved in our system. 4.6 External Forces Things like gravity, springs or muscles between bodies, motors, etc. are implemented as external forces. External forces in general are a function of the position and velocity of the bodies they are attached to. The ExternalForce object implements this function. In addition to this, some ExternalForce objects also implement functions that return the gradient of the force with respect to changes in position 66 and velocity. This information is needed for our implicit time-stepping method (see Section 2.7.4). This is important in external forces that change quickly for small changes in position or velocity. For example, when two rigid bodies are attached to each other by a stiff spring, the spring will resist being extended by a small amount with a large force. 4.7 Interacting with the Simulation User interaction in the system was accomplished in two ways. The first is through a mouse-controlled attractor-repulsor external force object. The object was repre-sented on the screen as a small spherical cursor that could repositioned by moving the mouse. Motion was normally in the same plane as the ground, but one could move the sphere vertically by dragging with the middle mouse button. Holding the left mouse button down caused the sphere to emit a repulsive force that pushed ob-jects away. The right mouse button would cause an attractive force. The magnitudes of these forces would increase as the sphere got closer to a rigid body. Another way we allowed the user to interact was by allowing them to throw a rigid ball into the scene. Again, the user would use the mouse to control this. When the user clicked, the ball would be thrown along the ray from the viewpoint, through the pixel on the near-plane that the user clicked on. 67 4.8 LCP Solver Implementation 4.8 .1 E x p l o i t i n g C o h e r e n c e B e t w e e n Bas i s M a t r i x of C o n s e c u t i v e I t e r a t i o n s For the most part, implementing Lemke's algorithm is straightforward. One point that may not be so obvious is how to avoid recomputing the inverse of the basis matrix from scratch in each iteration of the algorithm. Recall from our description of Lemke's algorithm (Section 2.7.1) that in each iteration, we remove one variable from the basic vector and add another variable. This corresponds to changing one row in the basis. Fortunately, there are ways to take advantage of the fact that only one row is changing in this matrix, meaning we do not have to run an expensive 0(n 3) matrix inversion in every iteration. To exploit this coherence between iterations, we use the Sherman-Morrison-Woodbury (SMW) formula [GvL89]. This formula gives an expression for the inverse of a matrix after undergoing some change, given the inverse of the original matrix. The general form of the SMW formula takes a matrix Ao and its inverse AQ~ as input, and returns the inverse of matrix As = Ao + R S T . The formula is: A ; 1 ^ A ^ - A ^ R p + S ^ A ^ R T ^ A o 1 . (4.19) In the case we are interested in, As and Ao differ only in one column, which we will call column i. We will say that v0id is the z'th column of Ao, and vnew is the i'th column of As. Then let An = vnew — v0id. The expression relating As to AQ is then 68 A, = A0 + Av fo 0 . . . 1 . . . 0 Oj • (4-20) only the i'th element is one, rest are zero If we use Av as R in Equation 4.19, and (o 0 . . . 1 . . . 0 0 ) a s ST , then S T A Q l is simply the i'th row of A^"1 (we'll use the symbol bT to denote this row vector). Then the SMW formula simplifies to A 7 1 = A 0 - i - ( ^ ^ - ) 6 r . (4.21) 0 \l + bTAvJ y ' Using this formula, the inverse of the basis can be updated in 0(n2) time in each iteration of Lemke's algorithm, rather than the 0(n3) time that a general matrix inversion would take. 4.8.2 A General izat ion of Baraff's Technique for Incorporat ing the Auxi l iary Constraints into a Linear T i m e Lagrange Multipl ier Approach In David Baraff's paper Linear-Time Dynamics Using Lagrange Multipliers [Bar96], he shows an approach for exploiting the sparseness when solving for the Lagrange multipliers in the constrained dynamics equations. One section of this paper deals with how to incorporate "auxiliary" constraints, such as contact constraints, which must be solved externally using an LCP solver. It is interesting to note that Baraff's technique can be generalized to any mixed LCP system, and the notation actually becomes much simpler in this generalization. The generalized version is as follows. Let us assume that the matrix P in the mixed LCP 2.40 is sparse, and we know of an algorithm sparsef actor to factor it in linear time, as well as an algorithm sparsesolve to solve a system Pa; = b in linear time. In this case, it 69 would be wasteful to explicitly invert P when using Equation 2.43 to LCP to a pure LCP. Instead, we can save effort by using our sparse solution algorithms to find P - 1 i i and P _ 1 Q without computing P _ 1 itself. The steps to the algorithm are: 1. Run sparse factor to factor P . 2. Run sparsesolve to find xbase such that -Pxbase = u. Then xbase = P _ 1 u . This xhase vector is the solution to the unconstrained variables x of the LCP, in absence of the constrained variables z. 3. Next we compute the matrix P _ 1 Q , which tells us how changes in z affect x. Let b\, &2) • • • > bk be the columns of the matrix Q. We can use sparsesolve k times to individually calculate the vectors P~lbi,P~1b2,..., P-1bfc. Con-catenating all these into one matrix gives P _ 1 Q . 4. P _ 1 w and P _ 1 Q can now be plugged into Equation 2.43. We solve this pure LCP using Lemke's algorithm to determine the value for z. 5. We run sparsesolve once more to calculate x, using the relationship Px = ( - u - Q z ) . 4.8.3 Numerical Difficulties We Encountered with Lemke's Algo-rithm We found implementing Lemke's algorithm to be a bit troublesome due to unex-pected numerical problems. The problems mainly had to do with the minimum ratio test, or the lexico-minimum ratio test. To recall, the minimum ratio test is where we search for the minimum positive element of B _ 1 q -=- B _ 1 c , where B is the basis matrix, c is the basis column of the entering variable, and q is one of the 70 original inputs to the problem. As one of the algorithm invariants, the elements in the vector B - 1 q are always all non-negative. Thus, if we are looking for the minimum positive element of B _ 1 q -r B _ 1 c , we can exclude all ratios i for which (B_ 1c)j < 0. It turns out that this is not quite sufficient. Due to numerical error, It can happen that there is some i for which both (B_ 1q)i and (B_ 1c)i are extremely small positive numbers, and the ratio between them is smaller than any of the other ratios. In this case (which happened to us surprisingly often), this i is mistakenly chosen by the minimum ratio test. We have addressed this problem by replacing the test (B - 1c) i < 0 with (B_ 1c)i < e for some tiny e that is large with respect to precision of the floating-point calculations. Another issue is that of ties in the minimum ratio test. As we discussed in Section 2.7.1, these ties can be resolved by using the lexico-minimum ratio test. However, sometimes in the presence of numerical error, there is a "near-tie", where the tying ratios differ by some very small amount. In this case, should we break the tie in favour of the minimum ratio, or pretend that the ratios are equal and move on to the next level of the lexico-minimum ratio test? The answer to this is not clear to us. 71 Figure 4.2: An example showing the relationship between an instance of RigidBodySet and several of RigidBody objects and Constraint objects. The p, v, M and Je variables of the RigidBodySet point to large vectors and matrices, while the p, v, M and Je variables of the RigidBodys and Constraints point to smaller sub-range views of these large matrices. 72 Chapter 5 Results and Discussion This chapter presents the results of our work. Two of the goals of our simulation software were to be extensible, and to run at interactive rates. To demonstrate that these goals were met, we show an application of our software to interactive human character animation. We also present some results from experiments that test the post-stabilization method discussed in Chapter 3. 5.1 An Application: Transforming Motion-Capture Data In this section, we describe an application of our rigid body simulator for animation of human characters in interactive virtual environments. We will explain some extensions we made to the simulator so that motion capture data could be used to drive physical simulations of humans which would react in physically plausible ways to external physical stimuli. Motion capture is an increasingly popular method for creating complex an-imations, especially for human motions. This involves recording kinematic infor-mation, usually as a set of joint angles sampled at regular time intervals. Using 73 kinematic data to directly animate the character is problematic in an interactive simulation such as a sports video game, because it is difficult to make it look like the character is really interacting with its environment. Physical constraints do not affect the character when the motions are just playbacks of the kinematic data. In situations where these physical constraints are important, we are better off using a dynamic physical model of the character and driving the model with forces that are derived from the motion capture data. We use a biologically-based motor control model to drive the character. We discuss the biomechanics background more extensively in a technical report [CYP02]. To summarize our approach, we model the character's muscle forces as a combination of a feedback term, which models the elasticity of the muscles, and a feedforward term, which is computed using inverse dynamics. Using our technique, motion sequences can be modified at interactive rates so that they react to unexpected disturbances. We demonstrate our technique using motion capture data from a sports video game. Our animated football player reacts to being hit with a ball, and then smoothly returns to the original motion sequence. 5.1.1 Implementation Details The implementation of our method consists of two main components. The first is a preprocessing stage, where we use inverse dynamics to estimate the feedforward torques from the motion capture data. The second component, which happens during the dynamic simulation, is the calculation of the actual muscle torques. The muscle torques are a combination of the precomputed feedforward torques, and feedback torques which depend on the difference between the trajectories of the rigid bodies in the motion capture and the trajectories in the dynamic simulation. 74 E x t e n s i o n s t o t h e S i m u l a t o r t o S u p p o r t I n v e r s e D y n a m i c s As we explained in Section 2.5.3, the row space of the Jacobian J is the space of possible constraint forces. We now wish to introduce an analogous matrix H whose row space is the space of all possible "muscle forces" which the joints can apply to their neighboring bodies1 . The rows of TL correspond to equal and opposite torques applied at the joint. The muscle forces are given by fm = HTT, (5.1) where r is a vector of multipliers, like the Lagrange multipliers, which are the magnitudes associated with the rows of 7i. Adding the muscle forces into our forward dynamics equation (Equation 2.32) gives M JT\ (</\ (fext + UTT . (5.2) J 0 / \ A / \ -k When doing inverse dynamics, the acceleration v is known, and the multipli-ers A and r are unknown. We rearrange Equation 5.2 to get ext (5.3) D e t a i l s of M a t r i c e s J a n d 7i In Section 4.4, we saw how to compute J for different kinds of joints. Knowing J, we can determine H by making a couple of assumptions: • The muscle forces and constraint forces originate at the joint, and must there-fore apply an equal and opposite force to the two bodies tha t are connected at lrH has a similar structure to J, and we will use the same fonts as we use for J (see page 22) when talking about the submatrices of 7i. 75 the joint. Mathematically, this means that if the coordinates of J and H are written with respect to a frame centered at the joint (call it frame j) the row space of I must be a subspace of the row space of 1 0 0 0 0 0 - 1 0 0 0 0 0 0 1 0 0 0 0 0 - 1 0 0 0 0 0 0 1 0 0 0 0 0 - 1 0 0 0 ... o ... 0 0 0 1 0 0 0 0 0 - 1 0 0 0 0 0 0 1 0 0 0 0 0 - 1 0 \ 0 0 0 0 0 1 0 0 0 0 0 - 1 / (5.4) (the 0's above are 6 x 6 blocks of zeros corresponding to the bodies that are unaffected by the joint. The remaining two 6 x 6 blocks are | ] and jH-b j^a corresponding to the two bodies, A and B, that are constrained by the joint.) jJfc • We assume that the muscle force basis vectors (the rows of H) always span all of the degrees of rotational freedom in the joint. Mathematically, this means (A that has rank 6. For instance, in a hinge joint, there is 1 degree of rotational freedom, and five DOF are removed from the system. For a ball joint, there are 3 degrees of rotational freedom, and 3 DOF are removed. (A Given these two assumptions, it follows that the row space of is iden-w tically the row space of the matrix in Equation 5.4. Like we did in Section 4.4.1, we must again transform the left and right halves of the above matrix (Equation 5.4) into frames A and B respectively, as these are 76 the frames in which our implementation records the velocity twists for the bodies. This transformation results in the following formulas for the J and H matrices. (% kx al lX 0 ~raz \ Tav a . h Ky al ly raz 0 ~ ~r a x a . 3z fcz al lz ~rav rax 0 0 0 0 0 0 0 1 0 0 1 0 0 °1 0 0 0 0 V u\ = \bJb) ( " • 3x Kx -bl 0 nz {-rby b . - h -k Ky - I by -nz 0 nx b . - 3z -% -X rK -nx 0 0 0 0 - 1 0 0 0 0 0 0 0 (5.5) In the above, the vectors j , k and I are the rotation axes and the forbidden axes of the joint. Inverse Dynamics Preprocessing Before beginning our dynamic simulation, we estimate a set of feedforward muscle torque multipliers r for each time step. Given the mass matrix A4, the constraint Jacobian J, the matrix H, the external force vector fext, and the acceleration a, we can calculate r using Equation 5.3. We can estimate the accelerations of the rigid bodies in each frame by fitting a smooth curve to the position data, and then finding the second derivative of the curve. One difficulty in evaluating Equation 5.3 is that the mass properties of the character's component rigid bodies are unknown. We deal with this by approximat-ing the shape of the character with polyhedra and computing the mass matrix for these, assuming the density of water (the body's average density is reasonably close 77 to that of water). We believe it may be possible to directly estimate the mass-inertia matrix of the figure from the motion capture data by making the mass properties unknowns in the inverse dynamics equation. However, this approach would require solving the inverse dynamics for every frame in the animation simultaneously, making it more computationally challenging. Estimating the mass properties from the geometry, on the other hand, allows us to solve each frame's inverse dynamics separately. We can use Equation 5.5 to compute J and "H. To do this, we require a kinematic model of the character to tell us the location and type of each joint. In our experiments, this model was given with the data set. Using Equation 5.3, we precompute feedforward torques for each of the frames in the animation before beginning the forward simulation. The total feedfor-ward torque is given by Ti T. But for the next section, we will need to break this down into smaller components ip1, ...iipn, each of which correspond to one degree of freedom of the joints. ft T = [hi h2 . . . hr ( \ T2 (5.6) \rnJ =hXTi + h2T2 + . . . + hnrn, = /01 + ^2 + • • • + VV The -0's are stored in body relative coordinates rather than world coordi-nates, because the orientation of the joints with respect to the world may be different in forward simulation than in the original motion capture animation. 78 Combining Feedback and Feedforward Torques During Forward Simula-tion The muscle torques applied during forward simulation are a combination of the feedforward torque (the torques calculated during the initial inverse dynamics phase) and the feedback torque, which models the muscle dynamics module of our motor control model. Individual feedback torques are calculated for each degree of freedom of all of the joints. Let 61,62, ...#n be joint angles corresponding to each degree of freedom, and 0i,02) •••Qn be joint velocities. The joint angles 9di,0d2, •••&dn a r e the "desired" joint angles - the joint angles from the motion capture data. The feedback torque tries to compensate small drifts and disturbances during the simulation. It is given by 7* = hi^Kiea - 6i) + kd{6dl - A*)), (5.7) where ks and kd are the stiffness and damping constants. Note that the stiffness is proportional to the magnitude of the muscle torque, hi\ri\, as observed empirically in muscle biomechanics. Our total muscle torques are given by the sum of the feedforward and feed-back torques: n i = l The muscle torques are added into the dynamics equation along with any other external forces, such as gravity and perturbations. 79 Algorithm Summary Our approach can be summarized by the following steps: • Preprocessing: inverse dynamics — Estimate the mass matrix M. for rigid bodies which approximate the shape of the character. — Fit a smooth curve to the position data. — Sample the accelerations of the rigid bodies at the rate at which we wish to run the dynamic simulation. — For each sampled time step: * Compute J and TL given the positions of the rigid bodies. * Solve the inverse dynamics equation to determine the muscle torque multipliers r . * Store the feedforward torques i/;1,..., i^n in body coordinates. * Store the current joint angles Oj and joint velocities Od-• For each step during forward simulation: — Compute the current joint angles 6 and joint velocities 6. — Compute the feedback torques 7X, •••,7n, using Equation 5.7. — Compute the total external force fext. — Solve the dynamics Equation 2.55 to determine the state of the system at the next time step. 80 5.1.2 Experiments and Discussion We performed our experiments on captured arm motions and full-body motions in football games. Figure 5.1 shows the skeleton model and the surface model we use in our simulation and rendering. Although our skeleton model is a relatively complex dynamic system, some of the joints are highly simplified compared to their real-world counterparts. For example, the spine is modelled with only 3 segments, which will cause unrealistic body response under some cases. The true human shoulder and wrist joints are also much more complex than the simplified ones in our model. Figure 5.2 shows motion perturbation on a sequence of arm motion. Figure 5.3 shows motion perturbation on a sequence of full-body motion. In both cases, the simulated skeleton responds to external disturbances and restores to the original motion naturally. There are several limitations in this work. First, our motor control model is very simple right now. It can only cope with small disturbances. We work under the assumption that small disturbances are totally recoverable by muscles, without triggering the brain to replan the motion. In a real-life interactive sports video game, realistic dodges and realistic falls are absolutely desirable but very difficult. We know of no system that can do this yet. More sophisticated motor control models need to be developed. We also simplify the system greatly by constraining the root joint (the joint located roughly at the Lumbosacral angle of the spine) to move along the motion capture path. We can thus omit the contact dynamics with the floor. This turns out acceptable since we are dealing with small upper limb perturbations. Even though motion perturbation is a subset of possible motion modifications, it is a large and common subset important for video games. 81 Figure 5.1: The skeleton model for our character simulation is shown on the left. Each square represents a joint. The total degrees of freedom is 54, and the degrees of freedom for each joint are labelled nearby. The skin geometry model for final rendering is shown on the right. One thing we discovered while implementing this was the need for implicit integration methods (see Section 2.7.4). Without this extension, we were unable to set the feedback term in our motor control to sufficiently high values without causing troublesome numerical problems (which cause the links of the character's body to go flying off in wildly different directions). Simulating realistic ground contact poses interesting questions for future 82 Figure 5.2: A sequence of a ball hitting a sim-ulated arm (left to right, top to bottom). The motion capture data (left arm in each frame) is shown for comparison. The simulated arm re-acts to the impact of the ball (which happens between frames 2 and 3), and then returns to the same path as the motion capture. 83 Figure 5.3: A sequence of a ball hitting a simulated body (left to right, top to bottom). The motion capture data (left body in each frame) is shown for comparison. The simu-lated body reacts to the im-pact of the ball (two collisions occur, one between frames 2 and 3, the other between frames 5 and 6), and then re-stores to the original motion. work. Due to the statically unstable nature of human walking, the character would not be able to stay upright if we simply added ground contacts and removed the root joint constraint. One difficulty is the lack of a good foot model. Humans can carefully adjust the distribution of contact forces between the foot and the ground; something our simplified rigid body model could not capture. Even with a more accurate foot model it would probably be necessary to alter the foot placements slightly to maintain the balance, using a controller like that of Pai [Pai91]. It is unclear how to blend in the results of this kind of balance controller in a way that retains the essential qualities of the original motion capture animation. This is an area for future work. 85 5.2 Experiments to Test The Stabilization Algorithm 5.2.1 6-Link Chain In the following test of our stabilization method, we simulate a 6 link chain falling freely under gravity (see figure 5.4). At time 0, the chain is completely horizontal. We observe the change in constraint error over time using one of three test conditions for comparison: no stabilization, Baumgarte stabilization, or post-stabilization. We evaluate the constraint function g(p) for each constraint, and use the maximum absolute value as a measure of constraint error. This number is roughly the largest joint separation distance. For comparison, each link in the chain is 100mm long. The time step size is 0.001 seconds. Figure 5.4: A simulation of a 6-link chain falling freely under gravity. Screen clearing between frames is turned off to show motion over time. In the version on the left, there is no constraint stabilization, so error in the constraints is evident as time progresses (note separation between the lowest two links of the chain). On the right, constraint stabilization keeps the links of the chain from drifting apart. Figure 5.6 shows us how the simulation behaves in the absence of any stabi-lization. The error grows over time as the joints of the chain separate. The graph has stair steps because of the periodic nature of the swinging chain, which behaves something like a pendulum. The error grows rapidly when the chain is moving more quickly. By the end of 600 time steps (0.6 seconds), the error has climbed to around 86 Figure 5.5: Unstabilized chain simulation: constraint error has grown very large. Links of chain no longer appear to be connected. 20mm. In figures 5.7 and 5.8, we see the effect of Baumgarte stabilization on the error. The choice of constant used in Baumgarte stabilization has a large effect on how well the stabilization works. In figure 5.7, we show two choices of constant (50 and 200) that seemed to work best for this situation. Below 50, the error became much larger. With constant of 50, the maximum error is around 5mm, and when the constant is 200, we get slightly better results, with the error never exceeding 3mm. If the constant is raised higher, the error does not continue to go down. In-stead, we start to notice another problem: the chain starts to jiggle unrealistically. This corresponds to the error correction term growing so large that it overshoots its goal in a single time step. Figure 5.8 shows an example of this, using Baumgarte stabilization with a constant of 2000. When one watches this simulation, one notices the chain bouncing around rapidly due to the large stabilization forces. Correspond-ingly, the error graph is quite jagged, although quantitatively the error is not much worse than when a constant of 50 is used. We show the results of using post-stabilization in figures 5.9 and 5.10. Figure 87 25 Maximum Constraint Error Over Time, Without Constraint Stabilization no stabilization 0 100 200 300 400 500 600 Time steps elapsed since start of simulation Figure 5.6: Maximum constraint error over time, for a simulation of a swinging 6-link chain without any stabilization 5.9 shows two curves: one is the constraint error measured in each step before the postStabilization step is applied, and the other is the error after the postStabiliza-tion step. Note that the scale in figure 5.9 is only one tenth that of the scale in the Baumgarte stabilization graphs above. Even in this scale, the "after postStabi-lization" curve is barely perceptible. Figure 5.10 shows just this curve, on an even smaller scale. The constraint error in this curve never goes above 0.01mm. 88 Maximum Constraint Error Over Time, with Baumgarte Stabilization Enabled •a-4 1= 2 1 -1 1 1 Baumgarte stabilization, constant=50 Baumgarte stabilization, constant=200 100 200 300 400 500 600 Time steps elapsed since start of simulation 700 800 Figure 5.7: Maximum constraint error over time, for a simulation of a swinging 6-link chain with Baumgarte stabilization, using a constant of 50 or 200 Maximum Constraint Error Over Time, with Baumgarte Stabilization Enabled 200 300 400 500 600 Time steps elapsed since start of simulation 800 Figure 5.8: Maximum constraint error over time, for a simulation of a swinging 6-link chain with Baumgarte stabilization, using a constant of 2000 89 Maximum Constraint Error Over Time, with Post-Stabilization Enabled 0.5 7 °-4 o CD a 0.3 2 0.2 0.1 ; postStabilization enabled, error measured after postStabilization postStabilization enabled, error measured before postStabilization 100 200 300 400 500 600 Time steps elapsed since start of simulation 700 800 Figure 5.9: Maximum constraint error over time, for a simulation of a swinging 6-link chain with post-stabilization enabled. Error is shown both before and after applying the post-stabilization step. Maximum Constraint Error Over Time, with Post-Stabilization Enabled 0.008 0.006 0.004 0.002 I I i i i i i , .A / \ A - ,AAf\. , . . ^ , sA ----^V 100 200 300 400 500 600 Time steps elapsed since start of simulation 700 800 Figure 5.10: Maximum constraint error over time, for a simulation of a swinging 6-link chain with post-stabilization enabled. This graph is a close-up of the smaller curve in figure 5.9 90 5.2.2 Stabilization of Contact Constraints Contact Constraint Error Over Time S. 1.2 w 0.8 <2 0.6 o O 1000 1500 2000 2500 Time steps elapsed since start of simulation 3000 3500 Figure 5.11: Contact constraint error over time for a rigid rectangle undergoing user interaction. No stabilization is used. Contact constraints also have problems with constraint error. A contact constraint is said to have error if the separation distance at the contact is less than the contact tolerance. Figure 5.11 shows contact constraint error over a period of time when a rigid rectangle is pushed around on the screen by the user. Over this time period, the body experiences many different kinds of contact: impacts, resting contacts, sliding contacts and rolling contacts, sometimes with just one point of contact, sometimes with more than one (see figure 5.12 for some examples of typical motions). When we run the same simulation with post-stabilization enabled, the con-straint errors are eliminated completely. The post-stabilization scales well to simu-91 Figure 5.12: Two screen captures from a contact simulation with a single moving rigid rectangle. User is pushing the rectangle interactively using repulsive forces at the mouse cursor. lations with many bodies and many contacts, such as the simulation shown in figure 5.13. Figure 5.13: Two screen captures from a contact simulation with five moving rigid bodies. 5.2.3 Advantages and Disadvantages of Post-Stabilization Comparing Baumgarte stabilization with our post-stabilization method, here are some of the trade-offs: • Baumgarte stabilization uses special constants that must be carefully set by 1)2 hand, and often there is no perfect value for. Post-stabilization does not require any tweaking. • With post-stabilization, the constraint error usually is eliminated in the same time step that it is created. With Baumgarte stabilization, usually it takes several time steps for the error to dissipate completely. The amount of time it takes to absorb the constraint error depends on the values for the special constants. If the constants are too small, then the error will dissipate slowly. If the constants are too large, the stabilization will over-correct, causing jerky oscillations of the bodies. • Baumgarte stabilization is easier to implement, and involves very little extra computation per time step. It only involves measuring the constraint error and adjusting the constant c in the constraint equation JM + c = 0. Post-stabilization actually requires formulating and solving another mixed LCP in addition to the dynamics LCP. This is quite a large penalty because solving these LCPs is one of the main bottlenecks in simulation performance. • Post-stabilization can perform poorly when there are large errors in the con-straints. This is because the linear approximation made in Equation 3.2 be-comes a poor approximation. Usually this results in a disturbing crash of the simulation, with the bodies flying off wildly. It is also possible for the bodies to jump into interpenetration during the post-step in some circumstances. This can be helped some by restricting the size of the post-step, and taking multi-ple post-steps in the same time step, but with the penalties of having to solve more systems, and having to choose thresholds. In our experience, Baumgarte stabilization seems to perform better when there is a lot of constraint error, 93 especially if the constants used are not too large. 94 Chapter 6 Conclusions and Future Work We have presented techniques for constructing a rigid body simulation system. We have described our implementation of a general-purpose system that can be used for interactive, real-time simulation. The system is a constraint-based system us-ing Lagrange multipliers, and it incorporates a time-stepping method that handles multiple contacts with static Coulomb friction in a general way. We presented a new constraint stabilization approach that combines the post-stabilization method of Ascher and Chin with an LCP formulation. This method is unique because it allows us to efficiently solve for the post-step when there are redundant contact constraints. Our experiments show that the post-stabilization eliminates constraint error very quickly. We have shown that our software system can be extended and used as a tool to support other interesting research. Our example of this is a project involving human character simulation. We demonstrated a technique for using motion capture data to drive a rigid body simulation of a human, which can then react to small perturbations such as being hit by a ball. 95 6.1 Future Work There are several directions in which this work could be built upon: • Hybrid system combining Feather stone's algorithm for large articulated bodies with Lagrange multipliers for contact. Featherstone's algorithm, which uses reduced coordinates, is still faster for large articulated structures than the lin-ear time Lagrange multiplier methods. Using reduced coordinates also carries the benefit of eliminating the need for constraint stabilization. Ideally, we would like a method where we could use an LCP framework that incorporates contact and friction, but also uses a reduced coordinate parameterization of the joints in the human kinematic model. The performance of the human character simulations in Chapter 5 would benefit greatly from such a method. • Real-time interaction through a data glove. One area we would like to explore is more natural interfaces for interaction with the virtual world. Using a da ta glove is an obvious choice for a natural interaction method, but it is unclear exactly how to incorporate the kinematic da ta measured by the glove into a physical simulation. This problem is similar to the problem in Chapter 5 of taking kinematic motion capture data and using it to drive a physical simulation of a running character. Would a similar method work for real time interaction using a glove? • Improved collision detection performance. Collision detection was not an area tha t we explored in much depth in this thesis. However, good collision detec-tion routines are very important for interactive rigid body simulation. Some important issues are: 96 — Exploiting temporal coherence. Rigid bodies should only change position by a small amount in each time step. Collision detection algorithms should be able to exploit this property. — Graceful degradation under time constraints. If we have hard time con-straints (e.g., must maintain a certain frame rate), it is better for the col-lision detector to be able to return approximate results than no results at all. One way way to do this is to use a hierarchy of successively finer ap-proximations to the real shape, such as sphere trees [Qui94]. O'Sullivan and Dingliana [OD99] present a method using sphere trees for collision response in interactive applications where only a fixed budget of time is available for collision detection and response. — Collision detector often returns too many contacts. When the meshes are highly tesselated and we ask the collision detector to return all pairs of features that are within e distance of each other, we may end up getting far more contacts than we really want. It would be ideal if the collision detector could have some smart way of returning just enough contact points so that there is no redundancy in the contact constraints. — Interpenetration Distance. Interpenetration distance is more difficult to calculate than separation distance, Few collision detection packages allow interpenetration distance queries. However it is easier to make rigid body simulations robust when this information is available. Many of these issues are being actively studied by collision detection re-searchers. 97 Bibliography [ACR94] Uri M. Ascher, Hongsheng Chin, and Sebastian Reich. Stabilization of DAEs and invariant manifolds. Numerische Mathematik, 67(2): 131-149, 1994. [AG85] W. W. Armstrong and M. W. Green. The dynamics of articulated rigid bodies for purposes of animation. The Visual Computer, 1:231-240, 1985. [Ani97] Mihai Anitescu. Modeling Rigid Multi Body Dynamics with Contact and Friction. PhD thesis, University of Iowa, 1997. [AP97] Mihai Anitescu and Florian Potra. Formulating dynamic multi-rigid-body contact problems with friction as solvable linear complementarity problems. Nonlinear Dynamics, 14:231-247, 1997. [AP98] Uri M. Ascher and Linda R. Petzold. Computer Methods for Ordinary Differential Equations and Differential-Algebraic Equations. Society for Industrial and Applied Mathematics, 1998. [AP00] Mihai Anitescu and Florian A. Potra. A time-stepping method for stiff multibody dynamics with contact and friction. Reports on computa-tional mathematics, ANL/MCS-P884-0501, Mathematics and Computer Science division, Argonne National Laboratory, 2000. 98 [APC97] U. Aschcr, D. Pai, and B. Clouticr. Forward dynamics, elimination meth-ods, and formulation stiffness in robot simulation. International Journal of Robotics Research, 16(6):749 758, 1997. [Asc97] Uri M. Aschcr. Stabilization of invariants of discretized differential sys-tems. Numerical Algorithms, 14(1 3): l-24, 1997. [Bar94] David Baraff. Fast contact force computation for nonpenetrat ing rigid bodies. In Proceedings of SIGGRAPH 1994, volume 28, pages 23-34, 1994. [Bar96] David Baraff. Linear-time dynamics using Lagrange multipliers. In Pro-ceedings of SIGGRAPH 1996, volume 30, pages 137 146, 1996. [Bar97] David Baraff. An introduction to physically based modelling: Uncon-strained rigid body simulation. SIGGRAPH '97 Course Notes, 1997. [Bau72] J. Baumgarte. Stabilization of constraints and integrals of motion in dynamical systems. Computer Metliods in Applied Mechanics and Engi-neering, 1:1-16, 1972. [BB88] R. Barzel and A. Barr. A modeling system based on dynamics con-sntraints. In John Dill, editor, Proceedings of SIGGRAPH 1988, vol-ume 22, pages 179-188, 1988. [BR79] O. Bot tema and B. Roth. Theoretical Kinematics. North Holland Pub-lishing Company, 1979. [Can86] John Canny. Collision detection for moving polylicdra. IEEE Transac-tions on Pattern Analysis and Machine Intelligence, 8(2):pp. 200-209, 1986. 99 [CD68] R. W. Cottle and G. B. Dantzig. Complementary pivot theory of math-ematical programming. Linear Algebra and AppL, 1, 1968. [CPS92] R.W. Cottle, J.S. Pang, and R.E. Stone. The Linear Complementarity Problem. Academic Press, Inc., 1992. [Cre89] J. F. Cremer. An architecture for general purpose physical system simula-tion - Integrating geometry, dynamics, and control. PhD thesis, Cornell University, 1989. [CYP02] Michael Cline, KangKang Yin, and Dinesh Pai. Motion perturbation based on simple neuromotor control models. Technical Report TR-2002-03, University of British Columbia, 2002. [Fea87] Roy Featherstone. Robot Dynamics Algorithms. Kluwcr, 1987. [GOM98] S. Gong, E. Ong, and S. McKenna. Learning to associate faces across views in vector space of similarities to prototypes. In British Machine Vision Conference, volume 1, pages 54-64, Southampton, UK, 1998. [GvL89] G.H. Golub and C.F. van Loan. Matrix Computations. Johns Hopkins University Press, 1989. [Hah88] James Halm. Realistic animation of rigid bodies. In John Dill, editor, Proceedings of SIGGRAPH 1988, volume 22, pages 299-308, 1988. [Hos] Wolfgang Hoschek. The colt distribution: Open source libraries for high performance scientific and technical computing in Java, h t t p : / / t i l d e - h o s c h e k . h o m e . c e r n . c h / ~ h o s c h e k / c o l t / i n d e x . h t m . 100 [KP02] P. G. Kry and D. K. Pai. Continuous contact simulation for sniootli surfaces. ACM Transactions on Graphics, 2002. to appear. [L82] P. Lotstedt. Mechanical systems of rigid bodies subject to unilateral con-straints. SI AM Journal on Applied Mathematics, 42(2):281-296, 1982. [LNPE92] C. Lubich, U. Nowak, U. Pohle, and Ch. Engstlcr. Mexx - numerical soft-ware for the integration of constrained mechanical mult ibody systems. Technical Report SC92-12, Konrad-Zuse-Zentrum fur Informationstech-nik, 1992. [MasOl] Mat thew T. Mason. Mechanics of Robotic Manipulation. M I T Press, 2001. [Mat] The matr ix and quaternion faq, h t t p : / / w w w . c s . u a l b e r t a . c a / ~ a n d r e a s / m a t h / m a t r f a q _ l a t e s t . h t m l . [MC95] Brian Mirtich and John F. Canny. Impulse-based simulation of rigid bodies. In Symposium on Interactive 3D Graphics, pages 181-188, 217, 1995. [Mir98] Brian Mirtich. Rigid body contact: Collision detection to force com-putation. Technical Report TR-98-01, Mitsubishi Electrical Research Laboratory, 1998. [MLS94] Richard M. Murray, Zexiang Li, and S. Shankar Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [Mur88] Ka t t a G. Murty. Linear Complementarity, Linear and Nonlinear Programming, 101 h t t p : / / i o e . eng in . umich. edu /books /mur ty / l inea r_complementa r i ty_webbook/ . Internet Edition, 1988. [MW88] Matthew Moore and Jane Wilhelms. Collision detection and response for computer animation. In John Dill, editor, Proceedings of SIGGRAPH 1988, volume 22, pages 289-298, 1988. [OD99] C. O'Sullivan and J. Dingliana. Real-time collision detection and re-sponse using sphere-trees. In 15th Spring Conference on Computer Graphics, pages 83-92, Budmerice, Slovakia, 1999. [Pai91] Dinesh K. Pai. Least constraint: A framework for the control of complex mechanical systems. In Proceedings of the American Control Conference, pages pp.1615-1621, 1991. [Qui94] S. Quinlan. Efficient distance computation between non-convex objects. In IEEE Intern. Conf. on Robotics and Automation, pages 3324-3329. IEEE, 1994. [Sho85] Ken Shoemake. Animating rotation with quaternion curves. In B. A. Barsky, editor, Proceedings of SIGGRAPH 1985, volume 19, pages 245-254, 1985. [ST96] D. E. Stewart and J. C. Trinkle. An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and coulomb friction. In-ternat. J. Numer. Methods Engineering, 1996. [Str91] W. J. Stronge. Unraveling paradoxical theories for rigid body collisions. Journal of Applied Mechanics, 1991. 102 [U1198] Chris Ullrich. Contact mechanics using green's functions for interactive simulated environments. Master's thesis, University of British Columbia, 1998. [Ver74] A. F. Vercshchagin. Computer simulation of the dynamics of complicated mechanisms of robot manipulations. Engineering Cybernetics, 6:65-70, 1974. [WGW90] Andrew Witkin, Michael Gleichcr, and William Welch. Interactive dy-namics. In Rich Riesenfeld and Carlo Sequin, editors, Computer Graphics (1990 Symposium, on Interactive 3D Graphics), volume 24, pages 11-21, 1990. 103
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Rigid body simulation with contact and constraints
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Rigid body simulation with contact and constraints Cline, Michael Bradley 2002
pdf
Page Metadata
Item Metadata
Title | Rigid body simulation with contact and constraints |
Creator |
Cline, Michael Bradley |
Date Issued | 2002 |
Description | We present techniques for constructing an interactive rigid body simulation system, and describe our implementation such a system. We use a constraint-based simulation approach, incorporating a time stepping method which handles multiple contacts with friction in a general way. We describe a method for constraint stabilization for simulations with contact. The method is based on the post-stabilization methods of Ascher and Chin, but is formulated as a linear complementarity problem, allowing us to solve for the stabilization step when there are redundant inequality (contact) constraints. As an application of our simulation software, we present a method for human character animation where motion capture data is used to drive a rigid body simulation. This allows physical constraints and forces acting on the character to be taken into account. |
Extent | 5438434 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
FileFormat | application/pdf |
Language | eng |
Date Available | 2009-09-15 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
DOI | 10.14288/1.0051676 |
URI | http://hdl.handle.net/2429/12756 |
Degree |
Master of Science - MSc |
Program |
Computer Science |
Affiliation |
Science, Faculty of Computer Science, Department of |
Degree Grantor | University of British Columbia |
GraduationDate | 2002-11 |
Campus |
UBCV |
Scholarly Level | Graduate |
AggregatedSourceRepository | DSpace |
Download
- Media
- 831-ubc_2002-0367_fixed.pdf [ 5.19MB ]
- Metadata
- JSON: 831-1.0051676.json
- JSON-LD: 831-1.0051676-ld.json
- RDF/XML (Pretty): 831-1.0051676-rdf.xml
- RDF/JSON: 831-1.0051676-rdf.json
- Turtle: 831-1.0051676-turtle.txt
- N-Triples: 831-1.0051676-rdf-ntriples.txt
- Original Record: 831-1.0051676-source.json
- Full Text
- 831-1.0051676-fulltext.txt
- Citation
- 831-1.0051676.ris
Full Text
Cite
Citation Scheme:
Usage Statistics
Share
Embed
Customize your widget with the following options, then copy and paste the code below into the HTML
of your page to embed this item in your website.
<div id="ubcOpenCollectionsWidgetDisplay">
<script id="ubcOpenCollectionsWidget"
src="{[{embed.src}]}"
data-item="{[{embed.item}]}"
data-collection="{[{embed.collection}]}"
data-metadata="{[{embed.showMetadata}]}"
data-width="{[{embed.width}]}"
async >
</script>
</div>
Our image viewer uses the IIIF 2.0 standard.
To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0051676/manifest