The next equality constraint we will derive is the prismatic constraint. A prismatic constraint is like the line constraint except it does not allow rotation about the anchor point. A prismatic constraint constraints the linear motion of the bodies along a line. An example of a prismatic joint is the slide of a semi-automatic pistol. The slide is moved back to charge the weapon, then released to its original position. The slide cannot rotate about the pistol, nor can it move up/down or left/right only along one axis.
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 Prismatic 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.
The Jacobian
As earlier stated, the Prismatic Joint is just like the Line Joint only it does not allow rotation about the anchor point. Because of this, we can formulate the Prismatic Joint by combining two joints: Line Joint and Angle Joint. This allows us to skip directly to the Jacobian:
\[J = \begin{bmatrix} \vec{t^T} & \vec{r_a} \times \vec{t} & -\vec{t^T} & -(\vec{r_b} + \vec{u}) \times \vec{t}\\ 0 & 1 & 0 & -1 \end{bmatrix}\]See the “Line Constraint” and “Angle Constraint” posts for the derivation of their Jacobians.
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}\\ 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} \vec{t} & 0\\ \vec{r_a} \times \vec{t} & 1\\ -\vec{t} & 0\\ -(\vec{r_b} + \vec{u}) \times \vec{t} & -1 \end{bmatrix}\]Multiplying left to right the first two matrices we obtain:
\[JM^{-1}J^T = \begin{bmatrix} \vec{t^T}M_a^{-1} & (\vec{r_a} \times \vec{t})I_a^{-1} & -\vec{t^T}M_b^{-1} & -((\vec{r_b} + \vec{u}) \times \vec{t})I_b^{-1}\\ 0 & I_a^{-1} & 0 & -I_b^{-1} \end{bmatrix} \begin{bmatrix} \vec{t} & 0\\ \vec{r_a} \times \vec{t} & 1\\ -\vec{t} & 0\\ -(\vec{r_b} + \vec{u}) \times \vec{t} & -1 \end{bmatrix}\]Multiplying left to right again:
\[JM^{-1}J^T = \begin{bmatrix} \vec{t^T}M_a^{-1}\vec{t} + (\vec{r_a} \times \vec{t})I_a^{-1}(\vec{r_a} \times \vec{t}) + \vec{t^T}M_b^{-1}\vec{t} + ((\vec{r_b} + \vec{u}) \times \vec{t})I_b^{-1}((\vec{r_b} + \vec{u}) \times \vec{t}) & (\vec{r_a} \times \vec{t})I_a^{-1} + ((\vec{r_b} + \vec{u}) \times \vec{t})I_b^{-1}\\ (\vec{r_a} \times \vec{t})I_a^{-1} + ((\vec{r_b} + \vec{u}) \times \vec{t})I_b^{-1} & I_a^{-1} + I_b^{-1} \end{bmatrix}\]If we use the following just to clean things up:
\[\begin{align} s_a &= \vec{r_a} \times \vec{t} \\ s_b &= (\vec{r_b} + \vec{u}) \times \vec{t} \\ \end{align}\]We get:
\[JM^{-1}J^T = \begin{bmatrix} \vec{t^T}M_a^{-1}\vec{t} + s_aI_a^{-1}s_a + \vec{t^T}M_b^{-1}\vec{t} + s_bI_b^{-1}s_b & s_aI_a^{-1} + s_bI_b^{-1}\\ s_aI_a^{-1} + s_bI_b^{-1} & I_a^{-1} + I_b^{-1} \end{bmatrix}\]And if \(\vec{t}\) is normalized we get:
\[JM^{-1}J^T = \begin{bmatrix} M_a^{-1} + s_a^2I_a^{-1} + M_b^{-1} + s_b^2I_b^{-1} & s_aI_a^{-1} + s_bI_b^{-1}\\ s_aI_a^{-1} + s_bI_b^{-1} & I_a^{-1} + I_b^{-1} \end{bmatrix}\]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
7 responses
I'm not sure I understand the difference you are pointing out. Are you saying that the derivation on the Line Constraint Post is incorrect (since that is where I get the velocity constraint for the Prismatic Joint)? Thanks for checking my work!
William
Hi William,
I haven't checked your line constraint. In your derivation for the prismatic constraint you are not accounting for the change n over time. I use n instead of t, because dt/dt might look confusing.
C = (x2 + r2 – x1 – r1) * n = 0
dC/dt = (v2 + cross( omega2, r2 ) – v1 – cross( omega1, r1 ) ) * n + (x2 + r2 – x1 – r1) * dn/dt
You are missing the RHS of the equation. So what is dn/dt? It is the change of a local axis. E.g. if you would store n in body1 dn1/dt = cross( omega1, r1 ). Lets define dp = (x2 + r2 – x1 – r1) you get
dC/dt = (v2 + cross( omega2, r2 ) – v1 – cross( omega1, r1 ) ) * n + dp * cross( omega1, r1 )
When you derive constraints do not start with the velocity constraint. Solving on the velocity level is a linearization oo the original problem since you now are solving in the tangent space of the original constraint. You also need the position constraint for stabilization (e.g Baumgarte or Post-Projection). The best advice I can give for constraints is the following recipe (which I know from the Shabana book, but which is very likely in the engineering community much longer) is.
1) Write down the position constraint
2) Build time derivative
3) Identify Jacobian by inspection using dC/dt = J * v
As a side exercise if you are interested you can think about this. The derivation I showed is also true for non-penetration constraints. But here the RHS is always zero.
This is a great site William. Keep up the fantastic work.
Cheers,
-Dirk
There is a typo in the equations where I use t instead of n. Maybe someone can fix this.
Dirk,
I think I have made the changes correctly to the equations above in your comment and thanks for the feedback.
As you describe here:
When you derive constraints do not start with the velocity constraint. Solving on the velocity level is a linearization oo the original problem since you now are solving in the tangent space of the original constraint. You also need the position constraint for stabilization (e.g Baumgarte or Post-Projection). The best advice I can give for constraints is the following recipe (which I know from the Shabana book, but which is very likely in the engineering community much longer) is.
1) Write down the position constraint
2) Build time derivative
3) Identify Jacobian by inspection using dC/dt = J * v
This is exactly what I do in the Line Constraint post (the full derivation is there). You can also see that the derivation does include the RHS. In my derivation I chose to fix n (I call it u) in body b's space (now that you point it out dt/dt might be confusing for people...).
I'm still failing to see where my derivation went wrong. Please check out the Line Constraint post and let me know if I have missed something there.
William
The line constraint looks correct. I make everything relative to body A so I might have overlooked a term. Sorry if my comments caused any confusion.
Cheers,
-Dirk
The Jacobian is not correct. You have to start from position constraint and then find the Jacobian by inspection.
C = (x2 + r2 – x1 – r1) * t = 0
This is a function of the C = f(t) * g(t) with f(t) = (x2 + r2 – x1 – r1) and g(t) = t. The derivative is dC/dt = df*dt * g + f * dg/dt.
This is a common mistake people make for prismatic joints. You can look into Box2D for a correct Jacobian.
For a general framework on how to indentify constraints and Jacobians I recommend looking at Shabana "Multibody Dynamics"
Cheers,
-Dirk