We present a formulation of nonpenetration constraint between pairs of polytopes which accounts for all possible combinations of active contact between geometric features. This is the first formulation that exactly models the body geometries near points of potential contact, preventing interpenetration while not overconstraining body motions. Unlike many popular methods, ours does not wait for penetrations to occur as a way to identify which contact constraints to enforce. Nor do we overconstrain by representing the free space between pairs of bodies as convex, when it is in fact nonconvex. Instead, each contact constraint incorporates all feasible potential contacts in a way that represents the true geometry of the bodies. This ensures penetration-free, physically correct configurations at the end of each time step while allowing bodies to accurately traverse the free space surrounding other bodies. The new formulation improves accuracy, dramatically reduces the need for ad hoc corrections of constraint violations, and avoids many of the inevitable instabilities consequent of other contact models. Although the dynamics problem at each time step is larger, the inherent stability of our method means that much larger time steps can be taken without loss of physical fidelity. As will be seen, the results obtained with our method demonstrate the effective elimination of interpenetration, and as a result, correction-induced instabilities, in multibody simulations.

## Introduction

There is a wide spectrum of applications for simulation, but an understandable bias toward real-time interactivity has bred a culture that leaves concerns of physical accuracy in a dark corner. As a result, applications which require some level of physical fidelity from simulation are left with a relatively underdeveloped set of tools. The major focus of the work presented herein is on improving the simulation accuracy, and consequently stability, without significantly increasing the computational cost.

The tradeoff of speed versus accuracy in simulation is a classic one, and many applications simply do not gain from improved physical accuracy. The gaming community is naturally concerned with methods that are fast and reliable, but not necessarily accurate [1–3], inspiring the term “game physics.” Many games, in fact, intentionally incorporate nonrealistic physics into their play. The graphics community, which is constantly pushing the boundaries of realistic-looking visual effects, does not offer many improvements to physically accurate simulation. Although there are many examples in a long history of “physics-based” methods in use in animation [4–8], there is really no need for such methods to do more than approximate some convenient alternative version of reality. Indeed, the goal in many graphics applications is to present the viewer with something that “feels” correct.

Particularly in the field of robotics, physical fidelity is a pressing concern and is especially challenging when simulating systems experiencing intermittent frictional contact, which is the case when robots work in unstructured, cluttered environments. Robotics experiments tend to be exceedingly costly and time consuming, so that the ability to perform realistic experiments in simulation would be a great benefit. Although work has been done regarding validation of simulation [9–12], the problem of measuring and quantifying the physical accuracy of simulation is an open issue that is challenging to solve. Comparison of a particular simulator's results with those obtained from a commercial simulator is meaningless without proof of the commercial simulator's accuracy. Even validation of a simulator with a physical experiment can be misinterpreted when it takes the tuning of several parameters to match a single or small number of experiments. Such an attempt at validation says little, if anything, of the simulator's ability to predict a different experiment.

Simulation can be a powerfully useful tool if shown to be accurate. A simulation requires accuracy in order to make any claims as to its ability to predict behavior in the physical world, yet the growing number of variants on old simulation methods all have at least this in common: they sacrifice physical fidelity at multiple stages in order to achieve computational speed-up. The ideal simulation tool would be both accurate and fast, but when we sacrifice accuracy, we are necessarily sacrificing the usefulness of that tool.

The polytopal exact geometry (PEG) contact model presented herein is a step toward improving the physical fidelity of simulations of polytopal rigid bodies in intermittent contact. In most popular simulation tools, in order to make it easy to formulate the nonpenetration constraints, the shapes of the bodies are effectively altered. Specifically, facets of polytopes are represented as hyperplanes (e.g., faces of polyhedra in 3D are represented as planes). As will be illustrated below, the price for this convenience is very high, as it leads to nonphysical simulation artifacts, such as huge constraint forces that eject bodies from the scene, or worse, cause the simulation's solvers to fail completely. By contrast, PEG exactly represents the boundaries of polytopal bodies, including their finite extent, and thus avoids these problems. To achieve this, the PEG contact model is more complicated than the standard contact model. With the same time-step sizes, simulation with PEG takes approximately twice as long as simulation with the standard contact model. However, its improved accuracy avoids nonphysical artifacts, including penetration-induced instabilities that can crash a simulation. In our opinion, this speed reduction is a penalty worth paying.

In the remainder of this section, we review multibody dynamics and some of the approaches used in the simulation. In Sec. 2, we detail the deficiencies of standard contact modeling practices and begin to derive the PEG contact model by introducing new approaches to cases that standard contact models fail to accurately represent. In Sec. 3, we define the general formulations of three classes of nonpenetration constraints and provide abstractions of them in order to model contact in a clean way. In Sec. 4, we precisely define contact constraint applicability and feasibility, and establish geometric tests to determine which contacts to include in PEG. In Sec. 5, we utilize the abstractions from Sec. 4 in order to define four fundamental contact constraints for spatial problems, which reduce to two when specialized to the planar case. We then demonstrate how to formulate a time-stepping subproblem that incorporates PEG. In Sec. 6, we compare the performance of multibody simulations using a popular time-stepping method with the standard contact model against the same time stepper with the PEG contact model. Finally, Sec. 7 discusses our results and possibilities for future work.

