The next equality constraint we will derive is the angle constraint. An angle constraint can be used to join two bodies forcing them to have the same rotation. This particular constraint will be added to other constraints (in later posts) to form more complex constraints.
- Problem Definition
- Process Overview
- Position Constraint
- The Derivative
- Isolate The Velocities
- Compute The K Matrix
Problem Definition
It’s probably good to start with a good definition of what we are trying to accomplish.
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 an Angle constraint.
Process Overview
Let’s review the process:
- Create a position constraint equation.
- Perform the derivative with respect to time to obtain the velocity constraint.
- Isolate the velocity.
Using these steps 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 \(A\vec{x} = \vec{b}\) general form equation.
Position Constraint
So the first step is to write out an equation that describes the constraint. An Angle Joint should allow the two bodies to move and freely, but should keep their rotations the same. In other words:
\[C = R_a - R_b - a_r = 0\]which says that the rotation about the center of body a minus the rotation about the center of body b should equal the initial reference angle calculated when the joint was created.
The Derivative
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).
As a side note, this is one of the easiest constraints to both derive and implement.
Start by taking the derivative of the position constraint:
\[\begin{align} \frac{d(C)}{dt} &= \frac{R_a - R_b - a_r}{dt} \\ &= w_a - w_b - 0 \\ &= w_a - w_b \end{align}\]Isolate The Velocities
The next step involves isolating the velocities and identifying the Jacobian. This may be confusing at first because there are two angular velocity variables. To isolate the velocities we will need to employ some matrix math.
\[\begin{align} \frac{d(C)}{dt} &= w_a - w_b \\ &= 0\vec{v_a} + 1w_a - 0\vec{v_b} - 1w_b \\ &= \begin{bmatrix} 0 & 1 & 0 & -1 \end{bmatrix} \begin{bmatrix} \vec{v_a} \\ w_a \\ \vec{v_b} \\ w_b \end{bmatrix} \end{align}\]Notice that I still included the linear velocities in the equation even though they are not present. This is necessary since the mass matrix is a 4x4 matrix so that we can multiply the matrices in the next step.
Now, by inspection, we obtain the Jacobian:
\[J = \begin{bmatrix} 0 & 1 & 0 & -1 \end{bmatrix}\]Compute The K Matrix
Lastly, to solve the constraint we need to compute the values for \(A\) (I use the name \(K\)) and \(\vec{b}\):
\[\begin{align} A\vec{x} &= \vec{b} \\ A &= JM^{-1}J^T \\ \vec{x} &= \vec{\lambda} \\ \vec{b} &= -J\vec{v_i} \end{align}\]See the “Equality Constraints” post for the derivation of the \(A\) matrix and \(\vec{b}\) vector.
The \(\vec{b}\) vector is fairly straight forward to compute. Therefore I’ll skip that and compute the \(K\) matrix symbolically:
\[JM^{-1}J^T = \begin{bmatrix} 0 & 1 & 0 & -1 \end{bmatrix} \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} \begin{bmatrix} 0\\ 1\\ 0\\ -1 \end{bmatrix}\]Multiplying left to right the first two matrices we obtain:
\[JM^{-1}J^T = \begin{bmatrix} 0 & I_a^{-1} & 0 & -I_b^{-1} \end{bmatrix} \begin{bmatrix} 0\\ 1\\ 0\\ -1 \end{bmatrix}\]Multiplying left to right again:
\[JM^{-1}J^T = I_a^{-1} + I_b^{-1}\]Plug the values of the \(K\) matrix and \(\vec{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.
Comments
8 responses
Yeah, that sounds reasonable. I think a lot of 3D engines offer a generic 6DOF joint in which each degree of freedom is defined separately; sort of like what you describe. Is it not working?
I can't really offer much help unfortunately, as I haven't done any 3D joint derivations (nor can I find any good references...)
William
Same here, references are hard to find. It behaves well until the constraint is violated about the restricted axis.
Perhaps someone else has had this problem before, I follow this process:
Initialise the joint:
1) Transform the world anchor (anchor – body.position) and world free hinge axis into local coords of each body
2) build an orthogonal basis consisting of the restricted axes (tangent1 and tangent2) from the local free hinge axis of body1
Each frame:
3) Transform (anchor – body.position) into world coords of each body
4) Transform the local hinge axis into world coords of each body
5) Transform each tangent vector into world coords of body1
6) Compute the constraint, bias, and K matrix
At each impulse iteration:
7) Solve for lambda as with other joints
8) Update the velocities of each body
THe only problem I see is the world hinge axis of body2 is not used but I don't see where to actually use it. Can anyone point me in the right direction?
Just to clarify, are you having trouble with the limits or just constraining the 5 DOF. I would try the joint without limits and see if you can get it to work without it locking, then add in the limits. Limits, especially rotational limits are annoying to get right (at least in my limited experience).
What you describe here seems to be correct. When you do the transformation of world to local coordinates you should be including the body rotation too. Also, on each iteration be sure to transform the local vectors and anchors properly. I had a few problems with this myself and it ended up being a problem in my local<=>world transformation code.
// assume that a body contains an augmented
// transformation matrix
T = [ R0,0 R0,1 R0,2 | x ]
[ R1,0 R1,1 R1,2 | y ]
[ R2,0 R2,1 R2,2 | z ]
// to transform from local coordinates to world
// you simply multiply the matrix with the
// vector/point that you want to transform
localV = [ x ]
[ y ]
[ z ]
[ 1 ]
worldV = T * localV
// then to transform back you need to invert
// the transformation matrix to do this, it's
// best not to use the augmented matrix since a
// rotation matrix can be easily inverted
// by taking the transpose:
R-1 = RT
Tx,y,zT = [ x y z ]
localV = R-1 * (worldV – Tx,y,z)
You may also want to consider looking at using Quaternions rather than using Euler angles since they can solve some gimbal lock problems (they may come with their own problems too though).
I think you only need the hinge axis in one body's coordinates since it can only be fixed in one coordinate frame.
William
Thanks William, I have got the hinge constraint to work, however I have a question about the derivation for an angle joint. You have started off with a matrix, then used the angular velocity to solve for lambda. However to compute the bias to prevent drift, the equation is as follows:
bias = beta/dt * C where C is a rotation matrix in this case and the bias required is a vector, due to the angular velocities being vectors.
Given C = Ra – Rb – Ri and the equation to solve for the impulse is:
JM^-1JT*lambda = -Jvi – bias
lambda = (-wa+wb-bias) (Ia^-1 + Ib^-1)^-1
So we are subtracting a matrix from vectors (-wa+wb-[bias]) and this is undefined. I am definitely missing something, any thoughts?
I'm not sure I understand your question completely, but let me take a stab at answering.
If you have solved the velocity constraint as shown above, you will experience drift at the limits. If you want to cancel out the drift you need to solve the position constraint:
Solve the following (so C will be a matrix,
basically the relative rotation):
C = Ra – Rb – Ri
Then solve using the K matrix (from the
velocity constraint derivation):
J = C * K-1
Then apply to the bodies:
Ba.R += J * Ia-1
Bb.R -= J * Ib-1
This will feed the error in the rotation back into the bodies which will be taken care of in the next velocity constraint solving iteration on the next world step.
If you are computing this before solving the velocity constraint (is that what you mean by "bias"?), just apply it to the bodies first, then solve the velocity constraint.
Hope that helps,
William
Thanks William.
Yes, that's what I mean by bias. It's computed once per frame before updating the velocities.
Just to give a few more details (I'm not sure if I answered your question fully). The typical flow is:
1. Apply external forces/torques to get new velocities.
2. Initialize constraints (compute the K matrices).
3. Solve velocity constraints using the K matrix. Using an iterative approach this will be called a number of times to reach a global solution, but since the K matrix is dependent on position and mass rather than velocity, it doesn't need to be recomputed.
4. Apply the velocities to get new positions
5. Solve position constraints (position correction). If you are going for the iterative approach, you need to be careful here. To do this right you will need to recompute the K matrix on each call since the K matrix is dependent on the position and the position is changing at the end of each call. (In some cases, like the angle joint, this isn't the case since K only includes the mass). Since this is a heavy process, you can probably get away with using the same K matrix without much loss in accuracy in most cases.
Then repeat. Applying the position correction will be seen in the computation of the K matrix and that's how it gets fed back into the velocity constraint upon the next iteration.
Hope that helps,
William
Hi William,
I am having trouble with the hinge constraint. Which equations do you use in addition to the revolute joint constraint?
I have C1 = xa + ra – xb – rb (rev joint)
C2 = t1.(thetaA – thetaB) = 0
C3 = t2.(thetA – thetaB) = 0
Where the dot product between the two axes perpendicular to the free hinge axis and the change in orientation between the two bodies is zero
I now have the combined Jacobian for C2 and C3 as follows
JC2 = [ 0 t1T 0 -t2T ]
JC3 = [ 0 t2T 0 -t2T ]
Using the fact that the dot product with a vector is equivalent to the inverse of that vector multiplied by it
IS this how you would go about this? Thanks for your input