A Differentiable Physics Engine for Deep Learning in Robotics

Submitted for International Conference on Learning Representations 2017

section_content: In this section, we will give a summary of the constrained dynamics theory as described in [1] and [5]. It will also allow us to introduce some notation that will be used throughout the text.
section_name: Introduction

section_content: Consider that we have two rigid bodies B 1 and B 2 with positions x 1 (t) and x 2 (t) and orientations q 1 (t) and q 2 (t). The orientation of a body B i is specified by a unit quaternion q i (t). Now, imagine that we describe the positions and orientations of both bodies using the state vector s(t). 

The motion of the bodies can be restricted by some constraints. It means that some forces and torques have to be applied to the bodies to keep the constraints satisfied. We will use the vector F c (t) for all the forces and torques that have to be applied to the bodies B 1 and B 2 to make sure the constraints remain valid. 

where f ci (t) is the force and τ ci (t) is the torque that need to be applied to the body B i. Similarly, we define the external forces and torques that can be applied on the bodies (like gravity) using the vector F ext (t). 

where f ei (t) is the external force and τ ei (t) is the external torque on the body B i. 

Using the Newton's second law, we get the following second-order differential equation to solve :

where M is the 12 × 12 mass matrix that contains the masses and the inertia tensors of the two bodies. 

where E 3 is the 3 × 3 identity matrix, m 1 and m 2 are the masses of the two bodies and I 1 and I 2 are the 3 × 3 world-space inertia tensor matrices of bodies B 1 and B 2 respectively. Also note that s 0 is the initial state (positions and orientations) of the bodies at time t = 0 and v 0 is the initial velocity state (linear and angular velocities) at time t = 0. 

We want to solve the second-order differential equation 4 to find the state s(t) of the two bodies across time. By introducing the velocity vector v(t) that contains the linear velocities v 1 and v 2 and the angular velocities ω 1 and ω 2 of the bodies B 1 and B 2 :

we can transform equation 4 into two first-order differential equations. 

In this equation, the 14 × 12 matrix S is given by:

This is coming from the way we compute the time derivative of a position vector x i and of a quaternion q i = (x i, y i, z i, w i ). We have:

A rigid body can move in six degrees of freedom (three for the translation and three for the rotation). Therefore, to describe the motion of two rigid bodies, we need a total of twelve degrees of freedom. However, you might have noticed that our state vector s(t) in equation 1 is specified with 14 values. This is because the orientations are represented with quaternions. A quaternion has four values but represents only three degrees of freedom. It means that our state vector s(t) is not the minimal vector that can represent the position and orientation of two bodies. A minimal state vector could be written as:

where α i, β i and γ i are the three Euler angles representing the orientation of body B i. Note that θ i is the integrating quantity of the angular velocity ω i. Therefore, we have:

In general, we do not use the state vector r(t) because Euler angles are problematic for describing rotations in Computer Graphics. Instead, we use the state vector s(t) with the matrix S to convert from velocity state vector v(t) of length 12 to the state vector s(t) of length 14. Now that we have the equations of motions (equation 7), we need to solve them to find the state vector s(t) at any given time t. To do this, we can use for instance the semi-implicit Euler (or symplectic Euler) scheme for the numerical integration of the differential equations. Consider that we are going to use the timestep ∆t for the iterations. Therefore, we have:

It means that if we know the current state s i, the current velocity state v i and the forces F ext and F c at any given time t i, we are able to compute the next state s i+1 at time t i+1 using the previous equations.
section_name: Equations of motion

section_content: Now that we have seen how to update the position of the bodies given the external force F ext and the constraint force F c, we need to figure out how to find the force F c that has to be applied to the bodies in order to keep a given constraint satisfied at all time. First, we need to understand what is a constraint. A constraint is an equation (or inequation) which depends on the state s(t) and that has to be satisfied during the simulation. Usually we use a constraint function C(s). So typically, we can have the following constraint:

We say it is a position constraint because it is a constraint on s which contains the position and orientation of the bodies. When the motion of the first body of the constraint is fixed, the second body can move relatively to the first body with at most six degrees of freedom. The constraint function C(s) can at most constrain those six degrees of freedom (but not necessarily all of them). Therefore, in general, the constraint function C(s) is such that C : R 12 → R n where n is the number of degrees of freedom that the constraint removes from the system. Moreover, we can separate the constraint function C(s) in two functions. One function C trans (s) for the translation motion and one function C rot (s) for the rotation motion. Note that it is also possible to have an inequality constraint like:

