Two changes:
- The default configuration was outside of the position limits causing issues in the simulation. The new configuration is within all joint limits.
- I changed three joints in the URDF to revolute from continuous. They do have limits and otherwise Pinocchio uses SO(2) which is not what we want.