The inertia tensor computation was wrong previously. Fortunately it does not cause a problem as our COM and the body origin coincide in the example
make modification based on Andres comments