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:
Next up will be using this to actually control a real leg.