Introduction
In this post I start to look into the deep world of Constraints and Constraint Solvers. You will appreciate why I say deep, if you do a google search for constrained dynamics or constraint solvers. You will most likely be inundated with papers with lot of maths involving very large matrices. I have had to sift through a lot of papers before getting even a basic understanding. Given this experience, my conclusions are:
You do not necessarily need a deep understanding of all the maths,
However, an understanding of the key principles / key elements will definitely help in implementing different types of constraints yourself
Therefore, I will document my interpretation of the basic elements of the generic framework for the formulation of constraint functions and solving such functions using impulses.
Objective
Let's clarify what we are trying to do.
The goal is to create a "magic box" that, when fed with the following inputs, will calculate impulses necessary to make a pair of bodies behave in a "physics-like" manner.
Jacobian matrix
Mass matrix
Velocity vector
Put simply, if you know the Jacobian matrix for a particular constraint you can create your constraint solving engine.
What is constrained dynamics?
Unconstrained dynamics
Where you have rigid bodies moving around the screen with no restrictions of any kind, that is unconstrained dynamics.
In unconstrained dynamics, a rigid body in 3-dimensional space has 6 degrees of freedom (DoF), as defined by the 3 components of translation (x, y, z) and 3 components of rotation. In 2 dimensions, a rigid body has 3 degrees of freedom as defined by 2 components of translation (x, y) and rotation about an axis perpendicular to the xy plane (θ).
Constrained dynamics
In contrast, in constrained dynamics, you have some kind of restriction on the bodies' degrees of freedom, ie you end up with smaller number of DoF than in constrained dynamics.
One example of a constraint is the distance constraint where the distance between two bodies is fixed.
Another example is the ground constraint whereby a body falling under the force of gravity is not allowed to go below the ground, ie the y-component of the body's position is not allowed to be below zero (assuming ground level is zero).
Solving Constraints
Constraint solving is the process by which we compute the corrective displacement, corrective impulses or corrective forces that, when applied to the bodies, will pull them together or push them apart, so their movement will be restricted and the rules imposed by the constraints will remain satisfied.
In the case of the ground constraint, this can be achieved by:
pushing the y-component of position back to zero, if the body has penetrated the ground, and
resetting the y-component of the velocity to zero or some positive number, if the ball is to bounce.
The above two steps are "solving" the "broken" constraint
What I am about to describe is a generic framework for expressing the constraint as a mathematical function - a constraint function - which when solved, induces physics like behaviour.
Constraint function
A constraint is defined in terms of a constraint function C, which takes the state of a pair of bodies as parameters and outputs a scalar number.
We would specifically express C as a linear combination of positional properties (linear position and angular position, ie rotation, etc, ie the "state" of the body);
When the value of this function is in the acceptable range, the constraint is said to be satisfied. If the "state" of the constraint function is composed of position properties, and we solve this "position constraint function" (e.g. change the negative y-component of position to zero, as in the ground constraint), it means that we are solving the constraint by directly manipulating the position properties of the bodies.
Another way of solving constraints is to come up with a constraint function that takes velocity components as input (i.e. linear velocity, angular velocity) and then solve that. This means that we are solving the constraint by manipulating the velocity of the bodies. This is the approach we are focusing on in this post.
Position constraint function
In order to come up with the necessary impulse, the first thing we must do is to come up with the position constraint function which expresses the constraint criteria as a mathematical formula.
For the moment, we will keep this discussion generic, and say that the position constraint function is expressed as follows.
We say this is a position constraint function because it is a constraint on x, which is the positional properties of the bodies (i.e. the position and rotation). This positional state x, in 2d is expressed as a 6x1 column matrix, like below (assuming we are dealing with a pair of bodies, body A and body B).
This position constraint function is said to be satisfied if the result is zero. On the other hand, the constraint is broken if it does not equal zero.
Equality constraint and Inequality constraint
The above position constraint function is that of an equality constraint, ie a constraint that is said to be satisfied if C is exactly zero. Examples of equality constraints are the distance constraint, revolute joint and prismatic joint.
You can also have inequality constraints which can be expressed as:
The inequality constraint is said to be satisfied if C is positive. The constraint is broken only if C is negative.
Examples of inequality constraints are contact constraint and joint angle limits.
As mentioned earlier, a broken position constraint function could be fixed by changing the positional properties of the bodies. That is one approach of physics simulation. Another approach is to manipulate the velocities by applying impulses to fix both the positional error and the velocity constraint. For that we need to express the constraint at the velocity level by taking the time derivative of the position constraint.
Velocity constraint function
Again, for the moment we will keep things very generic and say that we want to express the velocity constraint as follows.
where v is the velocity vector of the pair of bodies, which is a 6x1 column matrix as below.
and J is a matrix called the Jacobian matrix.
Jacobian matrix
The Jacobian matrix is a s x 3n matrix (s being the number of constraints) and n is the number of bodies (3 being the degrees of freedom for a single body in 2d, like below:
The Jacobian for a simple constraint like the distance constraint between a pair of bodies (from hereon, assume we are dealing with a pair of bodies, unless otherwise stated) would be a 1 x 6 matrix.
Fixing broken velocity constraint
This constraint function is NOT satisfied if it does NOT equal zero. To fix a broken velocity constraint, we need to change the velocities. Hence the objective is to find Δv so that the velocity constraint function does equal zero, ie:
More specifically, we aim to find impulse P to get to Δv (remember that impulse is a change in momentum, ie mΔv), or Δv = P/m).
Lagrange Multipler
We are trying to find the impulse P. It will become clearer later on, but the Jacobian is the "axis" along which the impulse will be applied. For that reason, the Jacobian is sometimes referred to as expressing the "constraint axis".
Following on from this, we can say that the impulse is some multiple of the Jacobian. Let us call this scalar multiple, λ, ie P = Jλ (the lambda is often referred to as the Lagrange Multiplier).
Again, leaving aside the detail for now, take it for granted that Δv is expressed as below.
(Note I have simply replaced M-1 with W in the final expression as that is a convention often used in tutorials)
where M is the mass matrix
Mass Matrix
The mass matrix is a block (i.e. a matrix composed of smaller matrices) diagonal matrix, as below:
Elements M1 and M2 are also a 2x2 matrices. In other words, M is actually a 6x6 matrix (in 2d).
Given that m and I are all scalars in 2d, inverting the above is easy.
In practice, one would probably not bother creating the 6x6 matrix and carry out the full matrix multiplication (although that would work perfectly fine), given its sparse nature. Instead, one would implement the matrix multiplication by dealing only with the non-zero elements.
Remember that we are trying to find Δv such that:
Reinserting the Δv magic equation back into this velocity constraint function, we get
Rearranging we can arrive at a formula for lambda (a scalar) as follows:
which in turn leads to,
Projecting the relative velocity along J
The numerator, Jv, is the product of the Jacobian matrix (s x 6 matrix) and the 6x1 velocity vector. You can think of this as the projection of the relative velocity of the bodies along the Jacobian, the constraint axis.
K-matrix and the Effective Mass
You will see the denominator, JM-tJT often referred to as the K-matrix and its inverse the effective mass. You will sometimes see JM-tJT itself referred to as the effective mass.
As mentioned earlier, the Jacobian for simple constraints will be a 1x6 matrix. In such a case, JM-tJT would be the product of a 1x6 matrix, 6x6 matrix and 6x1 matrix, which would result in a scalar.
For more complicated constraints, the Jacobian may be a 2x6 matrix, in which case the K-matrix would be a 2x2 matrix. In such a case the lambda expression would be as below, requiring the 2x2 K-matrix to be inverted to arrive at the lambda:
Once we have the magnitude of the impulse, the impulse can be calculated (Jλ), from which Δv can be calculated by the following.
Lambda should be bounded.
For inequality constraints, λ should be positive. We can define this condition by bounding lambda with lower and upper bounds, as below. We will see why this is important when we come to impementation.
For convenience, the bounds for an equality constraint may be bounded as follows:
Adjusting for position error
Solving the velocity constraint equation given above is not always enough, however. The Δv may resolve the velocity constraint. However, this will not always fix the position constraint, as we would have only eliminated the velocity break along the constraint axis. The bodies must be given a little bit of extra "oomph" along the constraint axis to satisfy not only the velocity constraint but also the position constraint. For that, we need to introduce what is referred to as bias velocity; an extra bit of impulse/velocity, to bias the result of the constraint calculation.
Bias velocity / Baumgarte Stabilization
The velocity constraint function then becomes:
where b is the bias velocity, and this is based on the output from the position constraint function C(x), as follows.
The bias velocity is taking the current amount that the position constraint is violated by, and add it to the components of v, artificially inflating them and thus causing the resulting impulse to be larger. The larger the positional error, the larger this bias becomes, which makes intuitively sense.
β is a value between 0 and 1 called the bias factor, which describes the amount of error that is corrected at each time step. If we want this error to be reduced to zero in the next timestep ∆t, the velocity needed to correct for this error is C(s) ∆t. However, we do not want the error to be removed in a single timestep. Instead, the velocity needed to correct for the position in small steps for stability reasons. The bias term is usually expressed as following:
This type of error correction is called Baumgarte stabilization.
"Shape" of the bias velocity
The shape of the bias term will edpend on the shape of J. For example if J is a 1x6 matrix, the bias term will be a scalar. If J is a 2x6 matrix, the bias term would be a 2x1 matrix.
Returning to the calculation of the Lagrange multiplier, if you work through the same calculations as above, you will get the following expression for lambda.
With this adjusted lambda, you can calculate the impulse to fix both the positional error and the velocity constraint.
Geometric analogy
(this section is pretty much a straight copy from this excellent article here)
Recall that a plane can be expressed as below:
If a point (x, y, z) is not on the plane, meaning ax + by + cz + d <> 0, the shortest way to bring the point onto the plane is by projecting the point onto the plane, along the plane’s normal (a, b, c).
If we group the coefficients and variables into separate matrices, the plane equation becomes:
NP + d = 0
where N is a 1-by-3 matrix representing the plane normal (a, b, c):
N = [a b c]
and P is a 3-by-1 matrix representing the point (x, y, z):
d is just a “bias term”.
The required change to P in order to project it onto the plane, denoted ΔP, is proportional to the plane’s normal, N^T
Notice that NP + d = 0 and JV + b = 0 have the same form, except that the former formula deals with a 3-dimension plane, and the latter deals with a 12-dimensional plane.
The “shortest” change to the velocity vector, ΔV, is along the “plane normal” J^T. However, different rigid bodies have different masses and inertia tensors, impulse cannot be applied uniformly to the entire velocity vector, hence J^T is modified by an inverse mass matrix, and finally we get the proportion M^{-1} J^T.
Useful References
Comments