Equality Constraints

After the first release of the dyn4j project, I felt that it would be good to pass along what I learned about constrained dynamics.

This is not an easy subject and aside from purchasing books there’s not much information out there about it for those of us not accustomed to reading research papers or theses.

In this post I plan to solve a velocity constraint generally. Later posts will be for the individual types of constraints.

A phyiscial constraint is defined by some function:

eqc0

Where x is the position of the body and R is the rotation of the body. Typically constraints are pairwise. You can formulate constraints using any number of bodies, however, pairwise formulations are simple. When this function is equal to zero the constraint is satisfied.

If we perform the derivative with respect to time we get the velocity constraint

eqc1

Since C is a function of positions and rotations who are themselves functions of time, we must perform the chain rule. The derivative of one vector function (C) by another vector function (x) yields a Jacobian matrix:

Equation 1

Equation 1

Where v is the vector of velocities from the bodies. The Jacobian rows contain the gradients of the scalar components of the constraint function C. We know that the gradient direction is the highest rate of increase of the scalar function C and unless the constraint is satisfied then it will be non-zero. Therefore the gradient is the direction of the illegal movement. We want the constraint force to act in this direction since we don’t want to constrain legal movement:

Equation 2

Equation 2

Where lambda a vector of the magnitudes of the constraint forces. The constraint force is the force that must be applied to counter act external forces so that the constraint is satisfied.

We know that the final velocity of a system is:

eqc4

We also know that the sum of all the forces on a body is:

eqc5

We can split the external forces and the constraint forces apart and then distribute:

eqc6

If we look closely we can perform the integration using the external forces separately therefore removing the external forces from the equation:

Equation 3

Equation 3

Now if we substitute equation [2] into [3]:

Equation 4

Equation 4

Now if we substitue equation [4] into equation [1]:

Equation 5

Equation 5

Now if we rearrange the terms to match the form Ax = b:

We also know that:

eqc11

Where P is the impulse, therefore:

Equation 6

Equation 6

Where lambda is the constraint impulse. We stated at the beginning of the post that if the constraint is satisfied if the constraint equals zero. This is true for the velocity constraint as well:

eqc13

eqc14

This will solve the constraint exactly, however, given that the integrator is only an approximation of the ODE and the lack of infinite precision the constraint will “drift.” For example, a point-to-point constraint (simulates a revolute joint) will drift, where the local points of the world space anchor point slowly separate over time.

Drift can be solve using methods like Baumgarte stabilization, post/pre stabilizations methods, etc. That will be left for another post.

The equation is now in the form:

eqc15

and can be solved using any linear equation solver desired. After solving for lamda, we can substitute it back into equation 4:

eqc16

to find the final velocities.

Remember that the velocity vector is the velocity of the system and the mass matrix is the mass of the system. For example, a two body system would have the form:

eqc17

eqc18

The Jacobian, as we will see in the next few posts, will be specific to the type of constraint being added and the number of bodies involved (as we see above).