### Previous Work.

The two main classes of simulation methods for multibody systems are known as *event-driven* and *time-stepping*. In event-driven methods, the simulation is monitored for impact and separation events as well as changes between sticking and slipping (see Ref. [13]). For systems with small numbers of contacts, this method is preferred, because it uses an adaptive step size to accurately pinpoint and process the contact state transitions. Its downfall is that in systems with many bodies and contact events, the size of the time step becomes so small that computation times become impractically large. On the other hand, this is precisely the circumstance in which time-stepping methods have their greatest value. Time-stepping methods use large time steps that can advance the simulation across a number of contact events in one step, such that at the end of the time step, Newton's laws are satisfied and bodies do not overlap [14].

Time-stepping simulation methods utilizing the *complementarity problem* (defined formally below) were introduced by Moreau and Panagiotopoulos [15]. In these methods, the data needed to advance the simulation by one time step are obtained as the solution of a complementarity problem, the so-called *time-stepping subproblem*. Stewart and Trinkle introduced a modification of this problem that incorporated stabilization of nonpenetration constraints [16], and they provided a proof that body trajectories approximated by the solutions to a sequence of these subproblems converged to a solution of the instantaneous-time formulation as the step size goes to zero [17]. Anitescu and Potra [18] included bilateral constraints and showed that the subproblem with linearized friction cones is always solvable.

Physics engines such as DART [19] use semi-implicit, velocity-based time-stepping subproblems, formulated as linear complementarity problems with nonpenetration constraint stabilization. However, because it uses standard contact modeling, it can suffer from inaccuracy and instability as discussed above. Chrono::Engine [20] is built on top of the Bullet collision detection engine [21], which reports penetration depth and gap distance, implying that penetration is not avoided. Across the gamut of available physics engines, simulations with bodies modeled as spheres or unions of spheres are popular for benchmarking, especially when there are large numbers of contacts. This is because when simulating polytopes in contact using the standard contact model, simulations can fail due to nonphysical, overconstraint imposed on the polygons. These overconstrained situations, henceforth referred to as *standard-model traps* (SM traps), can cause bodies to obtain huge nonphysical velocities or result in complete failure of the simulation.

### Background.

In this section, we will introduce an *instantaneous-time model* of polyhedral bodies in intermittent contact, and then discretize it to obtain the discrete-time counterpart. To simulate the motions of the bodies, the *discrete-time model* (i.e., the time-stepping subproblem) must be formulated and solved repeatedly, each time advancing the simulation forward in time by the chosen time step. Note that the development herein excludes contact friction in order to maintain a sharp focus on the new contact model, which depends only on body geometry and not friction. Friction can, however, be easily incorporated by augmenting the time-stepping subproblem with one's favorite friction model, several of which are detailed in Refs. [22,23].

Before formulating PEG and a time-stepping subproblem that employs it, it is important to introduce the complementarity problem [24].

#### Complementarity.

The complementarity problem offers a convenient way to model contact because it naturally represents disjunctive conditions. The two key cases in multibody dynamics are: two bodies are in contact OR not in contact and an existing contact is sticking OR sliding. There is a vast body of research on complementarity problems and methods for their solution [25,26].

*n*-dimensional Euclidean space. Given a mapping $f(z):\mathbb{R}n\u2192\mathbb{R}n$, the complementarity problem is to find the unknown vector $z\u2208\mathbb{R}n$ that satisfies the three conditions

*is the matrix transpose operator. For the sake of brevity, we may write all three conditions as*

^{T}where $\u22a5$ represents the orthogonality of condition 3 in Eq. (1). It is common to refer to all three of these conditions jointly as a *complementarity condition.*

There are many general-purpose solvers available for complementarity problems [27] and in particular for linear complementarity problems (LCPs) [24,28]. In an LCP, $f(z)$ is a linear function of $z$, i.e., $f(z)=Az+b$, where $A$ is a known constant square matrix, and $b$ is a known constant vector of compatible size. Much theory for LCPs has been developed and used to design reliable solvers. It is known, for example, that if $A$ is positive definite, then the LCP is equivalent to a convex quadratic program, and therefore has a unique solution and can be solved efficiently. If $A$ is positive semidefinite, then a solution can be found efficiently also, but some unknowns may not have unique solutions. In multibody dynamics, this case arises only for frictionless systems, and for these, the body motions have unique solutions, but the contact forces do not.

