Last time I updated my inverse kinematics solution to also include dynamics, velocities and forces.  Now I’m in the process of integrating this onto the robot.

The old SMMB / HerkuleX control software commanded the servo positions in an open loop, which did not take into account the actual position of the joints in any way.  What I’ve done now is implemented a control flow where at each cycle the state of all 12 servos is sampled, then the control laws are applied based on that state, then the resulting commands are sent out.

So far, the only control laws I have ported are a simple one which just sends raw commands to each joint, and a somewhat more useful one which commands the end effector of each joint in terms of position, velocity, and force.  Here’s a quick video showing it commanding various constant forces with no position constraints.  The scale isn’t super accurate, nor is the actual force coming out of the device, but I think it should be good enough.