- Problem Definition
- Process Overview
- Position Constraint
- The Derivative
- Isolate The Velocities
- Compute The K Matrix
It’s probably good to start with a good definition of what we are trying to accomplish. From the last post on equality constraints it may not be clear what we are trying to do.
We want to take two or more bodies and constrain their motion in some way. For instance, say we want two bodies to only be able to rotate about a common point (Revolute Joint). The most common application are constraints between pairs of bodies. Because we have constrained the motion of the bodies, we must find the correct velocities, so that constraints are satisfied otherwise the integrator would allow the bodies to move forward along their current paths. To do this we need to create equations that allow us to solve for the velocities.
What follows is the derivation of the equations needed to solve for a Point-to-Point constraint which models a Revolute Joint.
Since this is the first specific constraint I’ll post on, we need to go through how we actually perform the derivation.
Erin Cato lays out the process quite simply:
- Create a position constraint equation.
- Perform the derivative with respect to time to obtain the velocity constraint.
- Isolate the velocity.
Using this formula we can ensure that we get the correct velocity constraint. After isolating the velocity we inspect the equation to find J, the Jacobian.
Most constraint solvers today solve on the velocity level. Earlier work solved on the acceleration level.
Once the Jacobian is found we use that to compute the K matrix. The K matrix is the A in the Ax = b general form equation.
So the first step is to write out an equation that describes the constraint. A Revolute Joint should allow the two bodies to rotate about a common point, but should not allow them to translate away or towards each other. In other words:
The next step after defining the position constraint is to perform the derivative with respect to time. This will yield us the velocity constraint.
The velocity constraint can be found/identified directly, however its encouraged that a position constraint be created first and a derivative be performed to ensure that the velocity constraint is correct.
Another reason to write out the position constraint is because it can be useful during whats called the position correction step; the step to correct position errors (drift).
Now that the constraint is written in the correct variables we can perform the derivative
The derivative of a fixed length vector under a rotation frame is the cross product of the angular velocity with that fixed length vector.
Isolate The Velocities
The next step involves isolating the velocities and identifying the Jacobian. This may be confusing at first because there are two velocity variables. In fact, there are actually four, the linear and angular velocities of both bodies. To isolate the velocities we will need to employ some identities and matrix math.
The linear velocities are already isolated so we can ignore those for now. The angular velocities on the other hand have a pesky cross product. In 3D, we can use the identity that a cross product of two vectors is the same as the multiplication by a skew symmetric matrix and the other vector; see here. For 2D, we can do something similar by examining the cross product itself:
Remember that the angular velocity in 2D is a scalar.
Removing the cross products using the process above yields:
Now if we employ some matrix multiplication we can separate the velocities from the known coefficients:
Now, by inspection, we obtain the Jacobian:
Compute The K Matrix
Lastly, to solve the constraint we need to compute the values for A (I use the name K) and b:
See the “Equality Constraints” post for the derivation of the A matrix and b vector.
The b vector is fairly straight forward to compute. Therefore I’ll skip that and compute the K matrix symbolically:
Multiplying left to right the first two matrices we obtain:
Multiplying left to right again:
Now if we write out the equation element wise:
Remember the inertia tensor in 2D is a scalar, therefore we can pull it out to the front of the multiplications.
Multiplying the matrices and adding them yields:
Plug the values of the K matrix and b vector into your linear equation solver and you will get the impulse required to satisfy the constraint.
Note here that if you are using an iterative solver that the K matrix does not change over iterations and as such can be computed once each time step.
Another interesting thing to note is that the K matrix will always be a square matrix with a size equal to the number of degrees of freedom (DOF) removed. This is a good way to check that the derivation was performed correctly.