If friction is included in the model, these nice properties are no longer present. Time steppers using polyhedral friction cones still yield LCPs, but $A$ becomes indefinite, and correspondingly, the solutions are not unique; they might not exist and when they do, there can be infinitely many. In the special case when all contacts are sticking and Coulomb friction cones are approximated by four-sided pyramids, the solutions are known to exist [29]. However, the problems are more challenging to solve if the friction model is nonlinear, as is the case for quadratic Coulomb friction, where no existence results are available. Nonetheless, some solvers have success solving the nonlinear complementarity problems (NCPs) that result. We utilize the general-purpose path solver for many NCP-based simulations. Work has been done to create specialized solvers for multibody dynamics [30].

#### Instantaneous-Time Model.

Consider two bodies $A$ and $B$ as depicted in Fig. 1. When the signed gap distance *ψ _{n}* is positive (as in the figure), the bodies are separated. When

*ψ*is zero or negative, they are in contact or interpenetrating. Ideally, the latter is not permitted, that is, $\psi n\u22650$. Generally, a contact between two bodies may be represented as a five-tuple $C={Aid,Bid,pa,n\u0302,\psi n}$, where $Aid$ and $Bid$ are body indices or identifiers for $A$ and $B$, respectively, $pa$ is the contact point on $A$ in world coordinates, and $n\u0302$ is the contact normal vector in the direction of $A$ onto $B$ by convention. The point of contact $pb$ on $B$ is available by $pb=pa+\psi nn\u0302$. The vectors $ra$ and $rb$ span from the center of mass of each body to the point of contact on that body. While not required, we use the convention that the origin of a body's body-fixed reference frame is coincident with its center of mass. Vectors $t\u0302$ and $o\u0302$ form an orthonormal basis with $n\u0302$.

_{n}*i*th block is the $(6\xd76)$ inertia matrix of the

*i*th body

*m*and $(3\xd73)$ inertia tensor $Ji(q)$, $\nu \u02d9$ is the time derivative of the vector of generalized velocities $\nu $ with $\nu i=[viT\omega iT]T$, $\lambda ext$ is the vector of generalized forces acting on the bodies (gravity, velocity product, and centripetal forces), $\lambda $ is the vector of contact force magnitudes (one element for each contact), and $G(q)$ is composed of the corresponding contact Jacobians where for the $jth$ contact

_{i}where $rij$ is the vector from the center of mass of body *i* to the point of application of the force due to the *j*th contact as in Fig. 1 above. The generalized body configurations are represented by $q=[xTqT]T$ for position $x$ and unit quaternion *q* [26].

for a contact where $\psi (q,t)$ is the signed gap distance which is dependent on the configuration of the bodies $q$ and time *t*, and *λ _{n}* is the magnitude of the force applied in the normal direction $n\u0302$ of the contact. Just as we will do for $G$ and $M$ below, we drop the functional dependence $(\xb7)$ to simplify notation.

The orthogonality condition of Eq. (4) enforces for each contact that if the force being applied is positive (compressive), then the gap distance must be zero, and if the gap distance is positive, then the force must be zero. The inequalities imply that the force cannot be tensile and the bodies cannot overlap. In the frictionless case, the continuous-time dynamic model of rigid bodies in contact is composed of Eqs. (3) and (4).

#### Discrete-Time Model.

*h*from step $\u2113$ to $\u2113+1$ to obtain

where $p$ denotes an impulse over the time step, i.e., $p=\lambda h$.

Equations (8) and (9) define the time-stepping subproblem of Stewart and Trinkle [16] without friction. The first row of Eq. (8) is the discrete-time Newton–Euler equation. The second row is merely a definition used to shorten the statement of the normal complementarity condition (9). Importantly, Eq. (9) defines the *standard model* of unilateral contact used widely in multibody simulators. The term $\psi n/h$ appearing in the standard model acts as a constraint stabilization term, which if removed, allows interpenetrations to occur and then remain indefinitely. Regardless of whether term $\psi n/h$ is removed or retained, the inequality on the left-hand side of condition (9) is the source of SM traps that we will see PEG eliminates.

#### State Update.

Since Eq. (10) does not preserve length, each quaternion should be renormalized after each time step.

## A New Approach

One approach to multibody simulation is to allow interpenetrations (and other constraint violations) to occur and then apply whatever hacks are necessary to catch poorly defined geometric configurations that inevitably result. Our approach attempts to prevent nonphysical configurations from occurring. In order to achieve this, we must detect potential contacts before interpenetration occurs.

Contact constraints have been traditionally modeled as fundamentally unilateral, however, representing features as infinite half-spaces at points of potential contact generates nonphysical behavior at corners where multiple finite features meet and terminate. Consider a vertex *v* near a corner as depicted in Fig. 2. When the collision detection routine identifies both $C1=C(v,e1)$ and $C2=C(v,e2)$, the vertex becomes erroneously trapped in the convex subspace of the free space surrounding the corner if both contacts are enforced. We refer to this as the *standard-model trap* (SM trap).

