bullet-featherstone: FrameData is incorrect for collisions, joints, and models

[iche033]

Environment

• OS Version: Ubuntu 20.04
• Source or binary build? source build, gz-physics7 branch

Description

In bullet-featherstone plugin, The frame data computation is done in the KinematicsFeatures::FrameDataRelativeToWorld. The Pose FrameData for collisions was updated to take into account collision pose offset in #647. However, the velocities FrameData still do not incorporate the collision pose offset.

[iche033]

The JointFrameSemantics may also be incorrect in bullet-featherstone plugin. After fixing / enabling these frame data checks, the test fails:

97: Expected equality of these values:
97:   F_WCexpected.pose.translation()
97:     Which is: { 0, 1.23942, -0.569664 }
97:   childLinkFrameData.pose.translation()
97:     Which is: { 0, 0.619709, 0.215168 }

The F_WCexpected.pose.translation() is calculated using joint frame semantics and the resulting values are incorrect, while the values of childLinkFrameData.pose.translation are correct.

[iche033]

ModelFrame data is also incorrect, see #647 (comment).

This commit adds a failing test: 27deae9