So after quite a bit on work on the motor controller firmware, things are starting to come together. Though the motor position estimation isn't working quite as well as I would like, it is good enough to control the motor and drive it reasonably efficiently. There is also quite a bit more parameter tuning needed on the PID controllers, but I will wait until a full leg is assembled so I can tune it with a more realistic load.
In the following video I am using a slider on a GUI to control the position of the motor. Here I am using a single 14.8V battery. I plan to use two of these in series to provide a 30V supply to reduce the current in the supply wiring, as you can see from the following video speed doesn't look like it will be a problem!