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.
Recall that in 3D, for each body, there are 6 degrees of freedom (DOF) - or ways to move: translation along the x, y, and z axes and rotation about those same axes. In 2D, there are only 3 DOF: x, y and rotation within the x-y plane. The DOF are what are constrained to produce mechanical effects like hinges, fixed distance, limits, etc. While constraints are typically defined as pairwise, they can be any combination. Pairwise constraints are simple to define, solve, and use which explains their ubiquity. Pairwise constraints can be combined to produce complex behavior without creating complex formulations. Unary constraints are even easier but provide less flexibility when it comes to simulation effects.
Constraints must be solved to enforce their properties and this can be done on 3 different levels: acceleration, velocity, and position. Each have their pros and cons, but velocity constraints are typically more stable, easier to construct and are easier to solve. That said, solving on multiple levels has its advantages as well. A great example is that of acceleration and velocity constraints - while we can solve these constraints exactly, the nature of finite precision, discrete timesteps, and non-global solvers will always introduce drift - the effect of the constraint slowly being violated. We can use furter solve the position constraints after solving the velocity constraints to prevent drift.
What follows is a framework for building a set of pairwise velocity constraints for 2D. This same framework can be applied to 3D as well. In subsequent posts, we’ll examine a number of common constraints, define their position constriannt and velocity constraint. They’ll be a little heavy on the Vector/Matrix algrebra side, but a few tricks come up which are really important to reducing down the formulations.
Position Constraint, Velocity Constraint, and the Jacobian
Let’s first start with our simple formulation of a position constraint:
\[C(x(t),y(t),\theta(t)) = C(\vec{q}(t))\]Where \(x(t), y(t), \theta(t)\) are the functions that define the translation along the x and y axes and the rotation within the x-y plane respectively. We simplify a bit by grouping these states into a vector \(\vec{q}(t)\). Next, our goal is to find the velocity constraint for the position constraint. To get the velocity constraint we need to take the derivative with respect to time of the position constraint. However, recall that our constraint function \(C\) is a vector valued function which requires us to apply the multivariable chain rule. This gives us the following:
\[\begin{align} \frac{d}{dt}C(\vec{q}(t)) &= J\frac{d}{dt}\vec{q}(t) \\ &= J\vec{v} \end{align}\]Where \(\vec{v}\) is a column vector containing both the linear and angular velocities and \(J\) is the Jacobian Matrix. We know that for the position constraint to be satisfied \(C = 0\) and the same applies for our velocity constraint:
\[\begin{equation} \frac{d}{dt}C = J\vec{v} = 0 \tag{1} \end{equation}\]Velocity Evolution
What might not be apparent still is how this applies to keeping the constraint enforced. The key here is understanding the evolution of a body’s velocity over time:
\[\vec{v_f} = \vec{v_i} + \Delta t (\vec{a})\]Also recall Netwon’s second law and solve for \(\vec{a}\)
\[\begin{align} \sum \vec{f} &= M\vec{a} \\ M\vec{a} &= \sum \vec{f} \\ \vec{a} &= M^{-1}(\vec{f}_{ext} + \vec{f}_c) \end{align}\]A key concept here is the split of the total force into external forces \(f_{ext}\) and constraint forces \(f_c\). In the next step this allows us to remove the external forces from the equation leaving only the the constraint forces. We can do this by integrating them before we start solving the velocity constraints by numerically evaluating them via an ODE and assigning the result to \(\vec{v_i}\) i.e. \(\vec{v_i} = \vec{v_i} + \Delta t M^{-1}\vec{f}_{ext}\).
Now if we combine these two equations:
\[\begin{align} \vec{v_f} &= \vec{v_i} + \Delta t M^{-1}(\vec{f}_{ext} + \vec{f}_c) \\ &= \vec{v_i} + \Delta t M^{-1}\vec{f}_c \end{align}\]The Constraint Force
At this point, we have a general equation for getting the new velocity of a body given it’s initial velocity, the sum of constraint forces, and the elapsed time. The next key step is to combine our equation (1) from above with this equation. But to do so we need to define \(\vec{f}_c\) as the magnitude of the force \(\vec{\lambda}\) times the direction of the force \(J^T\).
\[\vec{f}_c = J^T\vec{\lambda}_f\]See here, here page 7, and here slide 15 for the best description of this.
If we substitute this equation into our last equation we get:
\[\begin{align} \vec{v_f} &= \vec{v_i} + \Delta t M^{-1}\vec{f}_c \\ &= \vec{v_i} + M^{-1}J^T(\Delta t \vec{\lambda}_f) \\ &= \vec{v_i} + M^{-1}J^T\vec{\lambda}_{imp} \tag{2} \end{align}\]Note that \(\Delta t\) is a scalar and can be moved around. Also note that \(\vec{\lambda}_f\) is the magnitude of the force and \(\Delta t\vec{\lambda}_f\) is the magnitude of the impulse \(\vec{\lambda}_{imp}\). This is how at the velocity level we deal with impulses.
Solving for the Impulse \(\vec{\lambda}_{imp}\)
Finally, we have what we need to solve the constraint by substituting the above equation for \(\vec{v}\) in equation (1):
\[\begin{align} J\vec{v} &= 0 \\ J(\vec{v_i} + M^{-1}J^T\vec{\lambda}_{imp}) &= 0 \\ J\vec{v_i} + JM^{-1}J^T\vec{\lambda}_{imp} &= 0 \\ JM^{-1}J^T\vec{\lambda}_{imp} &= -J\vec{v_i} \tag{3} \end{align}\]It may not look like much, but this is exactly what we need to solve the constraint now. It’s now in the general form for solving a system of linear equations \(A\vec{x} = \vec{b}\) where:
\[\begin{align} A &= JM^{-1}J^T \\ \vec{x} &= \vec{\lambda}_{imp} \\ \vec{b} &= -J\vec{v_i} \end{align}\]This will solve the constraints exactly, however, given that the integrator is only an approximation of the ODE and the lack of infinite precision, the constraint will drift as mentioned in the beginning. 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. which is best left for another post.
You can solve the system of equations using a general linear equation solver or via matrix multiplication \(\vec{x} = A^{-1}\vec{b}\)
After solving for \(\vec{\lambda}_{imp}\), we can substitute it back into equation 2 to get the final velocities.
Final Comments
What we’ve done is built a foundation for solving a general set of constraints (equation 3). This general formulation requires the definition of the velocities, masses, and the Jacobian. With this new found ability, the next few posts will focus on describing the position constraints for fundamental constraint types, performing the derivative, isolating the velocities, and identifying the Jacobian by inspection. Then, we’ll take the masses and Jacobian and build the \(A\) matrix and \(\vec{b}\) vector so we can solve for the impulse \(\vec{\lambda}_{imp}\). And finally, after solving for the impulse we’ll use it to compute the new velocities.
The next few posts will focus on pairwise constraints in 2D. Each constraint’s Jacobian will be different, but we can get a little head start by defining the mass and velocity vector which will be the same for every constraint:
\[\begin{align} M^{-1} &= \begin{bmatrix} M_a^{-1} & 0 & 0 & 0 \\ 0 & I_a^{-1} & 0 & 0 \\ 0 & 0 & M_b^{-1} & 0 \\ 0 & 0 & 0 & I_b^{-1} \end{bmatrix} \\ \vec{v}_i &= \begin{bmatrix} \vec{v_a} \\ w_a \\ \vec{v_b} \\ w_b \end{bmatrix} \end{align}\]Note that in 2D the angular velocity \(w\) is a scalar.
These are denoted this way because they represent the mass and velocity of the system.
A final note on solving the system of equations. In 2D there will be a maximum of 3x3 system to solve (though this could depend on how you solve other constraints like limits) if you solve each constraint individually. The formulation above could be used to solve the entire system of all constraints for all bodies, but the challenge is the size of the resulting \(A\) matrix and computing it’s inverse (or solving for it - these concepts are the same). Instead, for dyn4j, we solve each constraint by itself sequentially. This can have the effect of invalidating a constraint after another one has been solved and so we do this same sequential solve process N times to reach a global solution. The trick to make this work is to clamp the total impulse, not the incremental impulse. This process ends up begin equivalent to solving the global solution (with enough iterations) but with the added benefits of a simpler and performance-accuracy trade off configuration (the number of iterations).
Comments
14 responses
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
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
Thanks a lot !
What a silly question. haha.
Maybe I was confused with many block matrix.
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.
Yeah, I should do this. Unfortunately, I don't have all my links and pdfs that had originally used as references... I went back through some of my references and here is what I could find. It's not much but it can get you started. The 3rd link has over 150 references at the end that cover a number of topics. (Thats how I typically go about finding information, endless references lookup using google)
http://www.cs.cmu.edu/~baraff/papers/sig96.pdf – David Baraff
http://code.google.com/p/box2d/downloads/list – Erin Catto GDC Slides
ftp://ftp.diku.dk/diku/image/publications/erleben.050401.pdf – Large reference for a number of topics
http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?p=&f=6&t=63 – A small list of references
http://blog.generalrelativity.org/actionscript-30/constrained-dynamics-2-joints-and-global-constraints/
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
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