Add contact forces to each body node as external forces. This is for correct joint force/torque sensoring.

This is not a long term solution.
Fix typo.
Merge branch 'contact_force'