PyKDL wrong GetQuaternion output?

asked 2017-03-22 10:28:59 -0500

m4nh gravatar image

Hi, i'm not sure if it's correct to ask something about PyKDL here but this is a bug produced by a ros node. The question is very simple, i have a PyKDL.Frame and i notice that its conversion to translation/quaternion (before send it to TF Broadcaster) si malformed. The following two rows:

print frame, "\n"
print frame.M.GetQuaternion()

produce this result:

[[ 3.21075e-06, 3.21074e-06,   -0.999999;
 -3.21074e-06,   -0.999999,-3.21075e-06;
    -0.999999, 3.21075e-06,-3.21074e-06]
[    0.632574,    0.982814,     3.08063]] 

(0.0039903364268270795, 0.0, -0.003990323614818683, 0.00040231585169223024)

The last row is the quaternion but is wrong since the previous Rotation component seems to be a canonical base (0.99999999 should be equal to 1, and xxe-06 should be 0).

In RVIZ the corresponding TF is slightly titled, that is why i realized the presence of something wrong. Thanks

edit retag flag offensive close merge delete