The constraint is satisfied when the constraint equation 13 (or inequation 14) is satisfied with the current state s(t) of the bodies. If we take the time derivative of the constraint equation, we have:

where J is a n × 12 matrix called the Jacobian matrix of the constraint. Because it is the time derivative of a position constraint, we say that equation 15 is a velocity constraint. If we always update the velocities of the bodies such that the constraint 15 is satisfied, the bodies will always end up in positions and orientations that satisfy the constraint. When using a velocity constraint solver, we are working on the velocity level. It means that we are trying to find the velocities of the bodies such that the velocity constraints are valid. Then, we use those velocities to update the position and orientation of the bodies. Note that sometimes, we do not have a position constraint but we can directly create a velocity constraint. 

In general, the velocity constraint looks like this:

where the vector b is called the bias velocity vector. If the vector b is not null, it means that the constraint force F c will work. Sometimes, this is needed. For instance, it can be used for position correction or joint motors. As we have seen before, we are solving the constraints on the velocity level. Sometimes, some error can be introduced when updating the position of the bodies. This issue is called position error or position drift. We can add a term b in the velocity constraint in order to correct for this problem. The error of the position constraint is measured by the position constraint function C(s). If we want this error to be reduced to zero in the next timestep ∆t, the velocity needed to correct for this error is C(s) ∆t. However, we do not want the error to be removed in a single timestep. Instead, the velocity needed to correct for the position error is:

where β is a value between 0 and 1 called the bias factor. The bias factor describes the amount of error that is corrected at each timestep. This type of error correction is called Baumgarte stabilization [2]. 

Remember that our goal is to find the force F c that has to be applied on the bodies to keep the constraint satisfy. The force F c should only be there to keep the constraint satisfied but it should not work. It means that the force should not add energy into the system. This is the principal of virtual work. As explained in [1], this is valid only if the force F c is such that:

where λ is a n × 1 vector. To prove that this force does not work, we can compute the power P and check that it is zero. 

The variable λ is called a Lagrange multiplier. Observe that if we know the Jacobian matrix J, we only need to find the unknown λ to find the force F c. 

If we look at equation 12, we want the new velocity v i+1 to satisfy the velocity constraint 16. Therefore, we have:

We want to find λ. Therefore, we need to solve the equation 20 for λ. If the matrix K is invertible, we have the solution:

Once λ has been found, we can compute the force F c. 

If we use the sequential impulse technique from [4] to solve the constraints, we need to find the impulse P c. 

Then, we can use the equation 12 to find the new velocities v i+1 and the new positions s i+1 of the bodies. 

To sum up, in order to create a constraint, we need to find the position constraint function C(s). Then, we need to compute the time derivative of this function to obtain the velocity constraint. Using the velocity constraint, we have to identify the Jacobian matrix J and the bias velocity vector b. Then, we need to compute the matrix K = JM -1 J T. In the next section of this document, we will explain how to find all those quantities for different kinds of constraints that are commonly used in a rigid body simulation.
section_name: Constrained Dynamics

section_name: Constraints

section_content: Here, we will derive the constraint needed to make sure that two bodies in contact will not penetrate each other but will collide instead. This is called the penetration constraint. We also need another constraint to simulate friction between two bodies in contact. This is called the friction constraint.
section_name: Contact and Friction

section_content: Let's start with the penetration constraint. Consider that we have two rigid bodies B 1 and B 2 that are in contact. We call p 1 and p 2 the two contact points (in world-space coordinates) on body B 1 and body B 2 respectively. If x 1 and x 2 are the positions of the center of mass of body B 1 and B 2 respectively and r 1 and r 2 are the vectors from the center of mass of the each body to the contact points, we have:

We also need to have the surface normal n 1 at the contact point p 1 on the body B 1. The contact normal is a unit length vector pointing outside the body B 1. 

In order to find the penetration constraint, we would like to compute the penetration depth of the two bodies in contact. The penetration depth is basically the distance between the two contact points p 1 and p 2 in the direction of the contact normal n 1. This penetration depth is our penetration constraint function C pen (s):

Observe that the penetration depth between the two bodies is positive when they are separated (not in contact) and negative when the two bodies are penetrating. We want the contact constraint to be satisfied when the bodies are not penetrating. Therefore, the penetration constraint is valid when:
section_name: Position constraint

