Fix FreeJoint velocity and position integration for articulated bodies

[azeey]

#10 attempted to fix an integration issue when the COM offset from the link origin. However, this seems to work only for single body systems. With an articulated body, the fix in #10 actually caused errors that lead to fictitious forced being applied. This in essence reverts #10 while keeping the necessary changes for updateConstrainedTerms and adding more tests.

However, reverting #10 means small errors will be introduced when the COM is offset from the link origin. In a similar vain, if two bodies are connected by a fixed joint and a torque is applied, the whole system will rotate about the lumped COM. However, due to the way the velocities and positions are integrated in FreeJoint, this will also incur errors. I had to reduce the force and torque values in order to make tests pass. I think we should revisit this and see if it would be possible to eliminate these errors by applying the integration at the lumped COM.

/cc @Yadunund

[mxgrey]

Just to clarify, do we believe that JS's comment here is correct, that the root problem is some kind of numerical error accumulation due to calculating the forward dynamics in the local body frame? If so, I imagine we should focus future efforts on revising that convention in the forward dynamics. It would be tedious work that would touch on all the different joint types in DART, but it should be very doable, and shouldn't require any special case handling. As far as I'm aware, the choice to use the local body frame for the forward dynamics is recommended by Featherstone because it reduces the number of matrix transform operations that need to be performed, making it more efficient than using the global frame. But it seems like the benefit of that efficiency is dwarfed by the errors that accumulate, assuming that is the root cause of this problem.

The measures in this PR seem like a reasonable way to deal with this problem for now, and the additional tests are definitely great to have, so I'm happy to give this an approval.

Regarding the failing tests, do we want to increase the tolerance to allow the tests to pass, or just leave the tests failing as a reminder to revisit this problem?

[azeey]

Thanks for the review @mxgrey! I do think the root problem comes from using body frames in the forward dynamics algorithm and how we integrate such quantities. Section 8.3 of Jakub Stępień's dissertation talks about the problem we're seeing here and attributes the issue to the use of body frames. But changing the system to use global frames is a daunting task. Using global frames may also have numerical issues when a body is far from the fixed frame's origin resulting in large values in the "linear" portion of the spatial velocity.

Regarding the failing tests, do we want to increase the tolerance to allow the tests to pass, or just leave the tests failing as a reminder to revisit this problem?

I believe the tests fail because the values expected (dq and dqq) are the component wise differences between quantities before and after a step(). However, when FreeJoint is involved, the quantities after step are not just changed by the integration, but also by the change in orientation of the frame (since the quantities are expressed in body frame). This causes an extra delta that is not accounted for when doing component wise differences. I think it's best if we leave the tests failing for now and revisit the problem.

Edit: Added part about failing tests.