Ask Your Question
0

pr2controller tutorial, quaternion not the same for same program in python and cpp

asked 2011-08-08 07:26:19 -0500

vincent gravatar image

updated 2014-01-28 17:10:10 -0500

ngrennan gravatar image

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.

edit retag flag offensive close merge delete

Comments

To mark a question as "Closed", please make sure that the correct answer is marked as "accepted". That is the green checkmark next to it (this question already has an accepted answer). There is no need to add a [Closed] to the title.
Eric Perko gravatar imageEric Perko ( 2011-08-08 10:48:55 -0500 )edit
Also keep in mind that "Closed" is used when admins/mods mark a question as a duplicate, offtopic, offensive, etc and technically doesn't mean the same thing as "Answered".
Eric Perko gravatar imageEric Perko ( 2011-08-08 10:49:45 -0500 )edit
thanks for the info. And thank tfoote for editing.
vincent gravatar imagevincent ( 2011-08-08 13:07:25 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2011-08-08 10:29:35 -0500

dornhege gravatar image

updated 2011-08-08 10:30:32 -0500

What is definitely wrong is as you suspected the use of getAxis.

I think the python code puts out the raw quaternion.

In c++ you use getAxis(). First, to get the rotation, you should use getAngle() (you output the non-existent w component of the axis, that is zero). This will print out the rotation axis and angle correctly.

However, a quaternion represents axis and angle, but it is not exactly xyz=axis and w=angle. Usually w is cos(angle/2) and the axis is scaled to give a unit quaternion. If you look at your output the python and c++ axes seem to be collinear, just differently scaled.

To get comparable outputs in c++ just use x(), y(),... to output the raw quaternion instead of getAxis.

edit flag offensive delete link more

Comments

exactly like what you said, getAxis() gives the unit axis vector. changed to x(),y(),z(),cos(0.5*getAngle()), worked! Thanks
vincent gravatar imagevincent ( 2011-08-08 10:43:32 -0500 )edit
There should also be w() directly.
dornhege gravatar imagedornhege ( 2011-08-08 10:54:02 -0500 )edit
haha, i made simple things complicated, thank you:)
vincent gravatar imagevincent ( 2011-08-08 11:01:52 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-08-08 07:26:19 -0500

Seen: 657 times

Last updated: Aug 08 '11