section_content: Now, we need to compute the time derivative of the contact penetration constraint in order to find the Jacobian matrix J. Here is how to do it:

In equation 27, we usually make the approximation that the penetration is very small (as in [3] ) and therefore we can ignore the second term. We have now found the 1 × 12 Jacobian matrix J pen for the contact penetration constraint. 

Note that we have not created a position constraint for friction. This is because the friction can only be described by a constraint on the velocity level (like a motor constraint). Consider two unit length vectors u 1 and u 2 that are orthogonal to the contact normal vector n 1. Those two vectors span the contact plane. The idea is to slow down the rigid bodies in the direction of the two vectors u 1 and u 2 to simulate friction. We will use the two following friction constraints for that:

Those constraints are satisfied when Ċfric 1 (s) = 0 and Ċfric 2 (s) = 0. It means that they will try to stop the relative motion of the two bodies. However, the constraint force F c that we are going to apply to keep the constraint satisfied have to be bounded. In the Coulomb's friction law, the friction force F c is bounded according to the following relation:

where F n is the contact normal force and µ is the friction coefficient. Therefore, we have:

Note that in the application, we use λ = λ∆t. Therefore, we have:

We have found the bounds on the Lagrange multiplier λ f ric used to find the constraint force for the friction constraint.
section_name: Time derivative

section_content: Now, we need to compute the 1 × 1 matrix K pen for the contact penetration constraint:

Note that we have used the fact that the normal vector n 1 is a unit length vector. Therefore, we have n T

1 n 1 = 1. Now, we have found the 1 × 1 matrix K pen. Now, we will see how to compute the two 1 × 1 matrices K f ric 1 and K f ric 2 for the two friction constraints.
section_name: Constraint mass matrix K

section_content: The bias velocity vector b pen for the contact penetration constraint is used for two things. 

First, we use it to correct the position error as discussed in section 1. The position error for this constraint is the penetration depth. As we have seen, we can compute the term b error of the bias velocity by:

where C pen (s i ) is the evaluation of the penetration position constraint at state s i. Note that in this situation, the vector b error is a scalar value. Secondly, we use the bias velocity vector b pen to introduce a velocity restitution. For instance, when an object falls on the floor, it might bounce. Therefore, we need to introduce some velocity reflection when a contact occurs. The relative velocity v n between the two bodies in the direction of the contact normal n 1 is given by:

Therefore, after the contact, we want the following relative velocity v n :

where α is the restitution factor between 0 and 1. When α = 0, the bodies will not bounce at all and when α = 1 the whole relative velocity before the contact will be restitued and the bodies will be very bouncy. We can use the following bias velocity vector b res for the restitution:

Therefore, the final bias velocity vector for the contact penetration constraint is:

At the end, here is our final velocity constraint for the contact penetration:

Usually, we do not need any position correction for the friction constraints. Therefore, we have the following velocity constraints for friction:
section_name: Bias velocity vector

section_content: The Ball-And-Socket joint only allows arbitrary rotation between two bodies but no translation. It has three degrees of freedom. To create a ball-and-socket joint, the user only has to specify an anchor point in world-space coordinates. At the joint creation, we store the anchor point in the local-space of each body. Then, at each frame and for each body, we convert the local-space anchor point back into the world-space to have the anchor point p i for each body B i.
section_name: Ball-And-Socket Joint

section_content: The ball-and-socket joint does not constrain the rotation motion and therefore, we only need a position constraint C trans for the translation. We have the following position constraint function:

where x 1 and x 2 are the world-space positions of body B 1 and B 2 and r 1 and r 2 are the vectors from body center to the anchor point p i in world-space coordinates (p i = x i + r i ). This constraint specifies that the world-space positions of the anchor points of both bodies must be equal. 

This constraint removes three translation degrees of freedom from the system. Therefore, we have : C trans (s) : R 12 → R 3. The constraint is satisfied when :
section_name: Position constraint

section_content: Then, we need to compute the time derivative Ċtrans (s) in order to find the Jacobian matrix. 

where E 3 is the 3×3 identity matrix and [r 1 ] x is the 3×3 skew-symmetric matrix constructed using the vector r 1 (see appendix A). We also have J trans which is the Jacobian matrix that is a 3 × 12 matrix in this case and v is a 12 × 1 vector that contains the linear and angular velocities of bodies B 1 and B 2.
section_name: Time derivative

