As the first entry after the Equality Constraints post, we will perform the derivation of the Point-to-Point constraint, which models a Revolute Joint, in 2D.
- 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. 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.
Process Overview
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 \(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 Revolute Joint should allow the two bodies to rotate about a common point \(\vec{p}\), but should not allow them to translate away or towards each other. In other words:
\[C = \vec{p_a} - \vec{p_b} = 0\]which says the anchor points \(\vec{p_a}\) and \(\vec{p_b}\) on body A and B respectively must be the same point. This allows the bodies to translate freely, but only together, and rotate freely about the anchor point.
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).
First lets understand how \(\vec{p_a}\) and \(\vec{p_b}\) are defined:
\[\begin{align} \vec{p_a} &= \vec{c_a} + R_a\vec{r_a} \\ \vec{p_b} &= \vec{c_b} + R_b\vec{r_b} \end{align}\]where \(\vec{c_a}\) and \(\vec{c_b}\) are the world space centers of mass of body A and B respectively, \(\vec{r_a}\) and \(\vec{r_b}\) are the local space vectors from the local space center of mass to the local space anchor point, and \(R_a\) and \(R_b\) are the rotation matrices of body A and B. This means that the world space anchor point for either body is the sum of the world space center of mass plus the world space vector from the local space center of mass to the local space anchor point (transformed by the body’s rotation matrix). This definition gives us the position of the anchor point relative to each body.
Note that the local space anchor point for Body A and B will be different, but doesn’t not change over time.
Then by subsitution:
\[C = \vec{c_a} + R_a\vec{r_a} - \vec{c_b} - R_b\vec{r_b}\]Now that the constraint is written in the correct variables we can perform the derivative. To do this, we need to identify what components are time dependent. The position of the bodies are obviously time dependent. The rotation matrices are also time dependent. The \(\vec{r}\) vectors on the other hand are not - they are locally defined and constant over time.
Another trick here is how to take the derivative of the \(R\vec{r}\) product. This turns out to be pretty simple as described here. For reference, the following identity applies:
\[\frac{d(R\vec{r})}{dt} = w \times \vec{r}\]The derivative of a fixed length vector under a rotation frame is the cross product of the angular velocity with that fixed length vector.
Writing out the derivative we have:
\[\begin{align} \frac{d(C)}{dt} &= \vec{v_a} + \frac{d(R_a\vec{r_a})}{dt} - \vec{v_b} - \frac{d(R_b\vec{r_b})}{dt} \\ &= \vec{v_a} + w_a \times \vec{r_a} - \vec{v_b} - w_b \times \vec{r_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.
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:
\[\begin{align} w \times \vec{r} &= \begin{bmatrix} -wr_y \\ wr_x \end{bmatrix} \\ &= \begin{bmatrix} -r_y \\ r_x \end{bmatrix}w \\ &= R_sw \end{align}\]Remember that the angular velocity in 2D is a scalar.
Removing the cross products using the process above yields:
\[\frac{d(C)}{dt} = \vec{v_a} + R_{sa}w_a - \vec{v_b} - R_{sb}w_b\]Now if we employ some matrix multiplication we can separate the velocities from the known coefficients:
\[\frac{d(C)}{dt} = \begin{bmatrix} I_{2x2} & R_{sa} & -I_{2x2} & -R_{sb} \end{bmatrix} \begin{bmatrix} \vec{v_a} \\ w_a \\ \vec{v_b} \\ w_b \end{bmatrix}\]And, by inspection, we obtain the Jacobian:
\[J = \begin{bmatrix} I_{2x2} & R_{sa} & -I_{2x2} & -R_{sb} \end{bmatrix}\]Where \(I_{2x2}\) is a 2x2 identity matrix.
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} I_{2x2} & R_{sa} & -I_{2x2} & -R_{sb} \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} I_{2x2} \\ R_{sa}^T \\ -I_{2x2} \\ -R_{sb}^T \end{bmatrix}\]Note that the transpose of a block matrix is the transpose of the elements.
Also note that the transpose of the identity matrix is the identity matrix, i.e. \(I_{nxn}^T = I_{nxn}\)
Multiplying left to right the first two matrices we obtain:
\[JM^{-1}J^T = \begin{bmatrix} M_a^{-1} & R_{sa}I_a^{-1} & -M_b^{-1} & -R_{sb}I_b^{-1} \end{bmatrix} \begin{bmatrix} I_{2x2} \\ R_{sa}^T \\ -I_{2x2} \\ -R_{sb}^T \end{bmatrix}\]Multiplying left to right again:
\[JM^{-1}J^T = M_a^{-1} + R_{sa}I_a^{-1}R_{sa}^T + M_b^{-1} + R_{sb}I_b^{-1}R_{sb}^T\]Now if we write out the equation element wise (remembering that \(I^{-1}\) is a scalar in 2D):
\[JM^{-1}J^T = \begin{bmatrix} m_a^{-1} & 0 \\ 0 & m_a^{-1} \end{bmatrix} + \begin{bmatrix} -r_{ay}I_a^{-1} \\ r_{ax}I_a^{-1} \end{bmatrix} \begin{bmatrix} -r_{ay} & r_{ax} \end{bmatrix} + \begin{bmatrix} m_b^{-1} & 0 \\ 0 & m_b^{-1} \end{bmatrix} + \begin{bmatrix} -r_{by}I_b^{-1} \\ r_{bx}I_b^{-1} \end{bmatrix} \begin{bmatrix} -r_{by} & r_{bx} \end{bmatrix}\]Multiplying the matrices and adding them yields:
\[\begin{align} JM^{-1}J^T &= \begin{bmatrix} m_a^{-1} & 0 \\ 0 & m_a^{-1} \end{bmatrix} + \begin{bmatrix} r_{ay}I_a^{-1}r_{ay} & -r_{ay}I_a^{-1}r_{ax} \\ -r_{ax}I_a^{-1}r_{ay} & r_{ax}I_a^{-1}r_{ax} \end{bmatrix} + \begin{bmatrix} m_b^{-1} & 0 \\ 0 & m_b^{-1} \end{bmatrix} + \begin{bmatrix} r_{by}I_b^{-1}r_{by} & -r_{by}I_b^{-1}r_{bx} \\ -r_{bx}I_b^{-1}r_{by} & r_{bx}I_b^{-1}r_{bx} \end{bmatrix} \\ &= \begin{bmatrix} m_a^{-1} & 0 \\ 0 & m_a^{-1} \end{bmatrix} + \begin{bmatrix} I_a^{-1}r_{ay}r_{ay} & -I_a^{-1}r_{ay}r_{ax} \\ -I_a^{-1}r_{ax}r_{ay} & I_a^{-1}r_{ax}r_{ax} \end{bmatrix} + \begin{bmatrix} m_b^{-1} & 0 \\ 0 & m_b^{-1} \end{bmatrix} + \begin{bmatrix} I_b^{-1}r_{by}r_{by} & -I_b^{-1}r_{by}r_{bx} \\ -I_b^{-1}r_{bx}r_{by} & I_b^{-1}r_{bx}r_{bx} \end{bmatrix} \\ &= \begin{bmatrix} m_a^{-1} + m_b^{-1} + I_a^{-1}r_{ay}r_{ay} + I_b^{-1}r_{by}r_{by} & -I_a^{-1}r_{ay}r_{ax} - I_b^{-1}r_{by}r_{bx} \\ -I_a^{-1}r_{ax}r_{ay} - I_b^{-1}r_{bx}r_{by} & m_a^{-1} + m_b^{-1} + I_a^{-1}r_{ax}r_{ax} + I_b^{-1}r_{bx}r_{bx} \end{bmatrix} \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
18 responses
Good question. To isolate the velocity terms we must convert the cross product into a matrix multiplication and depending on the number of dimensions you will get different Jacobians.
In 2D the angular velocity, w, is a scalar and you can simply separate the scalar and matrix with a multiplication (as shown in the post). Performing a transpose in 2D is not necessary, although, it may have made things easier to follow. (For 2D using the transpose vs. not transpose will still yield the same jacobian, but only because w, I, and M are all scalars).
However, in 3D you cannot do this since w is a vector and matrix multiplication is not commutative. For 3D you must use this formula to separate the angular velocity. You will need to use the second equation which gives you RTw so that you can further separate out the Jacobian but retain the order of matrix multiplication.
William
Hi William,
I am having trouble solving for lambda in J*M^-1*JT*lambda = -Jvi in 3D
With the following equation:
( Ma^-1 + skew(ra)*Ia^-1*skew(ra)T + Mb^-1 + skew(rb)*Ib^-1*skew(rb)T ) * lambda = ( -vai – skew(ra)*wai + vbi + skew(rb)*wb)
THe matrices added are of the following dimensions for 3D:
( [4×4] + [3×3] + [4×4] + [3×3] ) * lambda = [3×1]
Since the addition of different sizes matrices is not defined, do you have any idea how to add these to end up with a 4×1 vector lambda to sub into the following equations:
vaf = vai + Ma^-1*lambda
waf = wai + Ia^-1*skew(ra)T*lambda
vbf = vbi – Mb^-1*lambda
wbf = wbi – Ib^-1*skew(rb)T*lambda
Much appreciate your help
Sorry forgot to mention, even if we have a 3×3 mass matrix, multiplying the inverse mass of each matrix by the identity, we'd still end up with a 3×1 vector lambda, when there are two linear and two angular velocities. Maybe I am going wrong somewhere
Please disregard the above posts and thanks for a great tutorial ! I have implemented the revolute joint without drift correction and it's working great.
Glad to here that you were able to solve it! If you have any tips for others, feel free to share. Solving these types of equations are difficult to get right since there are so many matrices (and block matrices). The 3D version is definitely more difficult to reduce down.
Just let me know if you have any other questions,
William
Thanks! I am sure you have implemented the 3D version and have better tips than me. I am however happy to send some c++ code if it helps anyone
I have a question about the Jacobian. Some people say its a 3×12
[ 1 skew[ra] -1 -skew[rb] ]
Since we have two scalars and 2 3×3 matrices, how would you arrange these to form a 3×12 matrix? I am not that familiar with a matrix inside a matrix
The Jacobian's row count may differ depending on the type of constraint you are creating.
Because we are creating a pair-wise constraint (involving only two bodies) then we will always have 4 columns in the Jacobian. As you point out, column 2 and 4 are 3×3, but column 1 and 3 are scalars.
They are getting a 3×12 because you can write the scalar elements as a matrix of the same size by multiplying by the identity matrix (sometimes this makes things easier):
1 = | 1 0 0 |
| 0 1 0 |
| 0 0 1 |
So the Jacobian would look something like:
J = [ | 1 0 0 | | sra sra sra | | -1 0 0 | | -srb -srb -srb | ]
[ | 0 1 0 | | sra sra sra | | 0 -1 0 | | -srb -srb -srb | ]
[ | 0 0 1 | | sra sra sra | | 0 0 -1 | | -srb -srb -srb | ]
Where sra and srb are the elements of skew[ra or rb] respectively.
William
Thanks for you help! I have implemented this constraint in 3D with drift correction, so it's working 100%.
For the distance constraint, I am having some problems:
I have dp = xa + ra – xb – rb
C = |dp| – L = 0
Initially, for example, I set the bodies to xa = (35,0,0) xb = (-35,-10,0) and the anchor point at (0,0,0)
So ra = (0,0,0) – (35,0,0) = (-35,0,0) and rb = (0,0,0) – (-35,-10,0) = (35,10,0)
L = |ra – rb| = 70.71
So C = | (35,0,0) + (-35,0,0) – (-35,-10,0) – (35,10,0) | – 70.71
C = 0 – 70.71 which is non-zero and causing me problems
I have tried different combinations and it seems like x and r cancel each other out in any case. In which cases are relative positions non-zero?
Also, as relative position is zero JM^-1JT is zero since the normal vector is dp/|dp| which causes a divide by zero for lambda. It makes sense in a point-point constraint that dp should always be zero so that C=0 but I am definitely missing something for the distance constraint.
Please help!
I am misrepresenting the distance of course, stupid me. The distance should be zero if the relative position is zero, so ra and rb should extend to the boundaries of the bodies and pa and pb should be points on the bodies rather than points at the anchor ! Sorry for the previous post.
Hi William, please write about contact constraint, i'm looking for "practical" contact constraint solving on the web, but i can't find anything.
@james
I agree that this is very hard to find. If I get some time in the future I certainly will, but at the moment my time is very limited.
William
hi william~
I have got another problem(the previous one was solved)~
"Remember that the angular velocity in 2D is a scalar."
Since angular velocity in 2D is a scalar, dose the scross product between a vector and a scalar make sense?
@Jiang Jin hao
Yes, this may seem strange. The angular velocity is a scalar in 2D because the the other two coordinates of the vector will always be zero. In other words, the angular velocity is always perpendicular to the 2D plane. Its 3D vector would look like [0 0 av]. Likewise, an arbitrary 2D vector in 3D would look like [x y 0]. If we take the cross product of these we get:
[ 0 * 0 - av * y av * x - 0 * 0 0 * y - 0 * x ]
which gives
[-av*y av*x 0]
and we can drop the z coordinate because it will always be zero for 2D. So we can reduce this to say that the cross product of a scalar (really a 3D vector with only a z component) and a 2D vector (really a 3D vector with only x and y components) is the following:
scalar x vector = [-scalar * vector.y scalar * vector.x]
In summary, we are technically doing a true cross product, but we can avoid a lot of work (and storage space) if we ignore the vector components who will always be zero.
William
thank a lot!
With your generous help, I have understood the resource of Box2d.
Thank you.
Hi William,
In the step where we want to isolate velocities, you have converted a vector into its corresponding skew-symmetric matrix.
For two vectors a and b, and a skew symmetric matrix [A] of a,
will b X a = [A] X b
or will b X a = (transpose of [A]) X b
Such a formula would change the Jacobian to contain transposes of the skew symmetric matrices, instead of the skew symmetric matrices. Which one is right?
Thanks