Figure 3 shows a simple situation in which two SM traps collude to generate a time-stepping subproblem with no solution. A common heuristic in collision detection is to choose contacts with deepest penetration. These contacts are then used to formulate a dynamics problem to move the simulation forward in time. Since the standard contact model requires that by the end of the next time step, both of the identified contacts must be non-negative, an impossible problem is generated. Poor contact choices lead to poorly defined dynamics problems, resulting in the all too familiar “popping” or “exploding” bodies in a simulation. Note that allowing bodies such as those in Fig. 3 to first interpenetrate before identifying contacts does not avoid this erroneous contact problem, as the same heuristic results in similarly poorly defined dynamics. Fundamental errors such as these are what lead to common ad hoc “corrections” to these problems, such as limiting the maximum magnitudes of contact forces.

### Deriving PEG, the New Model.

*v*approaching a corner composed of two edges

*e*

_{1}and

*e*

_{2}. We identify two potential contacts $C1=C(va,e1)$ and $C2=C(va,e2)$ with corresponding gap distances

*ψ*

_{1}and

*ψ*

_{2}. We wish to enforce that

allowing *v* to penetrate either half-space corresponding to *e*_{1} or *e*_{2}, but not both.

Lemma 1.1. Given $a,b\u2208\mathbb{R},max(a,b)=a+max(b\u2212a,0)\u22650$.

*Proof*.

Case $a\u2265b\u21d2a+max(b\u2212a,0)=a+0=a$.

Case $a\u2264b\u21d2a+max(b\u2212a,0)=a+(b\u2212a)=b$.◻

Lemma 1.2.*Given*$a,b\u2208\mathbb{R},b=max(a,0)\u21d40\u2264b\u2212a\u22a5b\u22650$.

*Proof*.

Case $a\u22640$ : $max(a,0)=0\u21d2b=0\u21d20\u2264b\u2212a\u22a5b\u22650$.

Case a > 0: $max(a,0)=a\u21d2b=a\u21d20\u2264b\u2212a\u22a5b\u22650$.

Case $b\u2212a=0,b\u22650$ : $b=max(a,0)=a$.

Case $b=0,b\u2212a\u22650$ : $a\u22640\u21d2max(a,0)=0=b$.◻

To give some insight into the slack variable *c* and its relationship with *ψ*_{1} and *ψ*_{2}, consider the figure below (Fig. 4) and the value of *c* on each side of the bisector where $\psi 1=\psi 2$. A vertex in the free space right of the bisector has $\psi 1>\psi 2\u21d2\psi 1\u2212\psi 2>0$, which requires *c* = 0 to satisfy the complementarity condition of Eq. (12). A vertex to the left of the bisector has $\psi 1<\psi 2\u21d2\psi 1\u2212\psi 2<0$, which requires $c=\psi 2\u2212\psi 1$.

We now wish to include potential forces *λ*_{1} and *λ*_{2} while ensuring that only one is nonzero since allowing both to be simultaneously nonzero would be nonphysical.

Lemma 1.3.*Given*$a,b\u2208\mathbb{R},\u2009b=|min(a,0)|\u21d40\u2264b+a\u22a5b\u22650$.

*Proof.*

Case $a\u22640$ : $|min(a,0)|=\u2212a=b\u21d2b+a=0\u21d20\u2264b+a\u22a5b\u22650$.

Case a > 0: $|min(a,0)|=0=b\u21d2(a+b)b=0\u21d20\u2264b+a\u22a5b\u22650$.

Case $b+a=0,b\u22650$ : $a\u22640\u21d2|min(a,0)|=\u2212a=b$.

Case $b=0,b+a\u22650$ : $a>0\u21d2|min(a,0)|=0=b$.◻

Lemma 1.4. *Given*$a,b\u2208\mathbb{R},\u2009a=0,b\u22640\u21d4(max(a,b)\u22650)\u2227(max(a,b)+|min(a,0)|=0)$.

*Proof*. The only way to satisfy both $max(a,b)\u22650$ and $|min(a,0)|\u22650$ is for both to reach equality. When $|min(a,0)|=0$, it is possible that $a\u22650$, so in order to have $max(a,b)=0$, we must have $a=0,b\u22640$. The argument for the reverse of *a* and *b* is similar.◻

Essentially, we have taken a set of contacts, in this case *C*_{1} and *C*_{2}, and replaced a logical AND condition of $C1\u2227C2$ with a NAND condition of $C1\u2227\xafC2$. Geometrically, this corresponds to replacing line representations of a body with line segments. At this point, Eqs. (12)–(14) correspond to the locally nonconvex (LNC) model proposed by Nguyen [32], which models the case for a single vertex against multiple edges. Although this model has been shown to contribute some improvement in physical fidelity over other methods [33], stopping here would correct the standard-model trap at the expense of permitting interpenetrations [34,35]. To illustrate this error, let us continue with a less trivial case.