19 thoughts on “Equality Constraints

  • hi, william

    how to understand “we can perform the integration using the external forces separately therefore removing the external forces from the equation”?

    why remove the external force from equation

    • We remove the external forces from the equation because they can be integrated (in other words, applied to the bodies) before we even attempt to solve any constraints. Take gravity as an example. Gravity will try to accelerate an object in a direction. We can go ahead and apply that force to the bodies to get new velocities. Afterwards, we can solve the constraints as normal using a much simpler equation.

      In short we remove those from the equation because we can apply them early (since any illegal movement will be removed by the constraints) and because it makes the solution for the constraints much simpler.

      William

  • Pingback: Constrained Dynamics – A Tour In Physics Constraints | The Little Balance
  • Thanks for great posting

    Btw I got a question about Ax=b form.
    Could it be cancel J matrix out on the left on both side ?
    What I mean is M^{-1} J^T = -v_i instead of J M^{-1} J^T = -J v_i.

    • Thanks!

      Great question! You are right if we left multiply of J^{-1} on both sides we could eliminate J. It’s funny too because, at first I was like, “wow why did I not see this earlier”, but alas, there is a problem with this:

      The Matrix inverse is only defined for square matrices. The Jacobian is not always square and therefore doesn’t always have an inverse. This is why we cannot reduce the equation further until we actually know the dimensions of J. In the later posts about specific equality constraints we see that many, if not all (I didn’t go back and look at all of them), are 1x4s or 2x4s.

      William

  • Hello,

    This is a very good description of constraint forces at play in physical systems. If you can, can you let us know what books/papers you have referenced in this theoretical development? I would like to learn more.

  • hi~William
    First of all, thanks a lot for the marvelous post.
    I’m a chinese student,so please tolerate the nonproficiency of my English.
    I can understand the computing process except for the equation[2] on the post.
    Appreciate for your apply, thank you ~

    • @Jiang Jin hao

      You’ve probably found your answer already, but it’s a good idea to respond since it will help others. Take a look a slide 15 here. This should explain where equation 2 comes from. It contains a lot of other good information too.

      William

  • Hi William,
    I think your site is the best resource on the net for this topic, and was so glad when I finally found it. Thanks a lot for putting it together!

    You mention solving for lambda using a linear equation solver, but I see in Erin Catto’s Box2D papers that he uses an iterative method.
    I just wanted to ask, for the constraints where the Jacobian is a matrix rather than a scalar, is it possible to solve for lambda by treating each Jacobian row as a separate constraint? Otherwise I don’t understand how an iterative method would work with these Jacobians.

    • @Nick

      There are two types of constraints that you may be referring to here. There are equality constraints, which are typically your joint formulations, and inequality constraints which include contacts (for collision resolution). For now lets focus on equality constraints.

      Equality constraints can be solved using any linear solver, this is true. For example, if you look at the b2WeldJoint implementation, in the InitVelocityConstraints method, it inverts the K matrix (which I call the A matrix). Later, in the SolveVelocityConstraints method, it multiplies the K matrix with the cdot or cdo1 (which I call the b vector). Computing the inverse of a matrix and multiplying it by a vector is identical to solving a system of equations.

      This solves the joint exactly, however, there’s an issue when you have more than one constraint. Let’s say you solve them one after another. You solve the first constraint exactly, then you move to the next one and solve it exactly, and so on. But as you are solve each subsequent constraint, the previous constraint’s solutions may be invalidated. What is required is a global solution. This is where the iterative solver comes in. Put simply, the iterative solver solves all joints, one after another, n number of times. It’s been shown that this will converge to the global solution. An alternative to an iterative solver is to put all the Ax = b equations into a giant matrix and solve it, but there are a lot of advantages to the iterative method, especially for real time simulation and when coupled with inequality constraints.

      William

  • Hi William,

    Thanks for your reply. That makes sense, although maybe I didn’t explain my question well enough. I’ll try to explain what I meant better, but please excuse me if any of the following is wrong as I’m still trying to figure it out :)

    What I meant was, say you have a system with several constraints (eg a weld joint between bodies 1 and 2, and a revolute joint between bodies 2 and 3). As you say, you can either create a large sparse matrix for all bodies that will have both of the constraints in it, and solve that, or you can apply an iterative method.
    Like you described, the iterative method would work by solving each constraint in isolation, and then repeating this a number of times.

    Some constraints are made by composing others (for example the weld joint is a revolute joint combined with an angle joint). What I was wondering is, just as you can consider the two constraints I mentioned above in isolation (when solving iteratively), does this mean you could also consider the 2 constraints of the weld joint separately in the iterative solver?

    If I understand correctly, when fully expanded (rather than in block form) the Jacobian for the weld joint will actually have 3 rows, one for each degree of freedom removed.
    That is, there is a scalar constraint equation to restrict each of the X, Y and angle of one body relative to the other.
    What I really was getting at was, can each of *these* be considered in isolation (as separate constraints) when writing an iterative solver?

    The reason I wanted to know that is because if K and lambda is scalar then lambda can be solved for directly, rather than having to worry about inverting K. And also from Catto’s GDC 2009 presentation he seems to describe K and lambda as scalars, so I assumed that’s what he meant, but then I see in the code he doesn’t do that so I am confused.

    Hope that makes more sense.

    • @Nick

      Ok, thanks for elaborating.

      Yeah, you can solve the compound constraints separately for sure. The problem with doing it that way is that it will be less accurate. This manifests itself as requiring more iterations of the global solver (which will solve every constraint every iteration). As a result, it’s much faster, and more accurate, to solve them together when possible.

      Aside from this, some small benefits include better encapsulation along with some other niceties (for example allowing the toggling/configuring of various properties of the joints (which could reduce or increase the number of DOF that are solved) without the caller destroying or creating new joints.

      William

Leave a Reply

Your email address will not be published. Required fields are marked *