SimbodyLink: add and set force and torque

[jeduardo211]

This PR adds the implementation of methods for applying forces and torques to links when using the Simbody physics engine.

The following methods are now implemented in the SimbodyLink class:

• AddForce
• AddRelativeForce
• AddForceAtWorldPosition
• AddForceAtRelativePosition
• AddLinkForce
• AddTorque
• AddRelativeTorque

Additionally, the implementation of the SetForce, and SetTorque methods was modified to ensure that setting the force applied to the body do not alter the torque currently applied to the body and vice versa.

In order to test the above methods, the AddForce test of the physics_link integration tests is now enabled for Simbody. Please note that Simbody required an additional step to update the pose of the link and its inertial origin, and thus the AddForce test was modified to reflect this.

Note: The OnWrenchMsg test is still disabled for Simbody. There is an easy way to enable this test, by adding a "auto_reset_forces" parameter to the SimbodyPhysics and checking the value of this parameter in the SimbodyPhysics::UpdatePhysics method to determine if the forces and torques applied should be cleared before the next step. However, the SimbodyPhysics class is not following the PIMPL pattern, and thus adding this parameter would introduce breaking changes in the API. Because of this these modifications were not included and the OnWrenchMsg test remains disabled for simbody. If this change is acceptable please advise and I will create a new PR to enable this test.

Solves issue #1478.

[jeduardo211]

@scpeters Could you please review this PR?

[jeduardo211]

Any updates on this?

[scpeters]

Any updates on this?

sorry I haven't had a chance to finish my review, but I'll try to take a look this week

[talregev]

@jeduardo211 Can you rebase to the latest gazebo11 dev?