dart seems to return incorrect world jacobian for the translational degrees of freedom of a freejoint

[markuswnuk91]

Bug Report

• I checked the documentation and the forum but found no answer.
• I checked to make sure that this issue has not already been filed.

Environment

• DART version: 6.12.1
• OS name and version name(or number): Ubuntu 18.04 and newer
• Compiler name and version number: GCC 8.0.0 or newer
• Using dartpy python API

Expected Behavior

dart::dynamics::bodyNode::getJacobian(offset, dart::dynamcis::Frame::World) or dart::dynamics::bodyNode::getWorldJacobian(offset) returning a jacobian in world coordinates also when using a dart::dynamics::freejoint as first joint.

Current Behavior

given a dart::dynamics::freejoint the jacobian entries corresponding to the translational degrees of freedom of the joint (q[3:6]) seem to be incorrect.

Steps to Reproduce

1. create a bodyNode with a freeJoint
2. set the a rotational degree of freedom of the joint, such that the body is not well aligned with the world frame (e.g. q[0] = 1)
3. obtain the worldJacobian for the center of the bodyNode and observe the jacobian entries corresponding to the translational degrees of freedom of the joint (e.g worldJacobian[(3:6),(3:6)])
4. observe that the entries of the jacobian are not an identity matrix.
5. observe that when the translational degrees of freedom q[3:6] are set , the body translates along the x,y,z coordinates of the world frame.
6. since the worldJacobian relates the positional changes of the body given in the world frame to the changes of the generalized coordinates, the jacobian entries for these translational should be an identity matrix.

Code to Reproduce

A minimal example to reproduce the observed behaviour is attached as python code. (just paste code provided in the attached .txt file in a .py file and execute with python 3)
It provides further explanation and visualization of what behavior is observed.

Further a little workaround which worked for my case is presented.
Solved by multiplying the part of the returned jacobian matrix which corresponds to the translational degrees of freedom with the inverse of the relative Transform's orientation of the freejoint.
Unfortunately, I am not able to provide a solution or fix for the C++ implementation.

testDartWorldJacobian.txt

[markuswnuk91]

in case it leads to trouble in executing the example code: delete line 8 --> sys.path.append(os.getcwd().replace("/test", "")) it is an artifact from my own coding project.