Point-to-Point Constraint

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.

  1. Problem Definition
  2. Process Overview
  3. Position Constraint
  4. The Derivative
  5. Isolate The Velocities
  6. 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:

  1. Create a position constraint equation.
  2. Perform the derivative with respect to time to obtain the velocity constraint.
  3. 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.

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, but should not allow them to translate away or towards each other. In other words:

ptpc (7)

which says the points pa and pb must be the same point. This allows the bodies to translate freely but only together.


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 if we write out how pa and pb are computed we obtain:

ptpc (8)

Where xa and xb are the centers of mass (or positions), rota and rotb are the rotation matrices, and ra and rb are the vectors from the centers of mass to the common point.

Now that the constraint is written in the correct variables we can perform the derivative

ptpc (9)

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:

ptpc (10)

Remember that the angular velocity in 2D is a scalar.

Removing the cross products using the process above yields:

ptpc (11)

Now if we employ some matrix multiplication we can separate the velocities from the known coefficients:

ptpc (12)

Now, by inspection, we obtain the Jacobian:

ptpc (13)

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.

ptpc (1)

The b vector is fairly straight forward to compute. Therefore I’ll skip that and compute the K matrix symbolically:

ptpc (2)

Multiplying left to right the first two matrices we obtain:

ptpc (3)

Multiplying left to right again:

ptpc (4)

Now if we write out the equation element wise:

ptpc (5)

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:

ptpc (6)

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.

18 thoughts on “Point-to-Point Constraint

  • 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

    • 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):

      So the Jacobian would look something like:

      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:

      which gives

      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:

      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

Leave a Reply

Your email address will not be published. Required fields are marked *