A while ago I derived some closed form solutions for the inverse force dynamics for a 2 dimensional mammal geometry leg.  Now that I want to execute more dynamic gaits, I need to be able to control the velocity and force of the 3 dimensional leg in real time.  That means I need to be able to go forward and back between the two representations.  The first being joint angles, joint velocities, and joint torques — the second being 3D position, velocity, and force.

Rather than derive those myself in 3 dimensions, I decided to save a bit of time now by using the DART kinematics and dynamics routines.  I might need to undo this later if it turns out to be too slow at runtime, but if nothing else it can be a good ground truth for anything I do myself.  I also took this opportunity to do all calculations in consistent rational coordinate frames, where before I had conventions that while documented, were rather unorthodox.

As it turns out, this demonstrated that my old 3 dimensional force calculation, was as I had expected, slightly incorrect.

The shoulder torque was the most affected, being off by 20% or so.

The source is at:

• https://github.com/mjbots/mjmech/blob/master/mech/mammal_ik.h
• https://github.com/mjbots/mjmech/blob/master/mech/mammal_ik.cc

Next up will be using this to actually control a real leg.