Consider two bodies approaching as depicted in Fig. 5. In this configuration, our collision detection routine identifies four potential contacts. These are $C1=C(va,eb1)$, $C2=C(va,eb2)$, $C3=C(vb,ea1)$, and $C4=C(vb,ea2)$. We should not simply constrain all of these contacts, for this would result in a dual standard-model trap. It is possible to make a guess as to which contacts to enforce, but this inserts the likely possibility of generating contact forces where there are none. Of course, we could also ignore all four contacts and allow interpenetration, but this too is clearly a nonphysical approach and leads to instabilities.

*C*

_{1}and

*C*

_{2}should both be able to be violated, but not simultaneously. This is similarly true for

*C*

_{3}and

*C*

_{4}. We have seen how these constraints can be represented with

but we make the important observation that this model erroneously allows for *ψ*_{1} and *ψ*_{4} or *ψ*_{2} and *ψ*_{3} to be simultaneously violated. An example of such a consequence is depicted in Fig. 6. The model of Eq. (15) can be completed by including constraints to prevent such interpenetrations. That is, we additionally constrain $max(\psi 1,\psi 4)\u22650$ and $max(\psi 2,\psi 3)\u22650$ with

Equations (15) and (16) together represent our first example of a geometrically accurate contact model. We will refer to the constraint type in Eq. (15) as *intercontact* constraint and the type in Eq. (16) as *cross-contact* constraint. These, along with unilateral constraint, form the set of three contact constraints necessary for our model.

## Three Fundamental Contact Constraints

In order to resolve the issues discussed in Sec. 2, we had to change the way we formulate contact constraints. In this section, we define three fundamental contact constraints in their general form and begin to describe how they can be applied.

### Unilateral Contact Constraint.

*C*and write it as

_{i}where *λ _{i}* is the force corresponding to contact

*C*.

_{i}### Intercontact Constraint.

An intercontact constraint ($I$ -constraint) is that which was demonstrated in Eq. (15) for preventing the standard-model trap. An $I$ -constraint deals with contacts of a single feature of one body against multiple features of another body. Here, we will define the general formulation of $I$ -constraint for a vertex of $A$ against multiple features of $B$ where a subset of these potential contacts may feasibly result in a contact force at the end of the current time step.

Consider two sets of contacts: $C\iota $ which is composed of all contacts which could feasibly result in a contact force, and $C\kappa $ which contains all contacts that could not feasibly result in a contact force but are necessary to accurately represent the contact geometry. An $I$ -constraint seeks to

constrain at least one gap distance $\psi {\iota ,\kappa}$ of ${C\iota ,C\kappa}$ to be non-negative

permit a nonzero contact force $\lambda \iota i>0$ for at most one contact in $C\iota $

only permit $\lambda \iota i>0$ when $\psi \iota i=0$ and all other $\psi \iota ,\kappa $ are nonpositive

Equations (20) and (21) enforce that at least one contact in ${C\iota ,C\kappa}$ is non-negative. Equations (22) and (23) allow at most one contact force to be nonzero, and only permit this force to be nonzero when all other gap distances are nonpositive. Solving such a set of constraints is possible as a linear program with complementarity constraints (LPCCs) [36], or by introducing an auxiliary variable to convert inequalities (21) into a linear complementarity conditions before using an LCP solver [32].

### Cross-Contact Constraint.

In contrast to $I$ -constraint which deals with a single feature of $A$ against multiple features of $B$, a cross-contact constraint ($X$ -constraint) deals with sets of contacts that may not depend on the same features, but could result in interpenetration if simultaneously violated, e.g., Eq. (16).

Given the sets of contacts $Ca$ and $Cb$, a $X$ -constraint attempts to constrain either

at least one non-negative

*ψ*in $Ca$_{a}OR

at least one non-negative

*ψ*in $Cb$_{b}

and can write the LCP in multiple forms. Conveniently, $X$ -constraints can be written in the form of Eqs. (20) and (21). That is, $X(Ca,Cb)$ is similar to $I(Ca,Cb)$, but excludes Eqs. (22) and (23) which involve contact forces.

Although a $X$ -constraint necessarily deals with contacts between the same pair of bodies, the contacts need not share features. That is, the geometric interdependencies of identified potential contacts, regarding which are to be enforced as defined by an $X$ -constraint, may be between two otherwise seemingly independent contacts, e.g., opposite corners of the face of a box. Subsequently, it is conceivable for two potential contacts, neither of which have a potential force associated with them, i.e., contacts in two different sets of $C\kappa $, to be $X$ -constrained. In such cases, the dynamics problem requires sufficient $U$ - or $I$ -constraints so as to allow generation of forces which may contribute to achieving configurations that will satisfy all constraints. This issue, however, while an important one when employing a heuristic approach to choosing primary contacts in $I$ -constraints, is beyond the scope of this paper.

## Contact Between Moving Geometries

In this section, we briefly discuss the nature of interpolytope contact and define a set of geometric tests that are useful for identifying and making heuristic choices regarding potential contacts.

