Algorithms

calcXd!(xx, x, m, tm, t)

Conventions: n̂ refers to the contact surface normal that points into body B v_cart refers to + velocity of B - the velocity of A the wrench is the wrench applied TO body B