worldPose() helper function in Util.hh does not properly compute world poses for Joints

[scpeters]

Description

• Expected behavior: the worldPose() helper function in Util.hh should be able to compute the world pose of a Joint.
• Actual behavior: it does not work properly (see note in ForceTorque system)

I believe this is because the recursive pose calculation computes the joint's pose relative to its parent model, rather than relative to its child link (as defined in SDFormat).

Steps to reproduce

1. TODO: update Unit test to demonstrate the error

Output

[azeey]

Related to #1042

[scpeters]

ok, I'll move discussion to #1042

[scpeters]

duplicate of #1042