The next equality constraint we will derive is the line constraint. A line constraint is like a prismatic constraint (which will most likely be the next post) except allows rotation about the anchor point. A prismatic constraint constraints the linear motion of the bodies along a line. An example of a prismatic joint might be a roller coaster on the track. The cars cannot translate or rotate except along the track. For simplicity the prismatic constraint we will define is only for straight lines.
- 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 a Line 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. A Line Joint should allow the two bodies to only translate along a given line, but should allow them to rotate about an anchor point. In other words:
\[C = \vec{t} \cdot \vec{u} = 0\]where:
\[\begin{align} \vec{u} &= \vec{p_a} - \vec{p_b} = (\vec{c_a}m_a + \vec{r_a}R_a) - (\vec{c_b}m_b + \vec{r_b}R_b) \\ \vec{t} &= \begin{bmatrix} -u_y \\ u_x \end{bmatrix} \end{align}\]
If we examine the equation we can see that this will allow us to constraint the linear motion. This equation states that any motion that is not along the vector \(\vec{u}\) is invalid, because the tangent of that motion projected onto (via the dot product) the \(\vec{u}\) vector will no longer yield 0.
The initial vector \(\vec{u}\) will be supplied in the construction of the constraint. From \(\vec{u}\), we will obtain the tangent of \(\vec{u}\), the \(\vec{t}\) vector. Each simulation step we will recompute \(\vec{u}\) from the anchor points and use it along with the saved \(\vec{t}\) vector to determine if the constraint has been violated.
Notice that this does not constrain the rotation of the bodies about the anchor point however. To also constrain the rotation about the anchor point use a prismatic joint.
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.
\[\frac{d(C)}{dt} = \frac{d(\vec{t} \cdot \vec{u})}{dt}\]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).
By the chain rule:
\[\frac{d(C)}{dt} = \vec{t} \cdot \frac{d(\vec{u})}{dt} + \frac{d(\vec{t})}{dt} \cdot \vec{u}\]Where the derivative of \(\vec{u}\):
\[\begin{align} \frac{d(\vec{u})}{dt} &= \frac{d(\vec{p_a} - \vec{p_b})}{dt} \\ &= \frac{d(\vec{c_a}m_a + \vec{r_a}R_a) - (\vec{c_b}m_b + \vec{r_b}R_b)}{dt} \\ &= \vec{v_a} + w_a \times \vec{r_a} - \vec{v_b} - w_b \times \vec{r_b} \end{align}\]The derivative of a fixed length vector under a rotation frame is the cross product of the angular velocity with that fixed length vector.
And the derivative of t:
\[\frac{d(\vec{t})}{dt} = w_b \times \vec{t}\]Here is one tricky part about this derivation. We know that \(\vec{t}\), like \(\vec{r}\) in the \(\vec{u}\) derivation, is a fixed length vector under a rotation frame. A vector can only be fixed in one coordinate frame, therefore you must choose one: a or b. I chose b, but either way, as long as the K matrix and b vector derivations are correct it will still solve the constraint.
Substituting these back into the equation creates:
\[\frac{d(C)}{dt} = \vec{t} \cdot (\vec{v_a}+w_a\times\vec{r_a}-\vec{v_b}-w_b\times\vec{r_b}) + (w_b\times\vec{t}) \cdot \vec{u}\]Now we need to distribute, and on the last term I’m going to use the property that dot products are commutative:
\[\frac{d(C)}{dt} = \vec{t} \cdot \vec{v_a} + \vec{t} \cdot w_a \times \vec{r_a} - \vec{t} \cdot \vec{v_b} - \vec{t} \cdot w_b \times \vec{r_b} + \vec{u} \cdot w_b \times \vec{t}\]Now we need to group like terms, but the terms are jumbled. We can use the identity:
\[\vec{a} \cdot (\vec{b} \times \vec{c}) = (\vec{a} \times \vec{b}) \cdot \vec{c}\]and the property that dot products are commutative to obtain:
\[\frac{d(C)}{dt} = \vec{t} \cdot \vec{v_a} + \vec{r_a} \times \vec{t} \cdot w_a - \vec{t} \cdot \vec{v_b} - \vec{r_b} \times \vec{t} \cdot w_b + \vec{t} \times \vec{u} \cdot w_b\]Now we can use the property that the cross product is anti-commutative on the last term to obtain the following. Then we group by like terms:
\[\begin{align} \frac{d(C)}{dt} &= \vec{t} \cdot \vec{v_a} + \vec{r_a} \times \vec{t} \cdot w_a - \vec{t} \cdot \vec{v_b} - \vec{r_b} \times \vec{t} \cdot w_b - \vec{u} \times \vec{t} \cdot w_b \\ &= \vec{t} \cdot \vec{v_a} + \vec{r_a} \times \vec{t} \cdot w_a - \vec{t} \cdot \vec{v_b} - (\vec{r_b} \times \vec{t} - \vec{u} \times \vec{t}) \cdot w_b \\ &= \vec{t} \cdot \vec{v_a} + \vec{r_a} \times \vec{t} \cdot w_a - \vec{t} \cdot \vec{v_b} - (\vec{r_b} + \vec{u}) \times \vec{t} \cdot 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 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.
All the velocity terms are already ready to be isolated, by employing some matrix math we can obtain:
\[\frac{d(C)}{dt} = \begin{bmatrix} \vec{t}^T & \vec{r_a} \times \vec{t} & -\vec{t}^T & -(\vec{r_b} + \vec{u}) \times \vec{t} \end{bmatrix} \begin{bmatrix} \vec{v_a} \\ w_a \\ \vec{v_b} \\ w_b \end{bmatrix}\]By inspection the Jacobian is:
\[J = \begin{bmatrix} \vec{t}^T & \vec{r_a} \times \vec{t} & -\vec{t}^T & -(\vec{r_b} + \vec{u}) \times \vec{t} \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} \vec{t}^T & \vec{r_a} \times \vec{t} & -\vec{t}^T & -(\vec{r_b} + \vec{u}) \times \vec{t} \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} \vec{t} \\ \vec{r_a} \times \vec{t} \\ -\vec{t} \\ -(\vec{r_b} + \vec{u}) \times \vec{t} \end{bmatrix}\]Multiplying left to right the first two matrices we obtain:
\[JM^{-1}J^T = \begin{bmatrix} \vec{t}^TM_a^{-1} & (\vec{r_a} \times \vec{t})I_a^{-1} & -\vec{t}^TM_b^{-1} & -((\vec{r_b} + \vec{u}) \times \vec{t})I_b^{-1} \end{bmatrix} \begin{bmatrix} \vec{t} \\ \vec{r_a} \times \vec{t} \\ -\vec{t} \\ -(\vec{r_b} + \vec{u}) \times \vec{t} \end{bmatrix}\]Multiplying left to right again:
\[\begin{align} JM^{-1}J^T &= \vec{t}^TM_a^{-1}\vec{t} + (\vec{r_a} \times \vec{t})I_a^{-1}(\vec{r_a} \times \vec{t}) - \vec{t}^TM_b^{-1}\vec{t} - ((\vec{r_b} + \vec{u}) \times \vec{t})I_b^{-1}((\vec{r_b} + \vec{u}) \times \vec{t}) \\ &= m_a^{-1} + (\vec{r_a} \times \vec{t})^2I_a^{-1} - m_b^{-1} - ((\vec{r_b} + \vec{u}) \times \vec{t})^2I_b^{-1} \end{align}\]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
5 responses
The u vector should be the locally transformed world axis (and normalized) passed to the joint. So you should be able to specify the axis of the joint (but using the center of masses is fine). You will also need the local tangent of that vector as well. You will also need an anchor point for the joint. This will allow you to compute the r vectors for both bodies (we need the r vectors so we can determine if the bodies have deviated from the tangent).
The local axis and tangent should be computed on initialization. Then in each iteration we transform those into world coordinates using the rotation matrix (only) of the body that we fixed the axis with.
One thing that may be tricky is solving the system. The b vector is going to be (which I didn't expand in the post):
b = -Jv
b = [ tT ra x t -tT -(rb + u) x t ] * [ va wa vb wb ]T
b = tT * va + (ra x t) * wa – tT * vb – ((rb + u) x t) * wb
Where u and t are the world space axis and tangent of the joint that we have from initialization.
William
Thanks William. I am having trouble with some parts of the derivation.
I understand as follows:
We have u initially as the relative position between the two bodies, which is transformed with one of the bodies.
t is always perpendicular to u
In your code, you are using the specified legal axis, and the tangent axis, and additionally, the relative position.
So since you have the original equation as
C = t.u = t.(xa + ra – xb – rb)
dC/dt = t.(va + Cross(wa,ra) – vb – Cross(wb,rb)) + Cross(wb,t).u
and u = xa + ra – xb – rb
Where do you incorporate the specified axis in the equation as you do in the code? All I see is t vectors in your derivation and the relative position u, and not the actual legal axis of movement.
I understand that even if we specify the legal axis upon initialisation, this axis and the tangent vector to this axis can change with the movement of one of the bodies.
Contrary to a revolute joint, where the relative position is always zero, we are saying the relative position between the two bodies can be non-zero only if the tangent vector is perpendicular to this relative position vector.
Thanks for your help
Yeah this is one of the more difficult parts to understand. Basically, to perform the derivative, we need to know how u is defined (in terms of what? A single body? Both bodies? Is it fixed?). Since we are doing pair-wise constraints we know that we want to have u in terms of both of the bodies so that their relative motion is constrained. We can define the position constraint using the initial relative position vector of the two bodies relative to the anchor point (this is why we still have to supply an anchor).
Take a look at this image:
If either body moves (in this case I move only body b), we can't recompute u, since the constraint would always be satisfied. That means that we compute u once, when the constraint is created, from the initial positions of the bodies. But if we are doing that, why can't we just supply an axis rather than compute it? The answer is, we can, because the creator of the constraint could do the same thing if they just positioned the bodies appropriately upon creation of the constraint. So if I were to position b in the second position in my image above initially, I could make the constraint use (1, 0) as the axis rather than (8, 6).
I hope that clears things up a bit,
William
Thanks William, that does clear things up.
I have managed to implement the 3D version, with two restricted tangent axis, thereby ending up with a 2×2 matrix. The only difference is for 3D, you need to build an orthogonal basis from the supplied legal axis of movement and derive a second constraint identical to the one you derived.
Hi William,
I have a a body at (-20,0,0) and an immovable body at (0,41,0). The world anchor point is at (-20,0,0). Therefore the initial world u vector should be along the line connecting the two body's center points? I am having trouble getting the simulation correct and trying the 2D case first.
Also, at each frame I have Cross(wa, t) in the velocity equation, therefore the tangent vector should change with body A and be fixed relative to body B.
Thanks for your help