Formulating 3D Contact Dynamics Problems 1

REPORTS ON COMPUTATIONAL MATHEMATICS, NO. 80/1995, DEPARTMENT OF MATHEMATICS, THE UNIVERSITY OF IOWA

Formulating 3D Contact Dynamics Problems Mihai Anitescu

James F. Cremery October, 1995 Abstract

Florian A. Potraz.

Complementarity formulations are a promising approach for solving dynamic multi-rigid-body contact problems. Here, we address two aspects of simulating contact in a complementarity setting. First, we develop an explicit formulation of the dierential equations governing contact points for bodies of general surface geometry. These equations may be used to integrate the contact position and to set up the basic dynamics equations. Second, we present an ecient method for handling frictionless planar contacts of arbitrary boundary shape. Throughout, we attempt to set up the problem as explicitly as possible, paying special attention to the way the contact geometry is related to the dynamics.

1 Introduction 1.1 Motivation

The dynamic multi-rigid-body contact problem is concerned with predicting the accelerations and contact forces of several rigid bodies in contact. Work in a number of research areas, robotics and virtual reality (VR) especially, has led to strong recent interest in this problem. Virtual reality applications, for instance, need improved physical believability. Many systems exhibit physical inconsistencies that are glaringly apparent to the user due to a poor treatment of loss of contact and multiple contacts between bodies. The approach of classical dynamics simulation is to consider this problem as a piecewise dierential algebraic equation (DAE, see Haug [5]), using the sign of the constraint force as a test to decide if the respective contact will break or not. This approach implicitly assumes that the contact forces are continuous with respect to time. Another approach initiated by Lotstedt in his Ph.D thesis (see also [6]) consists in formulating the multi-rigid-body contact problem as a complementarity problem. The latter approach works, in special cases, even if the contact forces are discontinuous. In the present paper we will employ the complementarity approach, although we will assume that the forces are continous. Special situations allowing for discontinuities will be investigated in a future paper. The complementarity approach appears to provide a satisfactory solution to many of the problems encountered by simulation systems [6, 1, 10]. However, several problems arise though when using such an approach . For instance, setting up the problem assumes that the contact constraints can be cast as a few easily available 5 degree-of-freedom (DOF) constraints (e.g. point-on-plane, edge-on-edge). But including a cylindrical glass on a table in a linear complementarity setting would require in nitely many point-onplane constraints to handle the circular shape of the contact surface. Furthermore, consider the case of two bodies whose shapes are regular (but general) surfaces. Determining the contact point and computing the parameters of the contact constraint only as a function of the positions and the shapes is usually connected with determining the distance between the surfaces as a function of the relative position of the bodies. But, determining the distance is itself no easy matter. It is the aim of this paper to address these two issues: planar contact with arbitrary shape and arbitrary shape contact with a unique contact point. For the former problem, we present an ecient solution for one  Program in Applied Mathematics and Computational Sciences, The University of Iowa, Iowa City, IA 52242 ([email protected]) y Department of Computer Science, The University of Iowa, Iowa City, IA 52242 ([email protected] ) z Departments of Mathematics and Computer Science, The University of Iowa, Iowa City, IA 52242 ([email protected]).

1

particular case (frictionless with nonsingular Jacobian) by treating the con guration as a planar constraint, with a procedure to determine if the contact breaks, and convert the solution into a feasible solution of a linear complementarity problem if it doesn't. The second problem is related to the contact con guration. Usually one has either a planar or linear contact region or a unique contact point. The former case is usually modeled as several point on plane constraints. If the latter is generated by point-on-plane or edge-on-edge constraints the contact points as well as the kinematic data needed to set up the dynamics equations can easily be found from the bodies' relative positions and velocities. But, for contact con gurations that involve smooth shaped bodies (by the latter term meaning that it is a surface or curve suciently many times dierentiable) we develop an explicit formulation of the dierential equations governing the contact points. We then use these equations to integrate the positions of the contact points as well as to set up the basic dynamics equations. The paper is organized as follows: in Section 2 we present a brief description of an extended de nition of contact points and the LCP approach; in Section 3 we present the explicit setting of contact constraints based on a generalized normal vector; Section 4 is concerned with determining the kinematic data of the contact constraint; Section 5 presents the approach to planar contact. Most of the proofs of the results in Section 4 have been deferred to the Appendix.

1.2 Related work

In the next section we will summarize the complementarity approach to solving multi-rigid-body contact problems in order to provide a context for the two particular issues addressed in this paper. For detailed discussions of Linear Complementarity approaches see [6, 1, 10]. For a discussion of the origin of the problem and extensive bibliography, see [10]. The issue of arbitrary shaped contacts in a general setting has received signi cant previous attention, especially in the area of robotics. Montana [7] derived explicit contact point evolution equations for surfacesurface constraints, based on the geometric parameters of the surfaces. Cai and Roth [11] developed similar equations in an implicit setting and discuss the situation where one of the shapes is a curve as a limit case. Bara [1] has used an implicit type approach in the context of a complementarity setting of the contact problem. In this paper we extend the formulations from [7] and [8] to nd the equations of the closest points on the bodies for the case the bodies are not necessarily in contact. We extend the analysis to cases where one or both of the contacting regions are curves. The equations of the closest points are obtained in an explicit manner. From the implementation point of view, there is not a major dierence between the implicit and explicit setting since the required matrix inversion is not too expensive in either case (for each contact the matrix is limited in size). However, the explicit setting allows a better analysis of the singularities. We also present a very general form of the contact constraints based on a general de nition of the contact normal. In what follows, we analyze only the frictionless case. For smooth shaped bodies the use of the approach is straightforward in the friction case. However, for arbitrarily shaped planar contact, extension to the friction case is more dicult and we defer discussion of it to a future paper. We also do not include discussion of the geometric issues of collision detection and contact classi cation, nor our methods of modeling collision dynamics.

2 The assumptions and setting of the problem

2.1 The extremal distance between bodies

Assume that we have two bodies in 3-dimensional space and the system con guration can be represented by q 2 R12 a vector of coordinates, half accounting for the translational coordinates and the other half for the rotational coordinates. Also, assume for now that the bodies have a suciently regular shape (ultimately we will relax this constraint and require only piecewise smoothness). We are interested in nding the distance between the bodies as well as the closest points on the bodies. A large part of this paper is concerned with 2

d=0

λ

n1

λ

n2

Figure 1: Generic contact situation between two bodies. d is the distance between the bodies, The ni are the surface normals at the contact point, and  is the contact force. nding the new closest points following a small relative motion. If a pair of bodies has been in \perfect" contact and we allow for a small motion subject to some nonpenetration constraints it might happen that we get a small subsequent interpenetration due to the numerical integration. It is then useful, in order not to run into kinematic inconsistencies, to de ne the \extremal distance" and the \extremal points" [1, p.298]. We consider the \extremal points" to be points P10 on the rst body and P20 on the other body such that for any pair of dierentiable curves P1 ( ) and P2 ( ) on the rst and second bodies respectively, with P1 (0) = P10 , P2 (0) = P20 , we have

d 2 d kP2 ( ) ? P1 ( )k = 0

(2.1)

