pr2controller tutorial, quaternion not the same for same program in python and cpp
I just went through pr2_controllers/Tutorials/Moving the arm through a Cartesian pose trajectory.
I tried to rewrite the client program from python to cpp. The program worked but got different quaternion vector(x,y,z,w) with the result from the original program.
The original code snippet from the tutorial:
#pretty-print list to string
def pplist(list):
return ' '.join(['%2.3f'%x for x in list])
#print out the positions, velocities, and efforts of the right arm joints
if __name__ == "__main__":
rospy.init_node("test_cartesian_ik_trajectory_executer")
tf_listener = tf.TransformListener()
time.sleep(.5) #give the transform listener time to get some frames
# not needed, fix tutorial
joint_names = ["r_shoulder_pan_joint",
"r_shoulder_lift_joint",
"r_upper_arm_roll_joint",
"r_elbow_flex_joint",
"r_forearm_roll_joint",
"r_wrist_flex_joint",
"r_wrist_roll_joint"]
positions = [[.76, -.19, .83], [0.59, -0.36, 0.93]]
orientations = [[.02, -.09, 0.0, 1.0], [0.65, -0.21, .38, .62]]
success = call_execute_cartesian_ik_trajectory("/base_link", \
positions, orientations)
#check the final pose
(trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))
print "end Cartesian pose: trans", pplist(trans), "rot", pplist(rot)
The result pose is
end Cartesian pose: trans 0.590 -0.360 0.930 rot 0.651 -0.211 0.381 0.621
which agrees with the result by doing rosrun tf tf_echo base_link r_wrist_roll_link
But the result of my program is
end Cartesian pose: trans,0.59,-0.36,0.93,rol,0.83,-0.27,0.49,0.00
Here is the code snippet:
tf::TransformListener listener;
ros::NodeHandle node;
ROS_INFO("Now calling the client function...");
success = call_execute_cartesian_ik_trajectory(node, "base_link", position, orientations);
ROS_INFO("Finished calling");
tf::StampedTransform transform;
try{
listener.lookupTransform("base_link", "r_wrist_roll_link",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
ROS_INFO("end Cartesian pose: trans,%3.2f,%3.2f,%3.2f,rol,%3.2f,%3.2f,%3.2f,%3.2f \n ", transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z(),transform.getRotation().getAxis().x(),transform.getRotation().getAxis().y(),transform.getRotation().getAxis().z(),transform.getRotation().getAxis().w());
I guess the error lies in misuse of the function transform.getRotation().getAxis().x()
Can someone point out what went wrong? Thanks.