We define a convex polyhedron in terms of features (Fig. 7). We may refer to a vertex either as an object *v* or as the vector $v$ representing the vertex's position. Similarly, an edge object $e=(vt,vh)$ corresponds to a vector $e=vh\u2212vt$. A body is composed of a set of vertices *V*, edges *E*, and faces *F* defined by three vertices in counterclockwise order. The outward pointing normal vector $\eta \u0302$ for a given face $f\u2208F$ is perpendicular to *f*. We define two vectors $t1$ and $t2$ for each edge that are planar with *f*_{1} and *f*_{2}, respectively, and perpendicular to $e$. These vectors are defined as

for convex bodies.

As we develop these tests, we keep in mind that a contact can occur anywhere on a polytope, but the possible normal vectors are restricted by the contacted feature. Figures 8–12 depict the possible positions and orientations of contact normal vectors for each feature in two and three dimensions. Since a vertex does not have a uniquely defined normal, we use the normal from the edges that connected to the vertex. It is important to realize that when using a time-stepping method, we hope to identify contacts which will reflect these feasible contact normals at the *end* of the time step. Yet, many methods of contact identification use approaches that assume continuous or static body positions. This is why we geometrically relax the following set of tests that will prove useful for identifying and making heuristic choices regarding potential contacts.

Consider the simple example in Fig. 13 where the two vertices, $va1$ and $va2$, of body $A$ are near an edge *e _{b}* of body $B$. In order to achieve stability in such a configuration, it is clear that both potential contacts $C1=C(va1,eb)$ and $C2=C(va2,eb)$ must be considered by the dynamics formulation. If only one were included, e.g.,

*C*

_{1}, then it is possible for the other to penetrate

*e*given a large enough time step or velocity. We observe that the second contact normal

_{b}*n*

_{2}is outside of the normal cone region of $va2$ and does not correspond to a physically reasonable contact normal. However, this is because

*n*

_{2}is determined at the beginning of the time step; yet, we wish to prevent interpenetration for the end of the time step. In order to avoid interpenetrations, we must detect and consider such contacts as

*C*

_{2}, even when they are not physically feasible at the current time step. This necessity requires that we geometrically relax the normal cone region or, equivalently, relax the definition of applicability.

### Contact Applicability.

The time required to solve a time-stepping subproblem, and therefore the time required to compute a simulation, is strongly dependent on the number of contacts *n _{c}*. While it may seem that the number of contacts is determined by the simulation, taking a close look at an individual discrete time step, as we do in Sec. 1.2.3, reveals that to enhance simulation accuracy and reduce instability, it is beneficial to include “potential contacts” even though they increase the problem size. We slightly modify classical

*applicability*conditions to derive a method to choose a small set of extra contacts.

*v*. Then, we can express the applicability condition as the Boolean function

_{a}where $\eta \u0302b$ is the outward unit normal of *e _{b}*, and the superscript ve denotes that the test is for a vertex and an edge.

Notice that the applicability test is dependent on $q$; more specifically, it is dependent on the relative orientation of the polygons, and not their relative translation. Later, when considering potential contacts to include in a time-stepping subproblem, we will introduce a relaxed form of inequality (28) to allow for possible rotation in a time step. In the case of our example, if the top polygon rotated enough in the clockwise direction in the time-step under consideration, contact could occur first at vertex $\nu a2$ rather than vertex $\nu a1$.

#### Relaxation of 2D Applicability.

*v*. We refer to

*v*having applicability with

_{a}*e*when $APPL{ve}(q,va,eb)\u2265\u03f5\theta $ for relaxation parameter

_{b}Whereas classical applicability is a Boolean function, our function returns a value as we may wish to use the value of applicability as a heuristic when comparing contacts.

#### Relaxation of 3D Applicability.

Figure 14 depicts two edges, *e _{a}* and

*e*, in contact. Just as there are vertex–face configurations that have proximity but could not result in a force, so too are there edge–edge configurations that should not be associated with potential contact forces. First, we should note that given an edge–edge contact normal $n=ea\xd7eb$, we cannot immediately say if we have the correct sign for $n$. However, we can still determine edge–edge applicability in the following manner. Let

_{b}### Contact Feasibility.

Feasibility is similar to applicability, but is dependent on feature positions as opposed to orientations. The region of feasibility is essentially the region in which we anticipate a contact between a vertex *v* and a facet *f* could occur. We define vertex–edge and vertex–face feasibility similarly. Given an angle $\varphi $ and a small value *δ* greater than machine precision, a vertex with contact $C(va,fb)$ has feasibility if

We can also write an edge–edge feasibility in 3D, which checks only to see if the nearest two points *p _{a}* on

*e*and

_{a}*p*on

_{b}*e*are within some small distance $\u03f5$.

_{b}## Polytopal Exact Geometry

From the set of fundamental constraints presented in Sec. 3, which are written in terms of complementarity problems, abstract out another step and look at pairs of bodies by cases of feature pairs.