at  = 0. If two bodies are separated or in perfect contact, the extremal points coincide with the closest points on the two bodies. In the case of interpenetration, assuming suciently small interpenetration and suciently smoothly shaped bodies, the \extremal distance" d is the distance between the extremal points taken with a minus sign. This de nes a unique set of contact points whose positions depend on the relative body positions in a regular manner; we de ne P1 (q) and P2 (q) such that for any q P10 = P1 (q) and P20 P2 (q) are the extremal points at corresponding to body positions q. We have chosen a dierent formal de nition of the extremal points than [1] because of the need to accommodate contact situations that involve curves. As an example of de ning such an \extremal distance" function take a xed table with emerging unit normal N , pt an arbitrary point lying on the table and p the coordinate of a body made of a unique point. Then the function d = N
(p ? pt ) coincides with the distance (from the point p to the table) when positive, is negative at interpenetration, and is clearly dierentiable in terms of coordinates. It is clear that such a distance function cannot be well de ned for arbitrary interpenetration of two bodies. But since our goal is to be able to set up some geometrical constraints, the function d needs to be de ned with this regularity only in neighborhoods of the current contact points. Subsequently, we will refer to extremal points as \contact points" even when there is interpenetration or no contact between the bodies, and we will continue to refer to function d as \the distance between the bodies". We will also assume that P1 (q) and P2 (q), the functions giving the contact points with respect to the body positions, as well as d, the distance function, are suciently dierentiable. We call con gurations meeting these conditions \regular". We will see in the following sections that these are reasonable assumptions if the distance d between the bodies is suciently small and the body shapes and positions do not exhibit certain peculiarities (such as parallel edge-on-edge contact or excessive mutual atness).

2.2 The complementarity condition

Consider two rigid bodies in contact as shown in Figure 1. Assume that the bodies have been in contact for some nite period of time. Therefore d = 0 and d_ = 0. Since d  0 it follows that we require d  0 in order to avoid interpenetration in a subsequent instant. 3

On the other hand, let  be the size of the mutual contact force. If the force has the direction in the gure, it follows that   0, for the two bodies can only push on the other and not pull one another (i.e., the contact can only sustain compressive force). This is the main dierence between contact and traditional joint constraints which can assume both positive and negative reaction forces. Thus, contact constraints are sometimes called unilateral constraints, while joint constraints are termed bilateral constraints. Assume now that d > 0. This means that in the following instant the contact between the two bodies will break. As a result, the mutual force must vanish ( = 0). This leads to d   = 0: (2.2) Two nonnegative variables that satisfy such a condition are said to be complementary variables. By taking the derivative of the relation d = (q), we get d = q q + ?(q; q_) (2.3) where ? is a function whose form is not important for the purpose of the subsequent theoretical derivations. This equation will be important in setting up a linear relationship between d and .

2.3 The general setting of the contact problem

