Possible Bug With Quaternions in CartToJnt

[Brian Zhan]

Using CartToJnt seems to achieve proper x y z end effector poses, but the rotation of the end effector pose seems to be off. It is consistently [ 1 1.04607e-06 -1.58426e-07 -1.50972e-06 ] when I run the following code using the pr2_arm.launch file.

In more detail, I used the CartToJnt pass coded in the attachment to obtain joint configurations that achieve specified cartesian coordinates. I then passed these joint configurations into the JntToCart pass, which produced accurate end effector poses, but incorrect quaternion rotation poses for the final location of the robot hand, which is important as well. In the attached code, I passed in the translation and rotation quaternion specified in lines 273-274 into the cartesianToAngle pass to obtain joint coordinates found in line 267. These are then passed into angleToCartesian, which produces an end effector quaternion of [ 1 1.04607e-06 -1.58426e-07 -1.50972e-06] instead of what was originally given.

Help with this issue would be much appreciated. Thank you for your time in advance.

[Patrick Beeson]

CartToJnt() does not change the end effector data structure from what is provided by the calling program. You can see that in the header file int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist& bounds=KDL::Twist::Zero());

CartToJnt() takes in a desired end effector pose in the chain base link's frame of reference, and a joint seed (a guess at the answer or a nominal pose), and returns the resulting joint configuration that achieves that pose. That is how Inverse Kinematics works. If you know the joint configuration and you want to know the Cartesian pose for those joint, that is Forward Kinematics (which is just trigonometry) and is what KDL's JntToCart() does.

Your program that shows the end_effector_pose rotation at 1,0,0,0 is simply printing out the rotation derived from JntToCart() Forward Kinematics from your input joints. This has nothing to do with CartToJnt().

[Brian Zhan]

Got it, thank you!