class constraintSolver

The constraintSolver class implements a modified version of Baraff's algorithm [1], [2] for bilateral and unilateral constraints, with and without static and dynamic friction.

Inheritance:

Inherited from synObject:

Public Fields

static int staticClassID

int objectID

int verboseLevel

Public Methods

virtual const char* className(void) const

virtual synObject* copy(void) const

virtual int isOfType(int typeNum, int derivedOk)

static int setStaticClassID(void)

virtual int classID(void) const

Documentation

The constraintSolver class implements a modified version of Baraff's algorithm [1], [2] for bilateral and unilateral constraints, with and without static and dynamic friction. Note that the names and algorithms for most of the major functions correspond to those found in [1], except for the modifcations as noted below and throughout the code. Users will not normally need to create a constraintSolver or use it directly; this documentation is intended to give an overview of the constraint satisfaction algorithm.

Building constraint equations, matrices, and vectors

While it is possible to build up your own constraint matrix A and vector b that relate contact forces to accelerations,

    a = A*f + b,
 

this code builds up A and b from the jacobian Jc of each constraint and the robot's dynamic equations

    T = M*acc + V
 

which are computing using s-vals (see svals.{cxx,h} and dynamics.cxx for the gory details). If you don't need this stuff, then you can probably ignore/rewrite composeMatrices and applyImpulse()

Conveniently, acceleration constraints can be expressed with s-vals and easily translated into the appropriate jacobian Jc. If we want a certain acceleration to be zero, we simply construct an s-val equation that computes the acceleration. If this accleration is called a and has dimension d, then it has the form:

    a = Jc*acc
 

where a is a vector of dimension d, acc is a vector of joint-space accelerations (dimension n, where n is the # of DOFs of the robot), and Jc is the Jacobian for the constraint (d x n). One might compute Jc 'manually', but each row of Jc is simply the s-val coefficients for an element of a.

The constraint matrix A and vector b can thus be computed as follows:

  T = M*acc + V - JcT*f

(where JcT is the transpose of the jacobian composed of all constraint jacobians stacked on top of each other and f is the constraint forces to be applied)

      Minv*(T-V) = acc - Minv*JcT*f    (multiplying by Minv and rearranging)
  ->  acc = Minv*JcT*f + Minv*(T-V)    (solving for acc)

      a = Jc*acc                       (from above) 
  ->  a = Jc*Minv*JcT*f + Jc*Minv*(T-V)   (substituting acc)
          -----------     -------------
      a =      A     *f +       b

  

Thus, once we solve for an f (using our fancy, incredibly complicated yet theoretically unproven algorithm) we just do

     acc += Minv*JcT*f
   

since acc was originally computed with f = 0.

Computing impulses

Before we can compute contact forces, we must ensure that the relative approach velocity at each contact is zero. This requires computing an appropriate impluse, applying to the system, and updating the vectors V (which depends on velocity), TmV, and b (which both depend on V). I fully admit that I pulled this method out of my...out of thin air, and it may not do things "important" things like conserving momentum, but it seems to work--that is, the relative approach velocity is non-negative at each contact point. Note that the coefficient of restitution is assumed to be zero, though I think it should be trivial to change (see computeImpulse() below).

The Jacobian Jc mentioned above also maps joint-space velocity into constraint-space velocity:

    v = Jc*vel
  

We want v to be nonnegative. For now, assume that every entry of v is negative; thus, we want v to be zero. We must solve for a contact-space impulse q such that v = 0 after applying the impulse, i.e. an impulse q such that

    v = Jc*vel + Jc*Minv*JcT*q = 0
 

thus

    A*q = -Jc*vel
    

This contact-space impulse is then mapped to joint-space chance of velocity by

    dvel = Minv*JcT*q
 

or (since M is symmetric and we have Jc*Minv handy)

    dvel = transpose(q)*Jc*Minv
 

and we update velocity simply as

    vel += dvel
 

We assumed above that all entries of v were negative and that we wanted to zero all of them (including, say, the tangential components of a frictional contact's velocity). When this isn't true, we simply remove the rows and columns of the system

    A*q = -Jc*vel
 

when solving it. If our solution impulse ends up making some other approach velocities negative, we add them to the system (i.e. stop ignoring them) and solve again. This is almost definitely not the most efficient method, but it seems to work.

Computing contact forces

This algorithm is way too complicated for me to describe here, so just browse the code and read comments within. The general idea for computing contact forces in the presence of static and dynamic friction is outlined in [2], though I discovered that some important details are left out. My implementation of [2] failed to provide "correct" (i.e. physically observed) behavior for some simple cases with static friction, such as a block resting on a plane near the edge of stability. Basically, the direction of the frictional force at each contact is constrained only by a*f <= 0, so you can end up with situations where the block *should* be at rest, but some friction forces are opposing each other (not just the pull of gravity) and thus don't have sufficient components opposing gravity to keep the block from accelerating. My solution to this was to solving for multiple constraint forces simultaneously, rather than iteratively. It seems to work fine, but in case there's some horrible fundamental problem with it, the original iterative algorithm is also included (computeForcesIter()). The "missing piece" from [1] is how to get physically realistic motion (that is, no motion at all in the above example) given the supplied algorithm.

Another missing piece is computing impulses to resolve inconsisent mechanisms when an "unbounded ray" is encountered by the algorithm. These mechanisms can occur when there is dynamic friction in the system; see [1] for details. In [1], when Lemke's algorithm finds an unbounded ray 'z' it is claimed that this ray represents an impulse such that (1) adding a positive multiple of z will convert at least one contact from sliding to non-sliding (e.g. tangential velocity will be zero after the impulse); and (2) the relative approach velocity after applying z will be non-positive at at least one contact point.

While this is true for 2-D systems, it is certainly not always true in 3-D systems. Consider the case in which we have a single sliding contact with tangential velocity of (-1, -1) and coefficient of friction mu = 0.7. The frictional force must be directly opposed to the velocity and must have magnitude equal to mu*fn (where fn is the normal force). If we are increasing fn by 1, we must thus increase (fx, fy) by (0.5, 0.5). If we find an unbounded ray z, it will be

    z = (1 0.5 0.5)
 

The problem is that when we map the impulse z into a change in velocity

    dv = A*z,
 

the tangential componets of dv will not necessarily be directly opposed to the tangential velocity because A is not diagonal. In this case, there is *no* multiple of z that will cause dv to cancel out the tangential velocity and thus convert the contact from sliding to non-sliding. This fact is not mentioned at all in either [1] or [2]. Fortunately, it's not a show stopper--we can construct an impulse z that satisfies conditions (1) and (2) above; see computeConsistencyImpulse() below.

Refernces:

[1] D. Baraff, Fast Contact Force Computation for Nonpenetrating Rigid Bodies. SIGGRAPH 94, pp. 23-34

[2] D. Baraff, Issues in Computing Contact Forces for Non-Penetrating Rigid Bodies. Algorithmica (1993) 10: 292-352

This class has no child classes.

Alphabetic index HTML hierarchy of classes or Java