Suppose that we have a system of several bodies along with a set of m contacts between the bodies. We assume that each contact is a regular contact and let di = i (q); 1  i  m, be the distance functions associated with the contact set, where q is a parameterization of the position of the bodies ( a list of positions and rotation parameters for each body). For our purposes each di has to be at least twice continuously dierentiable. Let  be the function that maps the con guration space (the space of q's) to Rm and which is made up of the i 's, 1  i  m. The functions di = i do not have to represent the distance between the two bodies in contact. For instance if we have two bricks one on top of the other, with parallel sides but not touching, no such dierentiable distance function exists. Instead we may choose 3 points on one of the bricks and require that the distance between each of them and the plane containing the corresponding side of the other brick be positive (point-plane constraint). Each of these three functions is dierentiable. Similarly, we can model a polygonal pyramid sitting with its base on a table by requiring that each of corner of the base not penetrate the table surface. Thus, we use a point-plane constraint for each vertex of the base. For completeness, assume that the system has, in addition to contact constraints, L joint-type constraints de ned by the map (q) taking the con guration space to RL. For details concerning joint constraints see, for example, [4]. With these two maps, we can formulate the contact problem:

Problem Given the positions q and velocities q_ of a set of bodies, nd the accelerations that de ne a motion consistent with the following equality and inequality constraints. (q)  0 (q) = 0

(2.4) (2.5)

2 It is known [4] that for frictionless cases the direction of the constraint force is perpendicular to the manifold de ning the constraint surface. Thus the forces due to active contact constraints are of the type q (q)T  where  is the vector of the contact forces. Similarly q (q)T  is the reaction force corresponding to the joint constraints (2.5), where  is the vector of the contact forces. By writing Newton's law and 4

computing the second derivatives of the constraints (2.4, 2.5), we get: ! F ! M ?Tq ?Tq ! q ! 0  + ?1 : 0 = q 0 (2.6) 0 s  ?2 q 0 0 where si = di = i (q; q_) 1  i  m is the vector of contact accelerations. F is the sum of input forces and the Coriolis forces and ?1 and ?2 are terms involving velocities, q_, from the second derivatives of  and  with respect to time. (This notation is slightly abusive since in a Newton-Euler setting, q = (r; !_ ), where ! are angular velocities, a globally nonintegrable vector eld). For each contact the complementarity relation (2.2) must hold. Thus, si i = 0; 1  i  m. By adding up the relations (2:2) for all the contact constraints, we get sT  = 0; s;   0: (2.7) The relations (2.7) and (2.6) determine a linear complementarity problem [2]. It is known that the problem has a unique q as a solution, but the forces  and  need not be unique. This can be seen in various easily constructed examples.

2.4 Problems related to the LCP setting

 To set up (2.6), q , q , ?1 and ?2 are needed. q and ?2 originate from derivatives of joint constraints, (q) = 0, that are are usually readily available explicit functions. This is not the case for q and ?2 , though, because the functions (q) can be computed for a given q only by solving a minimization problem (for a general contact situation), which incurs considerable diculties in getting the aforementioned data.  For good computational results the problem should have a reasonable dimension. But if the contact region between two bodies is planar and arbitrarily shaped (non polygonal), we might have to use in nitely many point-on-plane constraints to model it (e.g. for a problem involving a cylindrical glass standing upright on a table).

3 Explicit setting of the dynamics equations In this section we focus on determining q and ?2 of (2.6) for a general contact situation. We restrict ourselves to the Newton-Euler formulation in global coordinates (which means that q of Section 2 consists of the usual translational and rotational coordinates for each body in the world frame). If B is a body, let p be the position, and R the rotation matrix of its center of mass in world coordinates. We analyze a regular con guration of two bodies B1 and B2 . Let d = (q1 ; q2 ) be the distance between the bodies as de ned in Section 2.1. Let (P10 ; P20 ) be the pair of contact points, with P10 2 B1 , P20 2 B2 . If d 6= 0, we de ne the \normal" (3.1) N = P10 ?d P20 : For the case d(q1 ; q2 ) = 0, let q0 be the concatenation of the body coordinates q1 and q2 . Because of our regularity assumption, for each q in a neighborhood of q0 there is a unique pair of contact points (P1 (q); P2 (q)). Assuming the limit exists, we de ne the normal in this case as P2 (q) ? P1 (q) : (3.2) N = qlim !q0 d(q) If it exists, it clearly de nes a unit vector N that emerges from B1 . If a regular surface is involved in the contact, then the de nition coincides with the surface normal. If the contact is an edge-on-edge 1 or 1

We use \edge-on-edge" to refer to contacts involving straight (non-curved) line segments.

5

curve-on-curve, then it can be shown that this normal is the cross-product of the tangents to the curves at the contact points (with the appropriate sign). It is clear that if we have a corner-on-corner contact, then we obtain dierent limits if we converge from dierent directions to q0 ; thus, such con gurations are singular. Finally, let now l1 ; l2 be the local coordinates of P1 (q) and P2 (q) relative to B1 and B2 respectively. Let p1 ; R1 and p2 ; R2 be the coordinates of the bodies with respect to the world. At the moment of the contact we have P1 (q) = P2 (q) = p1 + R1 l1 = p2 + R2 l2 . Lemma 3.1 The Jacobian row J12 = dq = q (in world coordinates) of the contact between the bodies at a speci ed time moment is: J12 = (?N T ; ?(R1 l1  N )T ; N T ; (R2 l2  N )T ): (3.3) Proof Assume that the bodies are separated at a distance d 6= 0. The result for d = 0 follows as the limit of the general case. The distance is continuously dierentiable with respect to time because of the regularity assumptions. Let (P1 ; P2 ) be the pair of closest points. Then we have jdj = kP2 ? P1 k, hence d = jddj kP2 ? P1 k and d_ = d kP ? P k jdj = d h(P ? P ); (P ? P )i 12 jdj = 1 jdj h d (P ? P ); (P ? P )i

dt

2

1

d

dt

2

1

2

d jdj d dt 2 1 2 1 d (P ? P ); dN i = h d (P ? P ); N i; = d1 h dt 2 1 dt 2 1 1

(3.4)

where by hx; yi we mean the scalar product xT y. It is now clear that the computation of the Jacobian row is no longer singular at d = 0. Replacing Pi by pi + Ri li ; i = 1; 2, yields: (3.5) d_ = hp_2 ? p_1 ; N i + hR2 l_2 ? R1 l_1 ; N i + hR_2 l2 ? R_1 l1 ; N i: We focus now on the middle term. We x the position of the bodies to be the one at time t = t0 . For this position, l1 (t) and l2 (t) are two curves on the bodies that trace the contact points. l1 (t0 ) and l2 (t0 ) are the current contact points. So, condition (2.1) applies and we get

hR2 dtd l2 (t0 ) ? R1 dtd l1 (t0 ); P2 ? P1 i = 0:

(3.6)

But now, dividing by d and using the de nition (3.1) of the normal, we get: (3.7) hR2 l_2 (t0 ) ? R1 l_1 (t0 ); N i = 0: Thus the middle term in 3.5 vanishes and we obtain: d_ = hp_2 ? p_1 ; N i + hR_2 l2 ? R_1 l1 ; N i = hp_2 ? p_1 ; N i + hR_2 R2T R2 l2 ? R_1 R1T R1 l1 ; N i = hp_2 ? p_1 ; N i + h!2  R2 l2 ? !1  R1 l1 ; N i = hp_2 ? p_1 ; N i + hR2 l2  N; !2 i ? hR1 l1  N; !1 i: (3.8) Since this is true for any velocities, and we must have d_ = J12 (p_1 ; !1 ; p_2; !2 )T (3.9) and the claim follows. 2 Remark It might seem strange that we are able to get continuous derivatives of the distance function, for this is a non-dierentiable function in R3 . The point is that we have a privileged direction in the space, the normal N , and we have de ned a signed distance which roughly corresponds to measuring the distance along the direction of N . This function has the required regularity (like the function f (x) = x for x 2 R). Obviously, such a de nition can be valid only locally. As a result of the previous lemma, we can completely de ne the terms involving contacts in (2.6). 6

Corollary 3.2 For each contact i; 1  i  m let l1i and l2i be the contact point coordinates for the bodies, B1i and B2i , involved in the contact. And let p1i ; R1i and p2i ; R2i be the coordinates of the bodies, and Ni the contact normal (emerging from body B1i ). Then i = (0;


0; ?NiT ; ?(R1i l1i  Ni )T ; 0;


0; NiT ; (R2i l2i  Ni )T ; 0;


; 0)

(3.10)

is the i-th row of the matrix , the nonzero entries corresponding to the coordinates of B1i and B2i , and

?2i = hN_ i ; p_2i i ? hN_ i ; p_1i i + h(R2i l2i  N_ i ); !2i i ? h(R1i l1i  N_ i ); !1i i + h(!2i  R2i l2i ); (!2i  Ni )i ? h(!1i  R1i l1i ); (!1i  Ni )i + h(R2i l_2i  Ni ); !2i i (3.11) ? h(R1i l_1i  Ni ); !1i i is the i-th element of ?2 . Proof The relation for i is a mere restatement of the previous lemma, whereas for ?2i we compute another derivative at (3.8) and take the terms that do not involve accelerations. 2 This corollary can be used to set up the dynamics equations, provided N_ i ; l_1i ; l_2i can be expressed in terms of the velocities of the bodies in contact. All these derivatives depend strongly on the particular geometry of the contact; therefore each contact type requires a dierent approach. If we have a simple geometry (point-on-plane, edge-on-edge) di = i (q) is a simple algebraic expression, and we do not need to go through the procedure of Lemma 3.1 . But for surface-on-surface, curve-on-surface and curve-on-curve constraints there is no easy-to-derive algebraic form of di = i (q); Corollary 3.2 helps in setting up the equations for those cases. Its main advantage is that the only thing required to explicitly set up 2.6 is some kinematic information (such as N_ and l_i ), which we investigate in the next section.

4 The kinematic description of the contact of smooth-shaped bodies In this section we analyze the kinematics of point contact between an arbitrary but smooth-shaped bodies. Here by smooth we mean that the position of the contact points of the bodies varies smoothly with the relative positions of the bodies. Even if the contact points on the bodies are on a curved edge, the contact is still smooth under some mild non degeneracy assumptions. We con ne ourselves to three cases of interest: surface-on-surface, curve-on-curve, and curve-on-surface. We could also have analyzed the point-on-surface case, but if the surface has an algebraic description, the contact (d = 0) case is treated very much like a joint constraint; for it, an analysis like the subsequent ones would be tedious and not particularly useful. There are various ways one can perform the kinematic description of an arbitrary but smooth shaped bodies contact. In the robotics literature there are two major approaches. The rst describes the shapes of the bodies in contact by functions of two variables and determins the kinematic parameters from the derivatives of these functions and the velocities of the bodies involved in contact [11]. A related approach has been used in 3D arbitrary shaped contact simulation where the motion of the contact points is described as constrained motion and the contact velocities are retrieved via the implicit function theorem [1]. The other approach, which we will use in this paper, assumes that the contact con gurations are described with local charts and, if a surface is involved, a local orthonormal frame is at hand [7, 8, 9]. The resulting kinematic relations are obtained in terms of the intrinsic parameters (curvature, torsion, etc.). In robotics, this approach is of special interest in contour following [7]. The analysis done here is within the framework described in [8, chapter2]. The basic object in this setting V , the relative velocity between two rigid bodies, is a six dimensional vector. Since we will make extensive use of it and its algebra, we present a brief review of its characteristics in Appendix A. The proofs of the kinematic descriptions are rather tedious and are deferred to the Appendices B through E. 7

The bodies considered below are not necessarily in contact, but can be at some distance d 6= 0 in the sense of Subsection 2.1. The analysis extends relatively easily for this case. For each of the three types of contact analyzed we will give conditions under which the contact velocities can be uniquely and dierentially determined from the relative velocities of the bodies in contact. As a consequence, any nonsingular con guration will be a regular con guration, because any relative con guration in the neighborhood of the given con guration will give a dierentiable contact pair.

4.1 The surface-surface contact 4.1.1 The equations

We consider two bodies whose center of mass frames are O and F respectively. On each body let Co and Cf be the instantaneous frames of contact on each body, and Lo( ) and Lf ( ) respectively be the family of frames associated with each body, such that at time t we have Lo(t) = Co and Lf (t) = Cf . Let also d be the distance between the two bodies, which is measured along the z direction of the contact frames, and the angle of rotation with respect to the contact frame around the z axis. is chosen such that a rotation with angle will make the axis x of Co and Cf parallel (see Figure2). We will assume that all calculations are done within a single map on each body, so that we will not worry about globality problems. The surfaces are given by means of the local coordinates, (u1 ; v1 ) and (u2 ; v2 ) respectively. co(o ) and cf (f ) represent the contact points in local coordinates, where o = (u1 ; v1 ) and f = (u2 ; v2 ) at the contact point. For each of the surface we denote by M; K; T the metric, curvature and respectively torsion tensor (with the appropriate subscripts). This case is a straightforward generalization of the case d = 0 which has been addressed in [7, 8, 9].

Theorem 4.1 Kinematic equations of contact The motion of the contact coordinates as a function of

the relative motion is given by

    _f = Mf?1 (Kf + K~o + dK~o Kf )?1 ((I + dK~0 ) ?!!xy ? K~0 vvxy )     _o = Mo?1 R (Kf + K~o + dKf K~o)?1 ( ?!!xy + Kf vvxy ) _ = !z + Tf Mf _f + ToMo _o d_ = vz

(4.1) (4.2) (4.3) (4.4)

h ? sin i ~ where (vx ; vy ; vz ; !x ; !y ; !z ) = Vlbo lf (see Appendix E), R = ?cos sin ? cos , and Ko = R Ko R . Proof See Appendix B. The only thing still required in order to set up the dynamics equations for this con guration is to determine the derivative of the contact normal. Lemma 4.2 For the surface-surface contact, the derivative of the normal in world coordinates is N_ = !o  N + Ho KoMo _ o ; (4.5) where Ho is the orientation matrix , !o is the angular velocity of the body O with respect to the world, and N is the normal with respect to the world. Ko; Mo; o are the curvature tensor, the metric tensor and the parameterization of the surface on the body O. _o is obtained from the previous theorem. Proof We have N = HoNo, where No is the normal in the coordinates associated with body O. We also o have dN do = Ko Mo from the de nition of the curvature tensor. But since

N_ = H_ o No + Ho N_ o = H_ o HoT Ho No + Ho N_ o = !o  N + Ho N_ o;

(4.6)

2

the conclusion follows. 8

Body F y2

Cf=Lf

Co=Lo

Body O

z1

Ψ

z2

x2

y1 x1

Figure 2: A general contact con guration between two arbitrarily shaped surfaces. ui and vi are the local parameterizations, [xi; yi; zi] is the orthogonal coordinate frame on each body and is the angle between the xi's .

r1

r2 d=r2-r1

Figure 3: A singular con guration for surface-surface contact

9

4.1.2 The singularities

Clearly, the correct de nition of the previous equations depends on the invertibility of the matrix (Kf + K~o + dK~o Kf ). For d = 0 we obtain the so-called \relative curvature" [8]. In this subsection we will discuss the singularity to get some intuition about it. If both bodies are locally strictly convex, the matrix is always positive de nite, thus nonsingular. If both are locally strictly concave, then the pair of points cannot be a minimum-distance pair. Consider the case where one is strictly convex and one is strictly concave. To simplify , assume that the bodies are locally spherical, thus 1 0 (4.7) Ko = 0r 1 ;  ? 1 0r  (4.8) Kf = 0R ? 1 : R Clearly, the matrix (Kf + K~o + dK~o Kf ) is singular if and only if

1 ? 1 ? d 1 = 0; (4.9) r R Rr or, multiplying by Rr, R = r + d. Thus the matrix is singular if and only if the two spheres are concentrical. But this means precisely that there are in nitely many minimum-distance pairs (any pair collinear with the common center. See Fig. 3) which con rms that the matrix and the con guration should be singular.

4.2 The curve-curve contact

4.2.1 The equations

The approach in setting up the curve-curve contact equations is essentially the same, except for the fact that in order to describe a contact point on a curve we need only one parameter, whereas two parameters were necessary in the surface case. To make up for the second parameter, we consider the angle of rotation around the local tangent to the curve. Once again we consider two bodies whose respective center of mass frames are O and F respectively. On each body let Co and Cf be the instantaneous frames of contact on each body. While for the surface-onsurface contact the de nition was obvious, here we de ne Co to be that frame that has the x axis along the tangent to the curve on body O and the z axis along the minimum distance line between the bodies. Cf is de ned analogously. Lo( ) and Lf ( ) respectively are the families of frames associated with each body, such that at time t Lo(t) coincides with the Frenet trihedron [3] on the body O's curve. Lf (t) coincides with the Frenet trihedron on body F 's curve. o is the angle de ned by Lo (t) and Co , measured around the common x axis. Similarly, f is the angle de ned by Lf (t) and Cf . Let also d be the distance between the two bodies, which is measured along the z direction of the contact frames, and the angle of rotation with respect to the contact frame around the z axis. is chosen such that a rotation with angle will make the x axes of Co and Cf coincide. We will assume that both curves are parameterized with respect to their natural parameter, the length, which will allow for the Frenet description (see Fig. 4). We denote by co (lo ) and cf (lf ) the contact points on each body in local coordinates. Theorem 4.3 Kinematic equations of contact for the curve-curve con guration The motion of the contact coordinates as a function of the relative motion is given by     _    lf + v~x = R l_o ? R ?dko sin o l_o (4.10) v~y 0 0 do l_o + d_ 0     kf sin f l_f +  !~y  = R ko sin o l_o (4.11) ?!~x ?ol_o ? _ o ?f l_f ? _ f 10

Body F y2 n2 b2

Lf

φ2 b1

z2

φ1 z1

ψ

Cf x2=t2

Co

Body O

x1=t1 y1 n1

Figure 4: A curve curve con guration. [ti; ni; bi] represents the natural frame for each curve (tangent, normal, binormal). The angle is the angle between the tangents, measured around the common perpendicular. [xi; yi; zi] are chosen such that the zi 's are along the common perpendicular and the xi 's along the tangents. The angles i measure the rotation between [xi; yi; zi] and [ti; ni; bi].

d_ = v~z _ = !~z + ko cos o l_o + kf cos f l_f

h where (~vx ; v~y ; v~z ; !~x ; !~ y ; !~z ) = Adgl?1c Vlbo lf , (see Appendix E), and R = ?cos sin f f Proof See Appendix C.

? sin i. ? cos

(4.12) (4.13)

2

T2 where N is the contact normal in the In the case of curve-on-curve constraint we have N = kTT1 T2 k 1 sense of (3.1) and T1 ; T2 are the tangents of the two curves involved in the contact, for an appropriate choice of the tangent directions. For setting up the dynamics equations, we need the derivative of the normal, given by the following result. Lemma 4.4 The derivative of the normal in world coordinates is N_ = u_ ? u hu;_ ui (4.14)

kuk kuk3

where

u = T 1  T2 _ u_ = (!o  T1 )  T2 + T1  (!f  T2) + k1 loN1  T2 + k2 l_f T1  N2

(4.15) (4.16)

and !o and !f respectively are the angular velocities of the two bodies. T1 ; N1 ; k1 and T2 ; N2 ; k2 are the tangents, the normals and the curvatures of the curves involved in contact on bodies O and F respectively. l_o and l_f are the derivatives of the length of the curves in time, computed with the help of the previous theorem. All vectors are expressed in world coordinates.

11

Proof We have T1 = HoT1o , N1 = HoN1o, T2 = Hf T2f , and N2 = Hf N2f , where the additional subscripts o or f on the vectors mean that they are computed with respect to the local frames on each body. We then get (4.17) T_1 = H_ o T1o + Ho T_1o = H_ o HoT Ho T1o + Ho k1 N1ol_o = !o  T1 + k1 N1 and similarly T_2 = !f  T2 + k2 N2 (4.18) 2 and the claim follows after taking the derivative of N = kuu k . 4.2.2 The singularities

For the important case of d = 0 we get the following specialization of the previous theorem: Corollary 4.5 For the case d=0 we have: l_o = ? sinv~y l_f = ?v~x + cot v~y ~x _o = ? sin _f = ?~y + cot ~x d_ = v~z _ = !~z + ko cos o l_o + kf cos f l_f where

   ~x   !~y   kf sin f l_f  ko sin o l_o : ? R + = ~y ?!~x ?ol_o ?f l_f

(4.19) (4.20) (4.21) (4.22) (4.23) (4.24) (4.25)

We can see that for this case, a singularity occurs if sin( ) = 0, which happens only when the tangents to the two curves coincide. This is an obvious result for the case the curves are straight lines. It is also intuitive in the general case, for in this case we cannot de ne a contact normal (which is the vector product of the two tangents).

4.3 The curve-surface contact

4.3.1 The equations

We now consider the case where the contact involves a curve on body F and a surface on body O. De ne the contact frame Cf to be that frame that has the x axis along the tangent to the curve on body F and the z axis along the minimum distance line between the bodies. Co is the same frame on body O as for the surface-surface contact. Lf ( ) and Lo ( ) now have a clear meaning, with the former being the same as its homologue from the curve-curve case, and the latter the same as its homologue from the surface-surface case. Let f be the angle de ned by Lf (t) and Cf , measured around the common x axis. d and have exactly the same meaning as in the previous sections. With these conventions, the left-hand side of the equations will be the same as the one of curve-curve contact, whereas the right-hand sides will be the same as for surface-surface contact (see Fig. 5).

Theorem 4.6 Kinematic equations of contact for the curve-surface con guration The motion of

the contact coordinates as a function of the relative motion is given by



l_f 0

   + vv~~xy = R 12

(1 + dKo)Mo _o

(4.26)

Body F y2 n2 b2

Lf

φ2 z2

Co=Lo

Body O

z1

Cf

ψ

x2=t2

y1 x1

Figure 5: A curve-surface con guration

 k sin  l_    f f f + !~ y ?!~x = ?R KoMo_o ; ?f l_f ? _ f d_ = v~z _ = !~z + ToMo _ o + kf cos f l_f

(4.27) (4.28) (4.29)

h ? sin i. where (~vx ; v~y ; v~z ; !~x ; !~ y ; !~z ) = Adgl?1c Vlbo lf (see Appendix E) and R = ?cos sin ? cos f f Proof See Appendix D. 2 For this case the derivative of the normal is exactly the same as for the surface-surface contact (Lemma 4.2), with the dierence that _o is now computed using Theorem 4.6. 4.3.2 The singularities

In order to discuss the singularities, we examine the case where O has a spherical shape and radius R. We also de ne = R _o . The evolution equations become: _    lf + v~x = (1 + d ) (4.30) v~y 0 R  k sin  l_   !~  1 f ff y (4.31) ?f l_f ? _ f + ?!~x = R ; From the structure of the system, it is obvious that the corresponding matrix is invertible if and only if (1 + d=R) 6= 0; (4.32) 1=R ? kf sin f (1 + d=R) 6= 0: (4.33) The former ensures that y can be solved for, whereas the latter ensures that l_f and x can be solved for. Let r~ = ? kf sin1 f be the eective curvature radius of the curve. Then the previous relations can be rewritten as d 6= ?R; (4.34) r~ 6= ?(R + d): (4.35) 13

We have dierent types of singular behavior:  d = ?R (with the spherical region on O concave). All points on O within the spherical region are at the same distance from the contact point on F . Thus the con guration must be singular (and there cannot be a unique contact pair).  r~ = ?(d + R) is the same type of singularity as for the surface-surface constraint (4.9). Thus the discussion for the singularities of the surface-surface case applies. The only dierence is that the convexity of the curve is discussed in the plane parallel to the tangent. Intuitively, for d = 0 this condition means that the eective curvature of the curve has to be dierent from the curvature of the surface along the tangent to the curve, or otherwise that the curve should have an excessively at contact with the surface.

4.4 Setting up the dynamics equations

We assume that the contacts i; 1  i  m, are of one of the types analyzed in the previous subsections or that the function di = i (q) can be explicitly determined (as is the case for point-on-plane, edge-on-edge or even point-on-surface for an explicitly de ned surface). The latter contacts have the additional advantage that the contact points can be easily determined. The contacts analyzed in the previous subsections are described by functions i of six variables. For instance, in the curve-surface case i = (uo; vo ; lf ; f ; ; d), where (uo; vo ), is a parametrization of the surface on body O and the rest of the parameters are introduced in the Subsection 4.3. The analysis done in the previous subsections gives us _i as a function of i , the positions and the velocities of the bodies involved in contact, say

_i = i (i ; q; q_)

(4.36)

where by q and q_ we denote the positions and velocities of all the bodies in the system. If i is known, then it is very simple to determine the position of the contact points, l1i and l2i and the associated normal, N . For instance in the curve-surface case, we have l1i = co (uo ; vo ), the local parametrization of the surface and l2i = cf (lf ), the local parametrization of the curve, as introduced in the previous subsections. For the normal we have N = HoNo (uo ; vo ), where Ho is the rotation matrix for the body O, and No(uo ; vo ) is the mapping that gives the exterior normal in local coordinates. Therefore we also have l_1i ; l_2i ; N_ i , by the chain rule, as functions of i ; _i ; q; q_. Thus we have the functions l_1i = 1i (i ; _i ; q; q_) (4.37) _l2i = 2i (i ; _i ; q; q_) (4.38) _Ni = i (i ; _i ; q; q_): (4.39) Hence, having the positions and velocities of all the bodies involved in the simulation as well as the i 's , we are able to compute l_1i ; l_2i ; N_ i , for 1  i  m. By using this data in Corollary 3.2 we can compute q and ?2 from (2.6). Since the terms q and ?1 can be easily computed from the joint type constraints (which are usually represented by explicit functions), the LCP (2.6) is now completely de ned. Next, assume that we are able to solve the LCP and get the accelerations. At that moment we can take an integration step and determine the new positions and velocities of the bodies. The problem is that at this new position we need to start an optimization algorithm for determining the new contact points. But we could also use (4.36) to integrate i which will give us the position of the new contact points, since we use only information readily available. Therefore the integration process can be implemented as follows:

 Start with q,q_ and assume that the i 's are available.  Set up and solve the linear complementarity problem (2.6, 2.7) for the accelerations q. To generate the right hand side, use (4.36) and then (4.37),(4.38) and (4.39) in Corollary 3.2. Each the functions i ; 1i ; 2i ; i is obtained from the analysis of the geometry of each contact. 14

 Get the accelerations from the LCP and integrate to get the new q, q_.  Integrate (4.36) to get i 1  i  m, at the new step, which gives us the new contact points. At this stage the process can be restarted.

5 Arbitrary shaped planar contacts

5.1 The ane relation between various representations of a planar constraint

In the following, we investigate the case where the contact region is planar but arbitrary shaped (as is the case for a glass on a table). If the contact region were a polygon, we would simply set up the contact constraint by requiring that each vertex of the polygon stays in contact with the face on the other body. There are two major disadvantages in this approach. First, if the contact region has more than 3 vertices, we will introduce a singularity in the matrix of (2.6) (because no matter how many points we introduce as a constraint, the linear subspace spanned by the corresponding columns has rank at most 3, as we will see). Second, if the polygon has too many edges, we can increase dramatically the size of the LCP. Things get even worse if the contact region is a closed curve for which case we should consider in nitely many constraints. We describe how to circumvent these problems for the special case of frictionless contact and nonsingular matrix of (2.6) by using just three points of the contact region to represent the planar contact. We also assume that the external forces are continuous. In the following analysis, we con ne ourselves to the Newton-Euler representation. If we de ne q12 = fp1; !_1 ; p2 ; !_2 gT , then the constraint that a speci c point on the planar contact region of one body is in contact with the other body is expressed as :

J12 q12 + ?12 = 0; (5.1) where ?12 represent Coriolis-type accelerations and J12 is given by (3.3) with N the normal to that plane, and l1 ; l2 the local coordinates of the contact point with respect to the two bodies. P3  x with Lemma 5.1 If x 1 ; x2 ; x3 are non-colinear points on the planar contact region and x4 = i=1 i i P3  = 1, then J (x ); i = 1; 2; 3 are linearly independent, and: 12 i i=1 i J12 (x4 ) = ?12 (x4 ) =

X

i=1;3

X

i=1;3

i J12 (xi );

(5.2)

i ?12 (xi ):

(5.3)

Proof The linear independence follows from of the Jacobian (3.3). Any zero linear combination P the form = 0 in order to cancel the rst component (3.3). But of these three Jacobians will have to have i i =1 ; 3 P then N ? i xi which makes it impossible to cancel the second component if any  is nonzero and the points xi non-colinear. The only way J12 depends on x is through l1 and l2 . But the dependency is clearly ane in x, since l1 (x) and l2 (x) are both ane functions, which proves (5.2) . On the other hand, the Coriolis accelerations depend only on the velocities, thus even if we assume that the planar constraint is bilateral, the Coriolis accelerations will be the same if we have contact at that speci c moment. For bilateral constraints, we have: 0 = J12 (x4 )q12 + ?12 (x4 ) =

X

i=1;3

?

i J12 (xi ) + ?12 (x4 ) =

(5.4)

i ?12 (xi ) + ?12 (x4 ):

(5.5)

X

i=1;3

2

which proves (5.3) and the Lemma. 15

Corollary 5.2 Let yi ; 1  i  n be a set Pofn points on the base and i ; 1  i  n, a set of forces such

P

i i (which is a point on the contact plane) is inside the that ni=1 i > 0. Then if the point x = Pi=1 n  i=1 i convex hull of the contact region, there are three points in the contact region x1 ; x2 ; x3 and three positive forces 1 ; 2 ; 3 > 0 such that

y

n X i=1

i J12 (yi ) =

3 X

j =1

j J12 (xj ):

(5.6)

Proof Let i = Pni i . Then Pni=1 i = 1, thus x = Pni=1 i yi is a point on the common contact plane i=1

(since the latter is an ane space). If x is in the interior of thePconvex hull of the contact region it follows that there are xj ; j = 1; 3 in the contact region such that x = 3j=1 j xj with j > 0 . By the Lemma 5.1 we have: 3 n X X (5.7) i J12 (yi ) = J12 (x) = j J12 (xj ) i=1

i=1

P

and the P conclusion of the corollary follows after multiplying the latter relation with ni=1 i and taking n j = j ( i=1 i ); j = 1; 3. 2 It is easy to see that there is no restriction on the sign of the i 's in the previous lemma, which leads to the idea that the contact constraint for this case (arbitrarily shaped contact regions) can be viewed as a planar joint constraint, expressed, for example, as three point-on-plane bilateral constraints. We now use the assumption from the beginning of the section that the matrix in (2.6) is invertible and that the external forces are continuous. We see that forces corresponding to these three bilateral constraints can be computed and are continuous functions; thus, the x obtained by the procedure from the proof of the previous corollary is a continuous function. As long as this point is inside the convex hull of the contact region, the planar contact will still be acting during the next integration step, because a solution to the LCP (2.6) has been found which has only positive forces and zero left hand side. When x moves outside this convex hull the contact will break, and the continuity of x allows us to decide what type of constraint to impose on the bodies after the planar constraint breaks. It is not necessary to require that the matrix from (2.6) be nonsingular. But, we need some procedure to ensure that the forces from (2.6) are continuous (see for instance [1]), for otherwise it becomes dicult to foresee the way the contact will break.

5.2 Integration strategy for arbitrary-shaped planar contacts

We assume that the bodies involved in the contact have been in contact for some nite time and that at the beginning of the simulation we will obtain solutions consistent with the contact being still in eect. For solving the dynamics, we have adopted techniques from both the previous subsection and the Section 4. Following the previous subsection, we adopt the following strategy:  We take three points on the boundary of the bottom of the glass and solve for the accelerations, by using the linear system setting, not the LCP (set the left hand side of (2.6)) to zero and solve for the forces and accelerations. This is possible, since the system matrix is now nonsingular. Among other data, we get i ; i = 1; 3, the forces associated to the three point on plane constraints.  If some of the 's are negative, we nd the point x from the Corollary 5.2 and check whether the point is inside the convex hull of the contact region (x is expressed in local coordinates with respect to the glass). { If yes,P that means that there is a choice of three points on the bottom of the glass such that x = i=1;3 i xi with i > 0, by the same Corollary. Following (5.2) if we replace the old three point-plane constraints with the point-plane constraints associated with these new points, the forces will be positive and the glass will still stay on the table. 16

Figure 6: Four frames from a simulation of a cylindrical object rolling and sliding moving on a tabletop.

{ If no, we estimate by interpolation where x has crossed the boundary of the convex hull of the

glass (because the system matrix is nonsingular, x is continuously dierentiable with respect to the time) and restart the simulation there, by using only a curve- plane constraint, initialized at x or something similar if the boundary of the bottom of the glass contains a line. P To use Corollary 5.2 we need to have 3i=1 i > 0. This is a reasonable assumption for most exterior forces. However, this is not true for example for a force that will P lift one body from the other. For this case we need 3 to adopt a dierent strategy once we have noticed that i=1 i = 0. But for most cases x will get out of P 3 the convex hull of the planar contact before i=1 i = 0 and the previous strategy will work.

5.3 Implementation details

We simulated the dynamics of a cylindrical glass on a table subject to an external force and torque. The force and torque were de ned such that the contact region was the bottom of the glass for a while at the beginning of the simulation, after which glass began to rotate and slide on the contour of it base (which is a circle). Four frames from the simulation are shown in Figure 6. In the rst stage the strategy from the previous subsection is applied. When the dynamics is no longer consistent with the contact region being the bottom of the glass, the point-on-plane constraints are replaced by a curve-on-surface constraint. The problem is that the con guration is singular for the curve on surface constraint while the glass is on the bottom, since f , the angle between the normal of the plane and the binormal is exactly , thus the condition (4.33) fails for R = 1 (the surface is a plane) and f =  (and this is true for any shape of the 17

bottom of the glass). We have solved that by requiring that an imaginary edge of the glass, centered at the intended contact point stays in contact with the plane, until the glass gets a pitch of at least :01 radians, which gets the simulation suciently far from the singularity. From that moment on, we have solved the dynamics via the Theorem 4.6.For this particular case, we could have done away with the contact equations, because the distance between a circle and a plane can be explicitly computed. Nevertheless it was a useful test of correctness for our equations plus that the extension is immediate for any kind of regular shape of the planar contact (bottom of the glass).

6 Conclusions We have presented two approaches to deal with smooth shaped bodies and arbitrarily shaped planar contacts.

6.1 Smooth shaped bodies in contact

In order to be able to set up the LCP (2.6) we need some kinematic data of the objects involved in contact including the contact points of the bodies in contact. Finding that information requires solving an optimization problem (minimize the distance between two points on the bodies) or some other (even discrete) search method. We have tried to use the regularity of the contacts to nd the derivatives of the contact points as a solution of a linear system (with the right hand side depending on the velocities of the bodies in contact) and use these to integrate for the contact points (but one needs to nd a pair of contact points at least at the beginning, to start the simulation). If the matrix of the linear system is nonsingular, for each relative velocity of the bodies we have a unique solution, thus for all nearby relative con gurations there is going to be a unique pair of contact points, and their position will depend smoothly on the relative position of the bodies. At a rst glance, our smooth-shaped bodies approach looks very much like an optimization problem, since we get a search direction via the right hand side of the contact evolution equations. The main dierence is that in our approach the step size is speci ed in advance being the integration step, plus the use of a high order integrator will result in a higher order approximation of the new contact point. If we use an optimization approach directly, we would throw away the knowledge from the previous integration steps. On the other hand, the contact evolution equations present an interest of their own. We have also extended the analysis of the contact con guration for the case when there is no mutual contact but we intend to follow the closest point on the bodies (d > 0) or allow as small amount of interpenetration (d < 0). We hope that this will help to simulate better the bouncing of complex shaped bodies, when restarting the collision detection could prove to be very costly. The singularity analysis we have performed in Section 4 can help us identify the range of d for which our contact equations are still valid. All the techniques presented here answer only to the local problem of the contact point evolution. In the case of non convex bodies in contact there might be necessary to use these methods in conjunction with some classical collision detection algorithms, to accommodate the change in the contact structure. The approach can also easily be extended for the (dynamic) friction case, for the kinematic description of the contact situations is identical. The only thing to be changed is the expression of the contact force which is not any longer Tq , in (2.6) having now a component tangential to the contact.

6.2 Arbitrary shaped planar contacts

We presented a way to approach planar contacts that essentially establishes criteria under which this contact can be treated as a bilateral planar constraint. This approach has a number of shortcomings, mainly that it is dicult to extend to the friction case because of the latter's nonlinearity. Also, the analysis does not apply in the case of a singular constraint Jacobian. The primary advantage is that it allows a reduced order system and it is encouraging for many heuristic approaches (especially if we have some technique to ensure that the contact forces are continuous). Another 18

advantage generated by Lemma 3.1 is that the results apply regardless of the representation of the planar contact ( three points on plane constraints, two curves on plane and one point on plane , etc.).

7 Acknowledgements This work was supported in part by Oce of Naval Research grant N00014-94-1-0576 as part of the Isaac project The project's World Wide Web homepage at URL http://www.cs.uiowa.edu/~cremer/Isaac.html contains project information, publications, and simulation demonstrations. The authors would like to thank Je Trinkle for carefully reading the manuscript and spotting several misprints.

A The algebra of relative velocities We will describe shortly the algebra of relative velocities from [8] that we will use in order to describe the equations of the contact. The relative position of two frames , say a; b is described by the position vector pab and by the relative rotation matrix Rab . The absolute rotation matrices satisfy Rb = Ra Rab . All the transformations can be de ned in terms of a matrix gab 2IR44 de ned as

h gab = R0ab p1ab

i

(A.1)

?1 With this de nition, the spatial and body velocities are de ned to be the V^abs ; V^abb 2IR44 ; V^abs = gab _ gab ? 1 b and V^ab = gab gab _ . We will be mostly interested in the latter case, for which we have the following explicit representation:



T T V^abb = Rab0R_ ab Rab0p_ab



(A.2)

On this basis we recover the familiar form of the relative velocity

Vabb =

 vb   RT p_  ab ab b = Tab (Rab R_ ab )

!ab

(A.3)

One of the most important relations used from here on will be the one relating velocities of several frames. For this purpose, one associates with each gab type matrix the following matrix: i h (A.4) Adgab = R0ab p^abRRabab Finally, with this notations we have the transformation relations:

Vacb = Adgbc?1 Vabb + Vbcb :

(A.5)

which we use extensively in the paper. At times, we will need the following velocity : Vlbo lf , where lo and lf are frames which are xed with respect to o and f respectively. Thus Volb o = Vflb f = 0. Hence, from A.5:

Vlbo lf = Adgfl?1 Vlbo f = Adgfl?1 Vofb f

f

19

(A.6)

B The proof of Theorem 4.1 All the calculations are done adapting the treatment from [7] for this case. Since all calculations involve body velocities, we drop the superscript b. We will also need the following Lemma B.1 . Induced velocity of the contact frame The (body) velocity of the contact frame C relative to the reference frame O of the body is given by Vocb = (voc ; woc ) where h i voc = M0_ (B.1) " 0 ?!z !y # " 0 ?TM _ # 0 KM _ !^ oc = !z 0 ?!x = TM _ (B.2) ?!y !x 0 ?(KM _ )T 0 Proof See [7]. 2

Proof of Theorem 4.1. Since the frame Lf (t) is xed relative to the frame F , the velocity of Lf (t) relative to F is given by Vflf = Volo = 0. Therefore using the velocity relationships (A.5),we get: Vfcf = Vlf cf (B.3) (B.4) Voco = Vlo co : Also, at moment t; Lo(t) = Co and Lf (t) = Cf . Therefore plo co = plf cf = 0 and Rlo co = Rlf cf = I at moment t. Also ! h i 0 pcocf = 0 ; Rco cf = R0 ?01 (B.5) d The latter relations happen because the z direction of the two contact frames is common, and is the amount of rotation measured around this axis. Now we will compute the velocity of Cf relative to Lo via two intermediate frames, namely Lf and Co . Following (A.5) we have  RT ?(RT p )^RT  co cf co cf co cf Voco + V co cf : (B.6) Vlo lf + Vlf cf = Vlo cf = Adgc?o1cf Vlo co + Vco cf = c0o cf Rco cf By de nition, Vlo lf = (vx ; vy ; vz ; !x; !y ; !z ) and Voco ; Vlf cf = Vfcf are given by Lemma B.1. Thus we can use the last relations to get the derivatives of the coordinates of the contact point.

 R Mo_o  + 0

0 0 d_

!

!   R

 Mf _f  0

+

vx vy vz

! !

=

(KoMo _o )y 0  ?(K + = o Mo _o )x 0 ?1 d Tf Mf _f  R Mo_o  0 !  dR KoMo_o  + 0 + 0 0 d_ 0 0

 Kf Mf _f 

!

!y ? !x = + Tf Mf _f !z 0 !  R K M _  o o o 0 ? ToMo_o : _

20

(B.7) (B.8) (B.9) (B.10) (B.11)

In describing the equations relating the angular velocities, we have multiplied the rst line with ?1 and switched it with the second line. After some amount of algebra we get the required form for the contact coordinate derivatives. 2.

C Proof of the Theorem 4.3

Lemma C.1 . Induced velocity of the contact frame

The (body) velocity of the contact frame C relative to the reference frame O of the body is given by Vocb = (voc ; woc ) where " l_ # (C.1) voc = 0o 0 2  l_ + _ 3 o oo !oc = 4 ko sin o l_o 5 ; (C.2) _ ko cos o lo where l(t) is the variation of the intrinsic parameter of the curve in time and ko ; o are the curvature and respectively the torsion of the curve on body O.

Proof Let ~t;~n; ~b be the tangent, the normal and the binormal of the curve. Let RT = [~t;~n; ~b] and

"1

0 0 Ro = 0 cos o ? sin o 0 sin o cos o Then clearly Roc = RT Ro . Let also

"

#

(C.3)

#

0 ko 0 K = ?ko 0 o (C.4) 0 ?o 0 where ko ; o are the curvature and respectively the torsion of the curve. Frenet's relations state that R_T = ?RT Kl_o . Therefore T R_oc = RT RT R_T R + RT RT RT R_ = (C.5) Roc o o o T o T T T T T _ _ (C.6) ?loRo RT RT KRo + Ro RT RT Ro = T T _ _ (C.7) ?lo Ro KRo + Ro Ro It is easy to see that

"0

0 0 # 0 0 ?_o : (C.8) 0 _o 0 T Roc we get the claimed relation for !oc after replacing all the terms in C.7. Since !^oc = Roc The things are easier for voc we have that poc(t) is the parameterization of the curve with respect to O. Therefore p_oc (t) = l_o~t and T p_ oc = l_o RT RT ~t (C.9) voc = Roc " 1 # "ol_ T# (C.10) l_oRTo 0 = 0o 0 0 which completes the proof. 2

RTo R_o =

21

Proof of Theorem 4.3 All the calculations are done adapting the treatment from [7] for this case, similar to the surface-surface contact. Because we are dealing only with body velocities, we drop the superscript b once again. Since the frame Lf (t) is xed relative to the frame F , the velocity of Lf (t) relative to F is given by Vflf = Volo = 0. Therefore using the velocity relationships (A.5),we get: Vfcf = Vlf cf (C.11) (C.12) Voco = Vlo co : The dierence from the surface-surface contact is that , at moment t; Co = Lo Ro and Cf = Lf Rf , with the matrices R being de ned by C.3. Therefore plo co = plf cf = 0 and Rlo co = Ro ; Rlf cf = Rf at moment t. Also ! h i 0 pcocf = 0 ; Rco cf = R0 ?01 (C.13) d The latter relations happen because the z direction of the two contact frames is common, and is the amount of rotation measured around this axis. Now we will compute the velocity of Cf relative to Lo via two intermediate frames, namely Lf and Co . Following (A.5) we have Adgl?1c Vlo lf + Vlf cf = Vlo cf = Adgc?o1cf Vlo co + Vco cf (C.14) f f

One dierence to the surface-surface case is that since Lf and Cf do not coincide anymore but are mutually rotated by an angle f , Adgl?1c is not the identity. After replacing the expressions for the adjoints and using f f (C.11) we obtain !  RT ?RT p^l c  lf cf lf cf f f Vl l + vv~~xy = (C.15) of 0 RlTf cf v~z 0 _ _ 1 _ ! 0! 0! h o lo + o i l o R 0 R 0 + 0 + 0  0 ?1 @ ko sin o l_o A (C.16) d d_ 0 ko cos o l_o or _ ! 0!  ! ! v~x ! ? dko sin o l_o l_f lo R R + 0 ? : (C.17) 0 0 + v~y = do l_o + d_ 0 v~z d_ 0 0 0 The equations involving the rotational velocities (the last three rows) are done by multiplying the rst row by ?1 and permuting it with the second row. 0  0 k sin  l_ 1 _o  1 l k sin  0 ! f ff !~ y ! o o @ ?f l_f ? _ f A + ?!~x = 0 ? @ R ?ol_o ? _ o A : (C.18) _ ! ~ _ _ z ?ko cos o lo kf cos f lf After separating the rst two rows from the last both for translational and rotational velocities we get the conclusion. 2

D Proof of Theorem 4.6

Proof We equal the left-hand sides of the computations from the curve-curve section with the right-hand sides of the computations from the surface-surface section and we get for the translational velocities: ! v~x !   0 !  dR KoMo_o  l_f R M  _ o o + 0 + (D.1) 0 + v~y = 0 0 v~z d_ 0 22

and for the rotational velocities:

0 k sin  l_ 1 f ff !~y ! @ ?f l_f ? _ f A + ?!~x = kf cos f l_f

!~z

0 0 _

!   o Mo _o : ? R TK o Mo _o

After separating the rst two rows from the last for each equation, we get the conclusion.

(D.2)

2

E The explicit value of Vloblf By (A.6) we have that Vlbo lf = Adgfl?1 Vofb = Adg?fl1f Vofb . Vofb is a velocity that can be computed from (A.3) f and it depends on the state of the bodies in contact and not on their particular geometry. Thus the only thing left that is geometry dependent is Adgflf . Most of the notations used here come from the introductory section of the appendix.

E.1 The surface on surface contact

We have that pflf = cf (f ), the local coordinates of the contact point with respect to the body F . We also have that Rflf = [ krruf k krrvf k N ], where f = (uf ; vf ); c = c(f ) is the parameterization of the bodies and uf vf N is the external normal, chosen such that the determinant of Rflf is 1. Rflf is the natural frame on any smooth surface. Now Adgflf is found by (A.4) and subsequently Vlbo lf .

E.2 The curve on curve contact

For this type of contact pflf = cf (lf ), the local coordinates of the contact point with respect to the body F , where lf is the natural curve parameter (the length of the curve with respect to some xed initial point). Hflf = [~t~n~b] the Frenet frame or Frenet trihedron [3, p.19] of the curve in local coordinates, thus with respect to body F . Adgflf is now found by (A.4) after replacing the corresponding expressions of pflf and Hflf . For applying Theorem 4.1 we also need Adgl?1c = Ad?gl1f cf . Since the points lf and cf coincide, we have f f that plf cf = 0. The frames Lf and Cf are rotated by the angle f , thus Hlf cf = Rf with the latter being given by (C.3) with o replaced by f . Adlf cf can now be computed from plf cf and Hlf cf by using (A.4).

E.3 The curve on surface contact

Since the curve is on body F , the approach is identical to the one from the previous case.

References [1] D. Bara. Issues in Computing Contact Forces for Non-Penetrating Rigid Bodies. Algorithmica 10:292352,1993. [2] R.W.Cottle, J.S.Pang, and R.E.Stone. The Linear Complementarity Problem. Academic Press, 1992. [3] M.P. do Carmo. Dierential Geometry of Curves and Surfaces. Prentice-Hall,Englewood Clis,NJ,1976. [4] E.J.Haug. Computer aided kinematics and dynamics of mechanical systems. Allyn and Bacon, 1989. [5] E.J.Haug, S.C.Wu and S.M.Yang. Dynamic mechanical systems with Coulomb friction, stiction,impact and constraint addition-deletion I: Theory. Mechanisms and Machine Theory, 21(5):407-416,1986. 23

[6] P.Lotstedt. Mechanical systems of rigid bodies subject to unilateral constraints.SIAM Journal of Applied Mathematics, 42(2):281-296,1982. [7] D.J.Montana. The kinematics of contact and grasp. In International Journal Of Robotics Research, 7(3):17-32,1988. [8] R.M.Murray, Z.Li, S.S.Sastry.A mathematical introduction to robotic manipulation. CRC Press,1994. [9] R.M.Murray and S.S.Sastry. Grasping and manipulation using multi ngered robot hands. In R.W.Brockett, editor, Robotics: Proceedings of Symposia in Applied Mathematics, Volume 41, pages 91-128. American Mathematical Society, 1990. [10] J.C.Trinkle, J.S.Pang, S. Sudarsky, G.Lo. On Dynamic Multi-Rigid-Body Contact Problems with Coulomb Friction. Texas A &M University, Dept. of Computer Science. Technical Report 95-003. [11] C. Cai and B.Roth. On the spatial motion of rigid bodies with point contact.Proc. 1987 IEEE Conf. on Robotics and Automation 686-695.

24