Previous URDF loader did not handle off-diagonal elements
in the URDF. This commit adds functionality to allow
the loading of an inertia matrix that has not already
been reduced to its principal components. It diagonalizes
the inertia matrix, tests that the values are real and
updates the inertial frame.
Small change that takes the world gravity and applies it to the
body during the call to inverse dynamics in the shared memory
interface. Otherwise the gravity vector is left at the default
value and there is currently no interface to set the gravity
for the inverse dynamics system.
Jeffrey Bingham