Recovering joint origin (Problem with of joint frame orientation)

[jmainpri]

Hi,

I am trying to recover the orientation of the joint origin in the URDF file by doing:

 info = p.getJointInfo(robot_id, i)
 state = p.getLinkState(robot_id, i - 1)
 t_com_l = transform_pyb(state[2], state[3])
 t_joint = transform_pyb(info[14]), info[15])
 origin = t_com_l @ t_joint

• state[2] : local position offset to COM
• state[3] : local orientation offset to COM
• info[14] : position of joint in parent frame
• info[15] : orientation of joint in parent frame

transform_pyb just creates a 4x4 homogenous transformation.

But the orientation info[15] in the transformation does not match the origin of the joint in the URDF. The resulting position in origin is ok.

I run out of ideas to go further apart from reading the C++ code. Is it a bug in my version of pybullet? Do I miss something? Are orientations and positions handled differently?

[erwincoumans]

Can you provide a small full working script, together with a specific URDF file, so someone maybe able to verify? (PyBullet has a few rudimentary methods such as pybullet.multiplyTransforms and pybullet.inverseTransform if needed) I doubt this is a bug, but the joint frame could be re-aligned, since URDF and PyBullet have different conventions, but it shouldn't affect joint angles or world-space coordinates.

[erwincoumans]

Another thing that may help, read and write the URDF file using the PyBullet UrdfEditor, it may re-align the joint frame as it us used internally:

Run this script (replace capsule.urdf with your robot)

from pybullet_utils import bullet_client as bc
from pybullet_utils import urdfEditor as ed
import pybullet
import pybullet_data
import time

p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())

kuka = p0.loadURDF("capsule.urdf")

ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(kuka, p0._client)

ed0.saveUrdf("capsule_out.urdf")

(this script requires a reasonably recent updated pip install pybullet)