section_content: Now, we need to compute the matrix K trans. Here is how to compute the 3 × 3 matrix K trans.
section_name: Constraint mass matrix K

section_content: The bias velocity vector b trans for the ball-and-socket joint is used to correct the position error as discussed in section 1. As we have seen, we can compute the term b trans of the bias velocity with:

where C trans (s i ) is the evaluation of the position constraint at state s i. At the end, here is our final velocity constraint for the ball-and-socket joint:
section_name: Bias velocity vector

section_content: A slider joint only allows relative translation between two bodies in a single direction. It has only one degree of freedom. The slider joint is defined by a slider axis a that is the direction of translation and by an anchor point in world-space coordinates. At the joint creation, we store the anchor point in the local-space of each body. Then, at each frame and for each body, we convert the local-space anchor point back into the world-space to have the anchor point p i for each body B i.
section_name: Slider Joint

section_content: We have the following translation position constraint :

where x 1 and x 2 are the world-space positions of body B 1 and body B 2 and r 1 and r 2 are the vectors from body center to the world-space anchor point of each body (p i = x i + r i ). The two vectors n 1 and n 2 are two unit orthogonal vectors that are orthogonal to the slider axis a. At the joint creation, we convert the slider axis a into the local-space of body B 1 and we get the vector a l. Then, at each frame, we convert the vector a l back to world-space to obtain the vector a w. Then, we create the two orthogonal vectors n 1 and n 2 that are orthogonal to a w. The two previous translation constraints specify that there should be no relative translation orthogonal to the slider axis a. This constraint removes two degrees of freedom from the system. Therefore, we have : C trans (s) : R 12 → R 2. The constraint is satisfied when:

Here is the rotation position constraint:

Here θ ix, θ iy, θ iz are the orientation angles of the body B i around the x, y and z axis. Those three rotation constraints mean that there should be no relative rotation between the two bodies. This constraint removes three degrees of freedom from the system. Therefore, we have: C rot (s) : R 12 → R 3. The constraint is satisfied when:
section_name: Position constraint

section_content: Then, we need to compute the time derivative Ċ(s) in order to find the Jacobian matrix. 

Here is the time derivative of the translation position constraint C trans :

The jacobian matrix J trans is a 2 × 12 matrix. Note that we have used the fact that the vectors n 1 and n 2 have been created from the vector a l that is stored in the local-space of body B 1. Therefore, the vectors n 1 and n 2 are fixed length vectors rotating at the angular velocity ω 1 of body B 1. This is why we have:

Here is the time derivative of the rotation position constraint C rot :

We have found the 3 × 12 Jacobian matrix J rot.
section_name: Time derivative

section_content: Now, we need to compute the constraint mass matrix K trans for the translation constraint:

where :

Now, we need to compute the 3 × 3 matrix K rot for the rotation constraint. 

(60)
section_name: Constraint mass matrix K

section_content: The bias velocity vectors b trans and b rot for the translation and rotation constraints of the slider joint are used to correct the position error as discussed in section 1. As we have seen, we can compute those vectors with:

where C trans (s i ) and C trans (s i ) are the evaluations of the position constraints at state s i. 

Finally, here are the final velocity constraints for the slider joint:
section_name: Bias velocity vector

section_content: It is possible to specify limits for the slider joint to constrain the range of motion along the translation axis. 

We consider the vector u between the two world-space anchor points p 1 and p 2 of each body:

If we take the dot product of u and the slider axis vector a, we get the distance d between the anchor points along the slider direction:

We will use this distance d between the two bodies as the relative translation along the slider axis. At the beginning, the vector u is zero and therefore, the distance d is also zero. The user is able to define two translation limits d min and d max such that d min ≤ 0 and d max ≥ 0. We will use two additional constraints for the limits of the joint. One for the minimum limit and one for the maximum limit. As for the slider joint position constraint, we will derive the position constraints for the limits. 

The minimum limit is specified by the d min distance. The minimum limit constraint is violated when:

Using this, we can create a minimum limit position constraint C min (s) :

This limit constraint is satisfied when C min (s) ≥ 0. This position constraint is such that : C min (s) : R 12 → R. As for the slider joint constraint, we need to calculate the time derivative of the position constraint in order to isolate the Jacobian matrix J min. 

Here J min is a 1 × 12 matrix. 