This abstraction layering is depicted in Fig. 16. At the lowest level is the complementarity problem which is utilized first as a model of unilateral contact with force, but further as a tool for generating logical NAND conditions on subsets of potential contacts. Sets of these low-level constraints can be entirely represented by abstracting to the three fundamental constraints of unilateral, intercontact, and cross-contact. In turn, the fundamental constraints are formulaically generated by the highest abstraction layer which looks only at what pairs of features between two bodies are near each other. In Sec. 5.2, we will see how precisely the same abstraction can be developed in 3D.

### PEG 2D.

We consider two contact configurations in 2D: vertex–edge and vertex–vertex. We do not consider edge–edge because this case is redundantly covered by the other two configurations [38]. To be clear, edge–edge penetration in 2D is impossible if vertex–edge and vertex–vertex constraints are properly enforced, simply because edges defined and composed of vertices, and for two edges to come near each other implies that the vertices of at least one of those edges are nearing the other edge. We can formulaically generate the set of constraints for the two 2D configurations as given below.

#### Vertex–Edge 2D.

*v*near a single edge

_{a}*e*, this case is modeled by a unilateral constraint on a single contact as

_{b}In this case, the vertex is within a reasonably small distance $\u03f5$ of the edge, but farther than $\u03f5$ from either of the edge's vertices.

#### Vertex–Vertex 2D.

where $C\iota 1$ contains at least one of *C*_{1} or *C*_{2}, $C\kappa 1$ may contain one of *C*_{1} or *C*_{2}, and similarly $C\iota 2$ contains at least one of *C*_{3} or *C*_{4}, and $C\kappa 2$ may contain one of *C*_{3} or *C*_{4}. As is the case for all $I$ -constraints, in which contacts are included in $C\iota $ or $C\kappa $ these are determined using the geometric heuristics defined in Sec. 4.

### PEG 3D.

There are four contact configurations in 3D: vertex–face, edge–edge, vertex–edge, and vertex–vertex. Again, we exclude certain feature pairs just as we did in 2D, namely, edge–face and face–face, because they become redundant when the other configurations are properly enforced. Edge–face is redundant since edges are composed of vertices, and for an edge to approach a face requires that either the vertices of that edge approach that face, or the edge approaches the edges at the perimeter of that face (possibly adjacent edges), reducing to either vertex–face or edge–edge, respectively, and edge–vertex in the case that the edges of the second body are adjacent. By a similar argument, face–face becomes redundant; a face, being composed of edges in turn composed of vertices, nearing another face implies that edges and vertices are approaching the other face. In all possible configurations, this results in one of the four feature combinations considered.

#### Vertex–Face 3D.

The vertex–face case is analogous to the 2D vertex–edge case. Given a vertex *v _{a}* near a face

*f*as depicted in Fig. 17, we include

_{b}in the set of constraints.

#### Edge–Edge 3D.

*e*against an edge

_{a}*e*by

_{b}#### Vertex–Edge 3D.

Consider Fig. 18 where a vertex *v _{a}* of body $A$ approaches the edge

*e*of body $B$. There are an arbitrary number of edges (greater or equal to three) that could connect to

_{b}*v*; however, we can limit the edge–edge contacts considered to a subset of those with relatively large magnitudes of edge orientation $|ea\xb7Oeb|$. Given $C1=C(va,f1)$ and $C2=C(va,f2)$, the set of constraints for vertex–edge is given by

_{a}where *C*_{1} and *C*_{2} are distributed appropriately into $C\iota $ and $C\kappa $. Each *C _{ei}* is an edge with positive edge orientation and each

*C*is an edge with negative edge orientation, and both sets of edges have applicability and feasibility with

_{ej}*e*.

_{b}#### Vertex–Vertex 3D.

In the case of a vertex *v _{a}* near another body's vertex

*v*as depicted in Fig. 19, we wish to avoid trapping

_{b}*v*against the faces of $B$, and similarly for

_{a}*v*against $A$. This first requires the set of $I$ -constraints

_{b}*v*and faces of $B$, and $C\iota b$ and $C\kappa b$ are composed of contacts between

_{a}*v*and faces of $A$. We additionally require $X$ -constraints for each face pair (

_{b}*f*,

_{a}*f*) in the form

_{b}where *f _{a}* has edges $ea1\u2009and\u2009ea2$ connected to

*v*, and

_{a}*f*has edges $eb1$ and $eb2$ connected to

_{b}*v*. These $X$ -constraints are enforced only for edges

_{b}*e*and

_{ar}*e*, which have edge–edge applicability and feasibility. The constraint in Eq. (38) is similar to the $X$ -constraints in Eq. (36) for the vertex–edge case, but permits a vertex to pass on either side of a face instead of either side of an edge.

_{bs}### Time-Stepping Formulation With PEG.

*m*$I$ -constraints, $cI$ is the solution vector composed of unknown slack variables

*k*and the

