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