Then, we can compute the 1 × 1 matrix K min :

Note that we used the fact that a is a unit vector and therefore a T a = 1. 

The maximum limit is specified by the d max distance. The maximum limit constraint is violated when:

Using this, we can create a maximum limit position constraint C max (s):

This limit constraint is satisfied when C max (s) ≥ 0. This position constraint is such that : C max (s) : R 12 → R. As for the slider joint constraint, we need to calculate the time derivative of the position constraint in order to isolate the Jacobian matrix J max. 

Here J max is a 1 × 12 matrix. 

When we compute the 1 × 1 matrix K max, we obtain the following result:

The bias velocity vectors b min and b max for the limits constraints of the slider joint are used to correct the position error. Here is how to compute them:

where C min (s i ) and C max (s i ) are the evaluations of the limit constraints at state s i. Finally, we have the following two velocity constraints for the limits:
section_name: Limits

section_content: The motor of the slider joint is used to keep a relative speed v motor between the bodies of the joint along the slider axis. In order to keep this relative speed we need to apply a force that cannot exceed a given maximum force F max specified by the user. The motor is represented by a new constraint between the two bodies of the joint. Note that for a motor, we do not have a position constraint. Instead, we are directly working on the velocity level. Here is the constraint involving the velocities of the bodies:

This equation means that the relative velocity difference between the two bodies projected onto the slider axis a has to be the motor speed v motor. Therefore, we can create the following velocity constraint function Ċmotor (s):

As usual we can rewrite the velocity constraint using a Jacobian matrix J motor :

Note that we have the velocity bias vector b = v motor (a scalar value in this situation). 

Now that we have the Jacobian matrix J motor, we can compute the matrix K motor :

We still need to find the valid bounds on the Lagrange multiplier λ motor used to solve the constraint. We know the maximum allowed force F max to be applied in order to satisfy the constraint. Remember that this maximum allowed force is given by the user. We want:

Note that in the application, we use λ = λ∆t. Therefore, we have:

We have found the bounds on the Lagrange multiplier λ motor used to compute the constraint force for the slider motor.
section_name: Motor

section_content: A hinge joint only allows relative rotation between two bodies around a single axis. It has one degree of freedom. The hinge joint is defined by a unit length world-space hinge axis a vector that is the rotation axis around which the two bodies can rotate and by a world-space anchor point. At the joint creation, we store the anchor point in the local-space of each body. Then, at each frame and for each body, we convert the local-space anchor point back into the world-space to have the anchor point p i for each body B i.
section_name: Hinge Joint

section_content: We have the following translation constraint function:

where x 1 and x 2 are the world-space positions of body B 1 and body B 2 and r 1 and r 2 are the vectors from body center to the anchor point of each body in world-space coordinates (p i = x i + r i ). This translation constraint specifies that there should be no relative translation between the two anchor points of each body. This constraint removes three degrees of freedom from the system. Therefore, we have : C trans (s) : R 12 → R 3. The constraint is satisfied when:

Note that this is exactly the same translation constraint as for the ball-and-socket joint (see equation 45). Now, we need to find the rotation constraint for the hinge joint. At the beginning of the simulation, when the user specifies the world-space hinge axis vector a, we convert this vector into two local-space vectors a 1l and a 2l in each local-space of the two bodies of the joint. Then, at each frame, we convert those two vectors back in world-space. So we get the two unit length vectors a 1 and a 2 in world-space coordinates. Then, we compute two unit orthogonal vectors b 2 and c 2 that are orthogonal to the vector a 2. 

Here is the rotation constraint function:

Those two rotation constraints mean that the only allowed rotation between the bodies is around the hinge axis. This constraint removes two rotational degrees of freedom from the system. Therefore, we have : C rot (s) : R 12 → R 2. The constraint is satisfied when:
section_name: Position constraint

section_content: Then, we need to compute the time derivative Ċ(s) in order to find the Jacobian matrix. 

As we have seen before, the translation constraint of the hinge joint is exactly the same as for the ball-and-socket joint. Therefore, the time derivative of the translation constraint is given by equation 47 and we have the following Jacobian matrix:

Now, we need to compute the time derivative of the rotation constraint C rot (s) in order to find the Jacobian matrix J rot. Here is how to do it:

Here, J rot is a 2 × 12 matrix.
section_name: Time derivative

