More integration fixes

[azeey]

While working on #9 , I found that getLinearAcceleration did not return the correct value. Investigating that further, I found that the integration fix in #5 did not apply when constraint impulses are involved. Furthermore, I found problems with the integration of position in FreeJoint::integratePosition when the COM of a body is not at the origin. This PR fixes both issues and adds a test for the COM offset issue. I wasn't able to create a simple test for the first issue, but I will update #9 to use getLinearAcceleration in the test.

The linear acceleration issue is fixed by a Coriolis term from the acceleration before integrating it to obtain a new velocity. My intuition for this is that the FreeJoint::integratePosition updates the spatial velocity based on the newly integrated position. This update to spatial velocity is in itself an acceleration and it does what the Coriolis term would have done had we FreeJoint::integratePosition not updated the spatial velocity.

The COM offset issue is fixed by transforming all quantities into the COM frame before integration. After integration, the values are transformed back into the body's origin frame.

I have tested this PR with Gazebo-classic and it fixes the test AddForce test in INTEGRATION_physics_link. I was even able to remove the tolerance adjustment in https://github.com/osrf/gazebo/blob/a9f093f2fc7fa7fc49efa73719f6536ad28f8af7/test/integration/physics_link.cc#L164

[scpeters]

this sounds great! I'll try running the benchmarks from https://github.com/scpeters/benchmark as well

[scpeters]

I haven't had a chance to review the math yet, but it fixes an INTEGRATION_physics_link gazebo test that had been broken for dart, which is an improvement. I also tested it with the boxes test in scpeters/benchmark and it has fixed all the accuracy problems I had previously noted. I didn't have enough time to get a test working with an offset inertial pose because the test uses SetLinearVel and SetAngularVel API's, which work differently in ODE and DART. I will try that test at some point, but I'm ready to approve this now.