3.1. How Featherstone is Organized
When we call Skeleton::computeForwardDynamics() or Skeleton::getCoriolisAndGravityForces(), we run a recursive algorithm called "Featherstone" to find appropriate behaviors for our multi-body skeletons. We need to both run these algorithms, and be able to take derivatives through them. As a reference for both the forward Featherstone algorithm and the gradients through it, we use the GEAR paper. Here are images describing how those algorithms map onto our code.
Here's how we compute Skeleton::computeForwardDynamics(), and how it maps onto the GEAR paper:
Here's how we compute Skeleton::getCoriolisAndGravityForces(), and how it maps onto the GEAR paper:
Here's how we compute the Jacobian of Skeleton::getCoriolisAndGravityForces(), with Skeleton::getJacobianOfC(), and how it maps onto the GEAR paper:
For each degree of freedom i of a Joint, we can understand joint.getRelativeJacobian().col(i) as being the screw (in SE3) along which we'll translate our joint.getRelativeTransform() when we perturb i.position += eps. So perturbing i.position += eps is the same as right-multiplying our relative transform by the exponentiated screw joint.getRelativeTransform() *= math::expMap(joint.getRelativeJacobian().col(i)*eps). It turns out that for the vast majority of joints, the screw given by joint.getRelativeJacobian().col(i) also tells us how we'll translate the child body node's childBody.getRelativeSpatialVelocity() as we perturb i.velocity += eps, using the exact same right multiply. This fact is broadly exploited around Robotics in general.
In our codebase, things aren't quite that simple. For FreeJoint and BallJoint, as we change joint velocity, we map onto spatial velocity with the identity (since these are defined in the same space). However, as we change joint position, we are performing an operation in SE3 space directly, and that operation isn't the same as rotating by a simple one-hot screw. For these, we need a different Jacobian, involving math::expMapJac().
So we need a distinction between a Jacobian composed of screws for relative spatial position changes, and a Jacobian composed of screws for relative spatial velocity changes.