*j*th $I$ -constraint has

*k*+ 1 potential contacts; $EI1$ is diagonally composed of submatrices where

*k*; $EI2$ is diagonally composed of submatrices of row vectors of ones where

*k*, $\psi I$ we will define as

*s*from 2 to

*k*where

## Results

The experiments discussed below compare simulations performed with the time-stepping methods defined by Eqs. (8) and (9) and Eqs. (48) and (49) in 2D and 3D. The only difference in the time steppers was that one used the standard contact model and the other used PEG. Both methods retained the term $\psi n/h$ for constraint stabilization, and the time-stepping subproblems were solved by path [27]. For simulations with PEG, a single primary potential contact was chosen for each set of potential contacts, which yielded the least accurate form of PEG. Despite this, the results show significant accuracy improvements when using PEG. The examples are of convex polytopes poured into nonconvex containers represented as unions of convex polytopes.

### Box of Polygons.

Figure 20 shows a snapshot from the 2D experiment, which consisted of 160 simulations (ten different initial configurations for 16 different time steps). In each experiment, 20 randomly generated convex polygons were “poured” into an open box fixed in space (the same initial conditions were used for PEG as for the standard model). One polygon was dropped every 0.25 s until the simulation ended at 5.0 s. At the end of each time step, the area of overlap among all polygons was computed and used as a metric of accuracy. When simulating the standard model, the simulation frequently crashed prior to reaching 5 s of simulation time, due to SM traps that caused the solver to fail. Typically prior to failure with the standard model, configurations were reached that subsequently allowed interpenetration or nonphysically large body velocities. Pivoting solvers, such as Lemke's algorithm [24], demonstrated similar difficulties in finding physically sensible solutions in SM trap situations. By contrast, time-stepping with PEG never encountered such problems since interpenetration and overconstraining were avoided.

Figure 21 shows the median and the first and third quartiles of area of overlap error for increasing time-step size. Even for small time steps, the time stepper using PEG produced errors several orders of magnitudes smaller than the time stepper using the standard contact model. As the error with the standard model grows with the step size, the error in PEG remains imperceptibly close to machine precision.

An interesting statistic to observe is the rate of occurrence of SM traps. In other words, how frequently was PEG necessary to achieve a geometrically accurate interaction between bodies. Figure 22 depicts the number of SM traps encountered at each time step for the 2D experiment. The top curve represents the total number of contacts, and the curve below it shows the number of SM traps. It shows that PEG was necessary in approximately 25% of time steps. One should realize that as the size of the time step increases, so would the percentage of SM traps.

### Box of Polyhedra.

As shown in Fig. 23, we extend the polygon pouring experiment to 3D. In the 3D simulations, the random polygons were replaced by convex polyhedra, the box became the union of five right hexahedra, and volumes of overlap replaced the error metric used in the 2D simulations. Again, there were ten sets of different initial conditions for each of 16 time-step sizes, and these sets were each used for both PEG and the standard model. A polyhedron was dropped into the box every 0.5 s until 5.0 s of simulated time was reached.

The constraint violation error defined as volume overlap is depicted in Fig. 24, which has extremely similar behavior to the 2D results. As the step size increases, the penetrations between polyhedra grow steadily larger with the standard contact model, whereas PEG virtually eliminates this error at all time steps.

The occurrence of SM traps is shown in Fig. 25, where we see a rate of occurrence similar to the 2D experiment. Although the shape similarity is intriguing, it is simply coincidence, as these results are only representative of a single experiment and time-step size for both 2D and 3D.

## Conclusions and Future Work

There have been previous good results regarding physical fidelity and stability of differential variational inequality (DVI) methods [10,12,42] upon which we have built our contact model. PEG further improves stability by avoiding interpenetrations, which can generate nonphysical behavior in ways that are difficult to predict and correct. PEG is also extendable to nonconvex bodies simply by subdividing bodies into convex parts [23]. Although PEG results in significantly smaller error while maintaining dynamic stability, there is still some increase in error for any time-stepping method as the time step increases. This is due to the fact that the time-stepping approach solves for impulses at the end of the time step based on normal directions determined at the beginning of the time step. As such, body rotations may generate unpredicted interpenetrations at the end of the time step, which are exacerbated given larger time steps.

In future work, we plan to explore efficiency and accuracy tradeoffs associated with the design of heuristics used to define the set of active contacts, the size of time steps, and the number of primary potential contacts. When simulation speed is the main goal, it is desirable to minimize the size of the active set and the number of primary potential contacts. However, when high accuracy trumps speed, then multiple primary potential contacts per contact should be used and the active set would be larger. The pursuit of this work will require a concomitant effort in the development of efficient, large-scale solvers of linear programs with complementarity constraints.

## Acknowledgment

This work was partially supported by the National Science Foundation through the National Robotics Initiative NSF CCF-1208468, DARPA W15P7T-12-1-0002, and Lockheed-Martin Advanced Technology Laboratory.