Fix joint force/torque feedback

[iche033]

We ran into an issue downstream in gz-physics (Gazebo) with large force / torque sensor feedback when using bullet featherstone (btMultibody) implementation. The problem is similar to what's described in https://github.com/bulletphysics/bullet3/issues/2966 where user reported larger than expected joint reaction force/torque values when using TORQUE_CONTROL. This bullet issue is closed as it seems like the original user switched to using POSITION_CONTROL. The patch in this PR is the code posted in https://github.com/bulletphysics/bullet3/issues/2966#issuecomment-688136359 by @Steven89Liu, which fixes the issue for us.

We have a simple test setup that checks the FT sensor output:

In the above setup, the payload (orange) is connected to the end link with a force/torque sensor attached. There is a joint controller at the revolute joint connecting the base link and the end link.

• Expected value of force: weight of the attached payload link in the sensor frame ([0 0 -9.8]).
• Actual value of force: different from above, and magnitude is larger than the weight of the payload link ([0 -0.34 -13.85]).

The force output value is correct when run with the physics engine (dartsim) in gz-physics. With this patch, the F/T sensor now also reports the expected value with bullet.

[azeey]

Hi @erwincoumans, we were hoping to resolve an issue in Gazebo with this PR. Would you be able to review it?

[erwincoumans]

Thanks for bringing this to attention. I've suspended the PyBullet project but let's merge this change.