True Rigidity: Interpenetration-free Multi-Body Simulation with Polytopic Contact  Evan M. Drumwright  Toyota Research Institute  Abstract.   An effective paradigm for simulating the dynamics of robots that lo- comote and manipulate is multi-rigid body simulation with rigid contact. This paradigm provides reasonable tradeoffs between accuracy, running time, and sim- plicity of parameter selection and identification. The Stewart-Trinkle/Anitescu- Potra “time stepping” approach is the basis of many existing implementations. It successfully treats inconsistent (Painlev ́ e-type) contact configurations, efficiently handles many contact events occurring in short time intervals, and provably con- verges to the solution of the continuous time differential algebraic equations (DAEs) as the integration step size tends to zero. However, there is currently no means to determine when the solution has largely converged, i.e., when smaller integration steps would result in only small increases in accuracy. The present work describes an approach that computes the event times (when the set of active equations in a DAE changes) of all contact/impact events for a multi-body simulation, toward using integration techniques with error control to compute a solution with desired accuracy. We also describe a first-order, variable integration approach that ensures that rigid bodies with convex polytopic geometries never interpenetrate. This approach per- mits taking large steps when possible and takes small steps when contact is com- plex.  1   Introduction  Verifying correctness for contact-free multi-body simulations is straightforward. As ex- amples, one can verify that energy remains constant on a conservative system, can com- pare state against the closed form solution for a classical system, e.g., a pendulum, or can compare state against trusted numerical solutions (produced using simulation soft- ware that has been carefully assessed for correctness). Verifying correctness for multi- body simulations with contact is much harder. There are few benchmark problems that possess known solutions for multi-body dynamics simulations with rigid contact, and we are aware of no such benchmarks for bodies with polytopic geometries. Meanwhile, researchers in the robotics, multi-body dynamics, nonsmooth mechan- ics, and computer graphics communities continually propose approaches for increasing simulation speed. When researchers have assessed correctness of an approach, they have done so by searching for qualitative behavior, like rattleback toy spin reversal [16] and pattern generation in vibrated bins of granules [20]. We aim to instead find accurate numerical solutions, thereby motivating the present approach. Approaches for simulat- ing multi-rigid body dynamics with contact would then have to produce   the   result as quickly as possible, rather than   a , perhaps plausible, result (the status quo).  arXiv:1608.02171v1 [cs.RO] 7 Aug 2016
While we envision the present work as leading directly to such an oracle that can produce the correct result, the current approach is usable for typical multi-body dy- namics simulation applications as well: the approach integrates the equations of motion directly to changes in the contact manifold. Since all such changes are detected and interpenetration is prevented, gross artifacts—like objects that are ejected from robots grasps—that frequently appear in robotic simulations should be eliminated. We focus on polytopic geometries, which are not only a popular representation but also allow us- ing the linearized halfspace constraints supported by linear complementarity problem (LCP) formulations without approximation.  2   Related work  This paper builds upon work from research in nonsmooth mechanics, computer science, and mathematics.  2.1   DAEs and DVIs for modeling multi-body dynamics with contact  Multibody dynamics with rigid contact and Coulomb friction—which captures impor- tant stick-slip transitions—can be modeled as a differential algebraic equation (DAE):   ̈ q   =   f   ( q ,    ̇ q ,   u )   (1)  0   =   φ ( q )   (2) where   φ ( . )   is a set of   active   algebraic constraints, out of   m   total constraints. Some constraints are always active, like bilateral joint constraints. Other constraints are only active if certain conditions are met; e.g., a contact constraint between two polyhedra would only be active when the bodies are in contact at that point and they would other- wise (i.e., without the constraint in place) interpenetrate at that point:   ̇ φ i ( q ,    ̇ q ) = 0   if   φ i ( q ) = 0   and   λ i   >   0 ,   for   i   = 1 , . . . , m   (3) where   λ i   acts as a Lagrange Multiplier (i.e., it is zero if the constraint is active and non- negative otherwise). A standard   index reduction   technique seeks to remove all algebraic variables from a DAE, yielding an ODE system explicit for all unknowns. We aim to apply error estimation for general ODEs to DAEs for multi-rigid body dynamics with contact. This requires us to identify   all   such points in time where any constraint becomes active or inactive. Root finding,   MATLAB ’s technique for handling such problems, is prone to missing such times as rigid bodies can make and break con- tact rapidly; if two bodies are not in contact at the endpoints of   [ a, b ]   but are in contact at  1 2   ( a   +   b ) , root finding will fail to find such   events . Between any two consecutive events, ODE-based techniques can be applied for estimating the error. We note that piecewise DAEs with unilateral constraints are generally modeled as differential variational inequalities (DVI) [19], which—for rigid body contact problems, at least—we find more challenging to pose, though existing solution approaches can work reasonably well. These approaches will be discussed in Section 2.3.
Fig. 1: Depiction of Stewart-Trinkle for simulating a ball moving ballistically toward a stationary box, with different fixed step sizes (top to bottom). Only some ball positions are depicted. Contact points lie at the source of arrows; arrows are drawn each time a contact constrains motion; feasible half spaces are drawn as a line segment with dashed stroke. Contact constraints are determined as described in [23, 21]: when bodies are found to be intersecting at   t   +   h   (circle drawn in dashed stroke), the simulation is regressed to   t   and a contact constraint is added. The motion does not qualitatively match the adaptive result (see Figure 7) until the step size is   ∆t/ 8 .
2.2   Event-driven approaches to simulation  Early works in simulating rigid bodies undergoing rigid contact were conducted by L ̈ ostedt [14, 15] and Baraff [4, 5, 6], and used root finding approaches to locate events. These works described some challenges with theory—inconsistent configurations, ex- emplified by the Painlev ́ e Paradox [18] that was well known to the theoretical mechanics community—and computational complexity: for example, Baraff showed that the prob- lem of classifying a contact configuration as inconsistent is NP-hard [6]. These works do not consider the challenges of locating all possible events or the failure to do so.  2.3   Time-stepping approaches to simulation with Coulomb friction  Stewart and Trinkle [23] and Anitescu and Potra [2] described an approach (based on Moreau’s “time stepping” [17] discretization of the rigid body dynamics and differen- tial inclusion theory) that provably mitigated the problem of inconsistent configurations. These authors proved that the complementarity problem [8] based approaches used in these works do non-positive work and always possess a solution if the contact con- straints are linearly independent. Shortly after introducing this method, Stewart proved convergence of the approach to the solution to the continuous time dynamics [22] as the integration step tends to zero. Stewart-Trinkle [21] and Anitescu-Potra [1] later described approaches that, respectively, prevent and minimize interpenetration; Fig- ures 1 and 2 illustrate the similarities and differences. Proofs of convergence and con- straint stabilization rely upon assumptions that the complementarity problem has unique solutions [21] and the   signed distance   function [1] is differentiable. A significant issue with time stepping approaches comes from determining con- straints that hold over the length of time intervals: inconsistent constraints can cause the complementarity problem to fail to have a solution, and movement between bodies may be unphysically constrained.  2.4   Conservative advancement  Mirtich pioneered   conservative advancement   (CA), which uses the distance between bodies and bodies’ velocities and accelerations, to integrate rigid bodies and multi- rigid body systems without missing contact events (a guarantee root finding approaches cannot make [16]). Assuming the typical first order integration process for multibody dynamics with contact, CA (depicted in Figure 8) can use the formula below, where variables    ̇ x   represent linear velocities,   ω   represent angular velocities,   r   is the minimum bounding sphere for the body, and   ˆ d   ≡   d / || d || :  ∆t max   ≡   || d || |   ˆ d T (  ̇ x A   −    ̇ x B   ) |   +   || ω A   ×   ˆ d ||   r A   +   || ω B   ×   ˆ d ||   r B  (4) Proofs of correctness of conservative advancement (i.e., proof that it generates a lower bound on the time of impact) are provided in [16]. The formula indicates that an- gular velocities parallel to   ˆ d —the normal to the contact manifold for “kissing” bodies— do not decrease   ∆t max , the conservative bound. When bodies are contacting, the safe
Fig. 2: Rotated depiction of Anitescu-Potra for simulating a ball moving ballistically toward a sta- tionary box, with different fixed step sizes (top to bottom). Only some ball positions are depicted. Contact points lie at the source of arrows; arrows are drawn each time a contact constrains mo- tion; feasible half spaces are drawn as a line segment with dashed stroke. Contact constraints are determined as described in [1] and as implemented in   ODE : contact constraints added at time  t   only when bodies’ geometries are intersecting at   t . Interpenetration is corrected at time   t   +   h , assuming immediate constraint stabilization and that contact constraints are identical at times   t  and   t   +   h . The motion does not qualitatively match the adaptive result (see Figure 7) until the step size is   ∆t/ 8 .
bound is zero, so a special strategy is necessary. Mirtich kept bodies undergoing sus- tained contact separated, using a “microcollision” approach, which not only results in poor computation rate on some scenarios (like stacking), but also introduces error that does not disappear as the integration step tends to zero. The approach described in the present work instead leverages knowledge that the geometries are convex polyhedra to address this problem. Note that non-convex polytope geometries may be decomposed into unions of convex polytopes (see, e.g., [13]).  Convergence rate of conservative advancement   If bodies initially separated by dis- tance   d   are bound by conservative advancement as moving toward one another at speed  v   but are really moving toward one another at true approach speed of   αv , for   0   < α <   1 , then the following sequence of integration steps will be produced.  d/v,   (( d   −   ( d/v )   ·   αv ) /v ) ,   ( d   −   (( d/v )   ·   αv   + (( d   −   ( d/v )   ·   αv ) /v )   ·   αv )) /v, ...   (5) This series is geometric. The decreasing distance is also a geometric series: it is the series obtained by multiplying each of the terms above by   v . For rigid bodies, this series’ convergence rate is dependent upon the proportion of linear velocity directed toward the contact surface and the magnitude of angular velocity orthogonal to the contact surface. Distances rapidly converge toward zero for bodies coming into contact, and integration steps increase rapidly for bodies moving apart due to the geometric nature of the series. As Figure 3 shows, even fast moving multi-bodies tend to yield large steps.  Fig. 3: The histogram of integration steps for the falling   chain   scenario (see   Section   6.2)   indi- cates that the maximum integration   step   is   se- lected the majority of the time.  3   Computing the contact event times  Conservative advancement permits finding the first time that two bodies come into con- tact. Examination of Equation 4 shows that when bodies are touching ( || d ||   = 0 ), the
conservative advancement formula is unable to provide a next “safe” step. This section addresses this problem of finding a next safe step, thereby permitting locating the next time that the contact manifold changes. Computing the times that the contact manifold (seen in Figure 4) changes relies upon Minkowski’s Hyperplane Separation Theorem.  Hyperplane Separation Theorem   Let   A   and   B   be two disjoint nonempty convex subsets of   R n . Then there exists a non-zero vector   ˆ n   and a real number   σ   such that:  x T   ˆ n   ≤   σ   ≤   y T   ˆ n   (6) for all   x   in   A   and   y   in   B . The hyperplane normal to   ˆ n   and defined using   σ   separates   A  and   B   [7].   V-Clip   can be used, albeit indirectly, to compute a separating plane in 3D in time   O ( m   +   n )   in the features of the two polyhedra.  Fig. 4: The separating plane between two convex polyhedra, a pyramid and a box. Points of contact between the polyhedra and the separating plane are highlighted with red circles.  Unique separating hyperplanes   The separating plane is uniquely defined when two convex polyhedra touch in one of the following ways: at a vertex and the interior of a face, on an edge and a face (with the intersection yielding a line segment), or at two coplanar faces. When two polyhedra touch at two vertices, or at a vertex and the interior of an edge, or at two edges (with the intersection yielding a line segment), the separating plane is not uniquely defined. The latter three cases, which are fortunately rare, are not treated in the present work to avoid overly constraining the motion; these cases can at least be detected and reported, however. This issue is discussed at depth in [24].  3.1   Bounding the time that features can newly contact the separating plane  Assuming that all points touching the separating plane remain on the separating plane, the earliest time that a given vertex   r   of a polyhedron   A   not   already touching the sep- arating plane can contact the separating plane can be bound by dividing the projected distance from the vertex to the plane by the motion bound for the two bodies:  ∆t max r  ≡   |   ˆ n T r   −   σ |  ( || ω A   ×   ˆ n ||   +   || ω B   ×   ˆ n || ) || r   −   ξ ||   (7)
where   ξ   is defined as the point furthest from   r   on the intersection of   A   and the separat- ing plane (see Figure 5). Ignoring vertices on the contact manifold also avoids undefined values where the equation could evaluate to   0 / 0 . The bound for when a new vertex, ei- ther from   A   or from   B , can come into contact with the separating plane is given by:  ∆t ∗   = min  s   ∆t max s  ,   ∀ s   ∈   vertices ( A )   ∪   vertices ( B )   s. t.   |   ˆ n T r   −   σ |   >   0   (8) Practically, one only need test the vertices adjacent to the vertices touching the sepa- rating plane—by convexity, these adjacent vertices must be at least as close as all other vertices not on the separating plane—but this fact does not alter the worst-case time complexity of this operation (linear in the number of features of the two polyhedra).  Fig. 5: Depiction of points from a polyhedron projected onto the separating plane. Dividing the minimum distance from these points to the separating plane by the relative motion that the two bodies can make in unit time yields a safe in- tegration step if the projected   relative   velocities at all contact points (circled in red) are zero at the time pictured.  Finally, if should be clear that if the relative velocities between the two polyhe- dra measured at three non-collinear points on the contact manifold and projected onto the separating plane remain zero throughout [ a, b ]   then any integration step is safe. However, we must conduct the operation described previously as part of the process necessary to ensure that these projected velocities do indeed remain zero.  Theorem 1.   If over interval   [ a, b ] , no vertex from either polyhedron passes through the separating plane defined at   a   and moving according to the relative velocities of the two polyhedra over   [ a, b ] , the two bodies cannot interpenetrate in   [ a, b ] . Proof.   Follows directly from the Separating Axis Theorem.  Theorem 2.   A feature from polyhedron   A   that lies strictly above/below the separating plane at time   t 0   must lie strictly above/below the separating plane during the half- closed, “safe” interval   [ t 0 , t 0   +   ∆t ∗ ) . Proof.   The numerator of   ∆t ∗   is comprised from the minimum distance between any feature on the body and the separating plane: if an oracle were to produce the “true” value of the numerator ( e , hereafter), that value would be at least as large. On the other hand, the denominator ( f   , hereafter)—which bounds the motion of the feature toward the separating plane—provides an upper limit. For any   0   ≤   δ e ,   0   ≤   δ f   < f   yielding the time of contact   t 0   +   e + δ e  f   − δ f   , it is clear that   ∆t ∗   ≤   e + δ e  f   − δ f   .
3.2   Providing a lower bound on the time vertices remain on the separating plane  ∆t ∗   bounds when vertices not already on the separating plane might contact it. For those vertices already on the separating plane, we must examine the velocities projected onto the separating plane at   t 0   to obtain   ∆t † , a lower bound on the time that vertices remain on the (possibly moving) separating plane.    ̇ φ p   will hereafter be denoted the relative velocity of the two bodies at point   p   projected onto the separating plane at   t 0 . Compute   Δ t*:  bound on  adjacent features  time to move to  the   separating  plane;  set   Δ t ✝   ←   Δ t*  Compute  𝝓   =   n ̂   (t ) ⋅ ( p A (t) -   p B (t))  over [t 0 , t 0 + Δ t ✝ ]   ∀ p   ∈  vertices(contact manifold)  Start/Stop  Halve  integration  interval  Δ t ✝   ←   Δ t ✝ /2  d 𝝓 i /dt   ≈   0 at all  ODE evaluations  No  ∃ i   d 𝝓 i /dt   > 0 at t 0 ?  Bodies disjoint  at t 0 ?  Yes  ( return   Δ t max   from Eq 4 )  No  Yes ( return   Δ t min )  No  Yes (return   Δ t ✝ )  Fig. 6: Flowchart for determining   ∆t   =   r ( q ,   v )   (see Algorithm 1).  The formal definition of    ̇ φ p   arises from the distance between a shared point on Body  A   and Body   B , projected along the normal to the separating plane   ˆ n :  φ p   ≡   ˆ n T ( p A   −   p B   )   (9) Constraint solvers (e.g., LCP codes) can ensure that the time derivative of this equation is non-negative at time   t 0   (denoted henceforth as the time of contact) 1 :   ̇ φ p   = ˆ n T (  ̇ p A   −    ̇ p B   ) +  ̇ ˆ n T ( p A   −   p B   )   (10) When the second term above is evaluated at   t 0 , point   p   is shared between the two polyhedra, so   p A   −   p B   =   0 , i.e., the second term goes to zero. We now consider two cases: (  ) the instantaneous velocities of all vertices projected onto the separating plane are zero and (  ) the instantaneous velocity of one or more vertices projected onto the separating plane is non-zero. In the latter case, the multi- bodies must be integrated by some small step,   ∆t min , until those active constraints (for the vertices with non-zero project velocity) become inactive as the distances between  1   Typical time stepping equations can be used for this purpose (see, e.g., [2]) as long as zero is substituted for   h .
those vertices and the separating plane become non-zero (refer to Equation 3). In the former case, we seek the first time (if any) that the time derivative of the signed distance of a vertex from the separating plane becomes non-zero (i.e.,    ̇ φ p ( t )   6   = 0 ).  φ p   can be integrated over an interval   free of changes to the active constraint set  using the ODE above (which requires integrating the equations of motion for Bodies  A   and   B ). Given that the active constraint set remains constant over the interval   [ a, b ] , variable step ODE integrators can compute   φ p   over   [ a, b ]   such that desired error toler- ances are met.   If all evaluations of    ̇ φ p   that are used to compute   φ p   are approximately zero, then   φ p ( t )   ≈   0 ,   ∀ t   ∈   [ a, b ] , subject to the specified error tolerances. If not all evaluations are zero, the active constraint set is going to change, so the step size is halved until all evaluations are zero. If all evaluations are zero but the error tolerances are not satisfied, the error estimates are used to select a new integration step (as de- scribed in [12], Ch. II.4) to meet the desired tolerances.   ∆t †   is determined as depicted in Figure 6.  Theorem 3.   The contact manifold cannot change in the half-open interval  [ t 0 , t 0   + min ( ∆t ∗ , ∆t † )) . Proof.   This proof relies upon the observation that the contact manifold does not change from   t 0   until a vertex from a polyhedron leaves or touches the separating plane.   ∆t ∗  bounds the time that no vertex from either polyhedron can newly contact the separating plane, and   ∆t †   bounds the time that no vertex can leave the separating plane.  4   Integration approach  The approach for integrating bodies over the time interval   [ a, b ]   uses Mirtich’s CA ap- proach to integrate bodies’ states to some time   a < t 0   ≤   b   such that the minimum distance between the bodies at time   t 0   is         1   (with      possibly equal to zero). The process described in the previous section takes over at that point, and ensures that en- suing steps are sufficiently small to detect changes to the contact manifold.  4.1   Conservative advancement and integration  Algorithm 1 describes a first-order integrator. CA can function with higher order inte- gration schemes [16], though computational efficiency is reduced (the integrator may have to re-integrate the same time interval repeatedly). By using Equation 4 to select the integration step, denoted as   h , bodies will not be interpenetrating (excepting small, floating point rounding error) at the end of the step. The function   f   ( . )   on Line 3 is defined as the ordinary differential equation for the equations of motion, which may be defined in minimal or absolute coordinates; the constraint equations are not considered at this point. Rather, bilateral and unilateral constraint forces are computed on Line 4. The constraint model from [9] is applied in the experiments in Section 6, though the models from [23, 2] are usable as well.  Theorem 4.   A positive integration step is always possible.
Algorithm 1   S IMULATE ( t i , ∆t ) Simulates a system of multi-rigid bodies from time  t i   to time   t f   ≤   t i   +   ∆t   using a step size such that contact events are not missed, to the accuracy of the first order integrator. This algorithm assumes that all bodies are at admissible configurations at   { t i ,   q i ,   v i } ; if the initial state of the simulator is feasible, S IMULATE   will keep the constraints feasible thereafter.  1:   h   ←   min ( r ( q i ,   v i ) , ∆t )   .   Compute conservative step (see Figure 6) 2:   q i +1   ←   q i   +   h v i   .   Integrate position forward 3:   v ∗  i +1   ←   v ∗  i   +   h f   ( q i +1 ,   v i )   .   Integrate velocity forward 4:   v i +1   ←   k ( v ∗  i +1 )   .   Determine contacts (if any) and apply constraint impulses 5:   return   { t i   +   h,   q i +1 ,   v i +1 }  Proof.   If bodies’ geometries are disjoint at   t 0 , Equation 4 must return a positive value (infinity if, e.g., the bodies are motionless). If bodies are in sustained contact, Equa- tion 7 will yield a positive value: the numerator must be positive—non-positive values correspond to vertices that lie on the contact manifold and are not evaluated by this equation—and the denominator must evaluate to a non-negative value. If contact be- tween bodies is transient, a small positive value is returned (see Figure 6).  Fig. 7: The adaptive approach described in this paper using a maximum step size of   h   =   ∆t , ap- plied to the scenario depicted in Figures 1 and 2. Adaptive stepping avoids the problems exhibited for time stepping with   h   = 3 ∆t/ 2   (missing the contact completely) and   h   =   { ∆t, ∆t/ 2 , ∆t/ 4 }  (contact is constrained in the wrong direction); see Figure 1.  Determining contact data   When bodies are separated by distance      or less, contact points and surface normals are determined by examining closest features. For polyhedra “kissing” (to floating point tolerances), these features are vertices, edges, and faces, and a pair of closest features might form a contact point, edge, or surface. The intersection of two convex polytopes results in a convex polytope of   O ( n   +   m )   vertices. Thus, solving LCPs corresponding to contact models on these inputs will exhibit running time   O ( n 3 ) , assuming   O ( n )   pivots for Lemke’s Algorithm [8]. Convexity implies that velocities that satisfy the non-interpenetration constraints at these   O ( n   +   m )   vertices also satisfy non- interpenetration constraints anywhere on the convex contact surface. Thus as long as the constraint algorithm is guaranteed to find a solution for forces at the   O ( n   +   m )
contact points—approaches in [23, 2, 10], for example, provide such guarantees—two polyhedra will remain disjoint through some time   t > t 0 . Space limitations prevent further discussion of contact determination.  5   Conservative advancement for multibodies  Determining the distance a link on a multibody can move in unit time along a vector  ˆ d   requires special consideration. If Body   A   is the   i th   link in an articulated body chain (and attached to its parent by joint   i ), the terms that include Body   A   in the denominator of Equation 4 would change to:  |    ̇ x T   ˆ d |   +   || ω || γ 1   +  i ∑  j =1  κ j   (11) where  κ j   =  {  |    ̇ q j   |   if joint   j   prismatic  ||    ̇ q j   || γ j   if joint   j   revolute, spherical, or universal   (12)  γ j   ≡  i − 1 ∑  k = j  d ` k   + 2 r A   (13)  d ` k   =  {  | q k |   +   |    ̇ q k |   if joint   k   prismatic  || ` k +1   −   ` k ||   if joint   k   revolute, spherical, or universal   (14) where    ̇ x   and   ω   are the linear and angular velocities of the robot’s base (which will be zero if the base is affixed to the environment) and   ` j   is the location of the   j th   joint (in the global frame) when all prismatic joints are configured to yield the minimum Eu- clidean distance between successive joints. We determined these formulas by analyzing the illustration in Figure 8 (R); they could also be derived by considering the possible Jacobian matrices with respect to   any point   on a link with spherical geometry; space limitations preclude a full derivation. We have tested that the formulas provide correct bounds on multi-bodies with both revolute and prismatic joints. Similarly, the equation below replaces Equation 7 (i.e., the constrained conservative advancement formula) when Body A is a link in a multibody:  ∆t max r  ≡   |   ˆ n T r   −   σ |  ( || ω A ||   +   ∑ i j =1   ||    ̇ q j   ||   +   || ω B   ×   ˆ n || ) || r   −   ξ ||   (15)  6   Experiments  We have implemented this research in the open source library,   Moby . We use a first- order, velocity-level approach with a no-slip contact model (described in [9]) to obviate the issue of indeterminate contact configurations. Constraint stabilization—which is
Fig. 8: (Left) Conservative advancement (CA) uses the distance between two rigid bodies to bound the motion between bodies over a time interval. For points   p A   and   p B   to collide, they must cover at least distance   || d ||   along direction   d . (Right) Our extension of CA to a multi-body uses Equations 11–14 to bound the movement of Body A (Link   i ) over a unit time interval.  generally used by simulators to correct interpenetration, albeit not without introducing new problems [3, 11]—is disabled in these experiments to gauge constraint violation in its absence. Experimental code is available for review at   https://github.com/ PositronicsLab/wafr16-experiments .  6.1   Rotating box experiment  We constructed the scenario of a die thrown onto a planar surface to assess the effi- ciency of detecting changes to the contact manifold. The initial position, orientation, and velocity of the die was set to an arbitrary (randomly selected) value, and the sim- ulation was run until the die became motionless. The simulation detected 102 changes to the contact manifold for this scenario; examination indicates that the large number of changes was due to chattering. The minimum distance observed between the die and the half-space was   1 . 0 × 10 − 8   m (i.e., there was no interpenetration). The mean time be- tween contact manifold changes was   5 . 3   ×   10 − 3   s, while the mean integration step was  2 . 7   ×   10 − 5   s. The minimum time between contact manifold changes was   1 . 0   ×   10 − 6   s. Therefore, the adaptive simulation technique took, on average, a step 27 times larger than the minimum necessary to detect all contact manifold changes and a mean step 196 times smaller than the largest mean step for detecting contact manifold changes.  6.2   Chain experiment  Another experiment was run for which it was known that there would be zero contact manifold changes because contact never occurs. The experiment uses a   100   m chain suspended   100 . 0001   m above the “ground”, extended   100   m horizontally, and then dropped; the chain is essentially a 100-link pendulum. The simulation was run until im- mediately after the chain skims the ground. When the chain reaches this nadir, the tip is
moving at high speed, stressing the multi-body conservative advancement described in Section 5. The smallest integration step recorded is   6 . 17   ×   10 − 4   s, and the mean inte- gration step is   0 . 0065   s. The nominal integration step is   0 . 01 : the adaptive process uses a step   1 2   times smaller than the optimum step for detecting contact manifold changes. Figure 3 shows a histogram of integration step sizes.  6.3   Quadrupedal trotting experiment  We used the scenario of a quadrupedal robot trotting on a halfspace to assess the effi- ciency of detecting changes to the contact manifold for a multi-body undergoing sus- tained contact. The 18 DoF robot model has been used in prior work (see, e.g., [25]), and uses spherical polyhedron feet with 1,000 vertices each. Gait generation and control is provided by   Pacer   [26]. The robot was simulated trotting for a single gait cycle. The simulation detected 317 changes to the contact manifold for this scenario. The mean time between contact manifold changes was   1 . 5   ×   10 − 3   s, while the mean integra- tion step was   7 . 2   ×   10 − 6   s. The minimum time between contact manifold changes was  1 . 3   ×   10 − 7   s. Therefore, the adaptive simulation technique took, on average, a step size  55   times larger than the minimum necessary to detect all contact manifold changes and a mean step   213   times smaller than the largest mean step for detecting contact manifold changes.  7   Discussion  This work aims to provide the correct result, to a desired tolerance, to the mathematical equations for rigid and multi-rigid bodies with convex polytopic geometries. Whether such rigidity is physically accurate or computationally efficient are inapposite, albeit interesting, questions. Two pieces of work remain to achieve this goal for general multi-body simulations. First, all events that correspond to (  ) hitting joint limits and to (  ) transitioning from sticking to sliding (and vice versa) for dry contact friction must be located. Locating both types of events superficially appears much easier than locating contact events. The second task remaining is that of incorporating the integration error estimates, produced through adaptive integration, into the conservative advancement process. Finally, a caveat remains for users that expect a solution to always be obtainable. Indeterminacy in the rigid contact model with Coulomb friction means that a single solution (i.e., a single trajectory) may not exist; instead, the solutions to some problems may be characterized by “trees”, some of which may possess an infinite branching factor.  Acknowledgements  The author thanks Russ Tedrake and Michael Sherman for excellent feedback. This work was funded by ARO grant W911NF-16-1-0118.
Bibliography  [1] M. Anitescu and G. D. Hart. A constraint-stabilized time-stepping approach for rigid multibody dynamics with joints, contacts, and friction.   Intl. Journal for Numerical Methods in Engineering , 60(14):2335–2371, 2004. [2] M. Anitescu and F. A. Potra. Formulating dynamic multi-rigid-body contact prob- lems with friction as solvable linear complementarity problems.   Nonlinear Dy- namics , 14:231–247, 1997. [3] U. M. Ascher, H. Chin, L. R. Petzold, and S. Reich. Stabilization of constrained mechanical systems with DAEs and invariant manifolds.   J. Mech. Struct. Ma- chines , 23:135–158, 1995. [4] D. Baraff.   Analytical methods for dynamic simulation of non-penetrating rigid bodies.   Computer Graphics , 23(3), July 1989. [5] D. Baraff. Coping with friction for non-penetrating rigid body simulation.   Com- puter Graphics , 25(4):31–40, 1991. [6] D. Baraff.   Fast contact force computation for nonpenetrating rigid bodies.   In  Proc. of SIGGRAPH , Orlando, FL, July 1994. [7] S. Boyd and L. Vandenberghe.   Convex Optimization . Cambridge University Press, 2004. [8] R. W. Cottle, J.-S. Pang, and R. Stone.   The Linear Complementarity Problem . Academic Press, Boston, 1992. [9] E. Drumwright.   Rapidly computable viscous friction and no-slip rigid contact models.   arXiv , 2015. [10] E. Drumwright and D. A. Shell.   Modeling contact friction and joint friction in dynamic robotic simulation using the principle of maximum dissipation. In   Proc. of Workshop on the Algorithmic Foundations of Robotics (WAFR) , 2010. [11] E. M. Drumwright and J. C. Trinkle.   Contact simulation.   In   Handbook of Hu- manoid Robotics . Springer, (under review). [12] E. Hairer, S. P. Nørsett, and G. Wanner.   Solving Ordinary Differential Equations I: Nonstiff Problems, 2nd. ed.   Springer, 2008. [13] J.-M. Lien and N. Amato. Approximate convex decomposition of polyhedra and its applications.   Computer Aided Geometric Design (CAGD) , 25(7):503–522, 2008. [14] P. L ̈ ostedt. Mechanical systems of rigid bodies subject to unilateral constraints.  SIAM Journal on Applied Mathematics , 42(2):281–296, 1982. [15] P. L ̈ ostedt. Numerical simulation of time-dependent contact friction problems in rigid body mechanics.   SIAM J. of Scientific Statistical Computing , 5(2):370–393, 1984. [16] B. Mirtich.   Impulse-based Dynamic Simulation of Rigid Body Systems .   PhD thesis, University of California, Berkeley, 1996. [17] J. J. Moreau. Standard inelastic shocks and the dynamics of unilateral constraints. In G. del Piero and F. Maceri, editors,   C.I.S.M. Courses and Lectures , volume 288, pages 173–221. Springer-Verlag, Vienna, 1985.
[18] P. Painlev ́ e. Sur le lois du frottement de glissemment.   C. R. Acad ́ emie des Sciences Paris , 121:112–115, 1895. [19] J.-S. Pang and D. E. Stewart. Differential variational inequalities.   Math. Program., Ser. A , 113:345–424, 2008. [20] B. Smith, D. M. Kaufman, E. Vouga, R. Tamstorf, and E. Grinspun.   Reflec- tions on simultaneous impact.   ACM Trans. on Graphics (Proc. of SIGGRAPH) , 31(4):106:1–106:12, 2012. [21] D. Stewart and J. C. Trinkle.   An implicit time-stepping scheme for rigid body dynamics with Coulomb friction. In   Proc. of the IEEE Intl. Conf. on Robotics and Automation (ICRA) , San Francisco, CA, April 2000. [22] D. E. Stewart. Convergence of a time-stepping scheme for rigid-body dynamics and resolution of Painlev ́ e’s problem.   Arch. Ration. Mech. Anal. , 145:215–260, 1998. [23] D. E. Stewart and J. C. Trinkle. An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and Coulomb friction.   Intl. J. Numerical Meth- ods in Engineering , 39(15):2673–2691, 1996. [24] J. Williams, Y. Lu, and J. C. Trinkle. A complementarity based contact model for geometrically accurate treatment of polytopes in simulation. In   Proc. ASME Intl. Design Engr. Tech. Conf. and Comput. and Inform. in Engr. Conf. , 2014. [25] S. Zapolsky and E. Drumwright. Quadratic programming-based inverse dynamics control for legged robots with sticking and slipping frictional contacts. In   Proc. IEEE/RSJ Intl. Conf. Intell. Robots & Systems (IROS) , 2014. [26] S. Zapolsky and E. Drumwright.   Pacer: Modular, real-time software for legged robot planning and control.   In   Proc. 11th Annual Conf. on Dynamic Walking , 2015.