section_content: As we have seen before, the translation constraint of the hinge joint is the same as the one of the ball-and-socket joint. Therefore, the 3 × 3 matrix K trans is already given by equation 48. 

Now, we need to compute the 2 × 2 matrix K rot for the rotation constraint. Here is how to do it :

where :
section_name: Constraint mass matrix K

section_content: The bias velocity vectors b trans and b rot for the translation and rotation constraints of the hinge joint are used to correct the position error as discussed in section 1. As we have seen, we can compute those vectors with:

where C trans (s i ) and C trans (s i ) are the evaluations of the position constraints at state s i. 

Finally, here are the final velocity constraints for the hinge joint:
section_name: Bias velocity vector

section_content: With the hinge joint, it is also possible to have limits to constrain the range of motion along the translation axis. The limits that the user can specify are the minimum and maximum relative rotation angle around the hinge axis. 

Consider q 1 and q 2 the two quaternions representing the orientation of body B 1 and body B 2. When the joint is created, we compute the initial orientation difference between the two bodies. This is another quaternion called q init. q init = q 2 q -1 1 (101) Then, at each frame, we compute the current orientation difference q current between the two bodies with:

Then, we compute the relative orientation difference q dif f between the current and initial state. We have:

Now, we need to extract the angle θ from the quaternion q dif f. To do this, we can rewrite the quaternion as:

The atan2(x, y) function returns an angle in the range (-π; π]. 

The user is able to define two angle limits θ min and θ max such that θ min ∈ [-2π; 0] and θ max ∈ [0; 2π]. Those are the two limit angles for the relative rotation of the two bodies of the joint around the hinge axis. Note that we consider that at the joint creation, the relative angle between the bodies is zero. Moreover, we only work with angles in radian in the range [-π; π]. 

We will use two additional constraints for the limits of the joint. One for the minimum limit and one for the maximum limit. As for the hinge joint position constraint, we will derive the position constraints for the limits. 

The minimum limit is specified by the θ min angle. The minimum limit constraint is violated when :

Using this, we can create a minimum limit position constraint C min (s) :

This limit constraint is satisfied when C min (s) ≥ 0. This position constraint is such that : C min (s) : R 12 → R. Now, we need to calculate the time derivative of the position constraint in order to find the 1 × 12 Jacobian matrix J min. 

where ω = ω 2 -ω 1 is the angular velocity difference between the two bodies. The function θ(t) is the angle between the two bodies around the hinge axis. The corresponding angular velocity ω is defined by :

where u is the unit length vector of the rotation axis. Here, the rotation axis is the hinge axis a. Therefore, we have :

Moreover, the hinge axis a is a unit length vector. Therefore, we have :

This equality has been used in equation 109. 

Then, we can compute the corresponding 1 × 1 matrix K min :

That is all for the minimum limit. Now, we need to consider the maximum limit. The maximum limit is specified by the θ max angle. The maximum limit constraint is violated when:

Using this, we can create a maximum limit constraint C max (s) :

This limit constraint is satisfied when C max (s) ≥ 0. This position constraint is such that : C max (s) : R 12 → R. Now, we need to calculate the time derivative of this position constraint in order to isolate the Jacobian matrix J max. 

Here J max is a 1 × 12 matrix. 

When we compute the 1 × 1 matrix K max, we obtain the following result:

Therefore, we have K min = K max for the limits of the hinge joint. 

The bias velocity vectors b min and b max for the limits constraints of the hinge joint are used to correct the position error. Here is how to compute them :

where C min (s i ) and C max (s i ) are the evaluations of the limit constraints at state s i. Finally, we have the following two velocity constraints for the limits :
section_name: Limits

section_content: The motor of the hinge joint is used to keep a relative angular speed ω motor between the bodies of the joint around the hinge axis. In order to keep this relative speed, we need to apply a force F c that cannot exceed a given maximum force F max specified by the user. The motor is represented by a new constraint between the two bodies of the joint. Here is the constraint between the angular velocities of the bodies :

This equation means that the relative angular velocity difference of the two bodies around the hinge axis a has to be the motor speed ω motor. Therefore, we can create the following velocity constraint function Ċmotor (s):

As usual we can rewrite the velocity constraint using a 1 × 12 Jacobian matrix J motor :

Note that we have the error velocity vector b = ω motor (a scalar value in this situation). Now that we have the Jacobian matrix J motor, we can compute the 1 × 1 matrix K motor. Observe that the Jacobian matrix J motor is equal to the Jacobian matrix J min of the hinge joint minimum limit constraint. Therefore we have:

Now, we still have to find the valid bounds on the Lagrange multiplier λ motor such that the constraint force F c used to satisfy the constraint is smaller than the maximum allowed force F max. Using the same derivation as for the slider joint motor, we have the following bounds:
section_name: Motor

section_content: A fixed joint does not allow any relative motion (neither translation nor rotation) between the two bodies of the joint. It has zero degrees of freedom. The fixed joint is defined by a single anchor point. At the joint creation, we store the anchor point in the local-space of each body. Then, at each frame and for each body, we convert the local-space anchor point back into the world-space to have the anchor point p i for each body B i.
section_name: Fixed Joint

section_content: We have the following translation constraint:

where x 1 and x 2 are the world-space positions of body B 1 and body B 2 and r 1 and r 2 are the vectors from body center to the anchor point of each body in world-space coordinates (p i = x i + r i ). This translation constraint specifies that there should be no relative translation between the two anchor points. This constraint removes three degrees of freedom from the system. Therefore, we have : C trans (s) : R 12 → R 3. The constraint is satisfied when:

Note that this is exactly the same translation constraint as for the ball-and-socket joint (see equation 45). 

For the rotation, we have the following constraint function:

Here θ ix, θ iy, θ iz are the orientation angles of the body B i around the x, y and z axis. Those three rotation constraints mean that there should be no relative rotation between the two bodies. This constraint removes three translation degrees of freedom from the system. Therefore, we have : C rot (s) : R 12 → R 3. The constraint is satisfied when:

Observe that this is exactly the same rotation constraint as for the slider joint (see equation 53).
section_name: Position constraint

section_content: Then, we need to compute the time derivative Ċ(s) in order to find the Jacobian matrix. 

As we have seen before, the translation constraint of the fixed joint is exactly the same as the one for the ball-and-socket joint. Therefore, the time derivative of the translation constraint is given by equation 47 and we have the following Jacobian matrix:

The rotation constraint of the fixed joint is the same as the one for the slider joint. Therefore, the time derivative of the rotation constraint is given by equation 58 and we have the following Jacobian matrix:
section_name: Time derivative

section_content: As we have seen before, the translation constraint of the fixed joint is the same as the one for the ball-and-socket joint. Therefore, the 3 × 3 matrix K trans is already given by equation 48. 

The rotation constraint of the fixed joint is the same as the rotation constraint of the slider joint. Therefore, the 3 × 3 matrix K rot is given by equation 60. 

(134)
section_name: Constraint mass matrix K

section_content: The bias velocity vectors b trans and b rot for the translation and rotation constraints of the fixed joint are used to correct the position error as discussed in section 1. As we have seen, we can compute those vectors by:

where C trans (s i ) and C trans (s i ) are the evaluations of the position constraints at state s i. 

Finally, here are the final velocity constraints for the fixed joint:

Ċtrans (s) + b trans = 0 (137) Ċrot (s) + b rot = 0 (138)
section_name: Bias velocity vector

section_content: The cross product a × b can be written as a multiplication of the 3 × 3 matrix [a] x and the vector b:

where [a] x is a 3 × 3 skew-symmetric matrix constructed using the vector a. Remember that a square matrix A is a skew symmetric matrix if we have:

B Time derivative of a rotation matrix

Let R = R(t) be a 3 × 3 rotation matrix. We would like to find an expression for the time derivative of this rotation matrix. We know that a rotation matrix is orthogonal and therefore, we have:

It also means that we have:

where E 3 is the 3 × 3 identity matrix. If we take the time derivative of both sides of this equation, we have: If we observe the equation 144, we can see that S is a skew-symmetric matrix (see equation 140). From equation 143, we have:

where [ω] x is a 3 × 3 skew-symmetric matrix created as in appendix A with the angular velocity vector ω. Therefore, if we want to compute the time derivative of a rotation matrix R knowing the angular velocity ω, we simply have the following equation:
section_name: A Cross product as matrix multiplication

section_name: C Time derivative of a fixed length vector

section_content: Consider that we have a fixed length vector v(t) that is rotating at an angular velocity ω(t). The time derivative of vector v(t) is given by:

Therefore, the time derivative of a fixed length vector is a vector perpendicular to the vector v and to the angular velocity ω.
section_name: C Time derivative of a fixed length vector