Understanding Forward Kinematics (MoveIt + PR2 + Hydro)

[Carlos Maestre]

Dear all,

I really do not know how to use Forward Kinematics. I made a simple example to test it, but there is something I am doing wrong.

My PR2 is in a initial position when I load it. The default eef of the robot for the right arm is the "r_wrist_roll_joint". So I thought that using getCurrentPose() and Forward Kinematics should provide me the same X Y Z coordinates for the robot. 

Here the simple code using the MoveIt!'s API:

  moveit::planning_interface::MoveGroup group("right_arm"); 

  ROS_INFO_STREAM("Initial pose " << group.getCurrentPose());

Output: 

[ INFO] [1405006035.569845583, 1378.744000000]: Initial pose header: 

  seq: 0

  stamp: 1378.744000000

  frame_id: /odom_combined

pose: 

  position: 

    x: -0.192969

    y: -0.791602

    z: 0.40146

  orientation: 

    x: 0.0487059

    y: 0.783749

    z: -0.243029

    w: 0.569476

Now trying to get the point pose using Forward Kinematics, that should get the value of the joints and calculate some coordinates:

  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");

  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

    const Eigen::Affine3d &end_effector_state = kinematic_state->getJointTransform("r_wrist_roll_joint");

  /* Print end-effector pose. Remember that this is in the model frame */

  ROS_INFO_STREAM("Translation: " << end_effector_state.translation());

Output: 

[ INFO] [1405006040.851465696, 1380.346000000]: Translation: 0

0

0

It would be great if any of you could explain me what I am doing wrong. Thanks,

Carlos

[Dave Hershberger]

Clearly RobotState::getJointTransform() needs some documentation, sorry.

getJointTransform() returns the transform representing just the angle or value of the given joint.  So if the joint is at angle 0, the transform will be identity.

What you want is RobotState::getGlobalLinkTransform().  That returns the pose of the link relative to the planning frame (RobotModel::getModelFrame()).

Dave

[Carlos Maestre]

Thanks a lot Dave for the answer. It was more or less what I had in mind.

The unique problem I found is that then I am getting the pose of a link, not a joint. And in some robots, e.g. the PR2, the default eef is a joint, i.e. the "r_wrist_roll_joint". So I guess I must change the eef for the robot to a link. Am I right?

Carlos

[Dave Hershberger]

When I look at pr2_moveit_config/config/pr2.srdf, I see these eefs:

   <end_effector name="right_eef" parent_link="r_wrist_roll_link" group="right_gripper" />
   <end_effector name="left_eef" parent_link="l_wrist_roll_link" group="left_gripper" />

The parent_link entries are definitely links, not joints.

The other piece of information that may not be clear is that the origin of every link is closely related to the pose of the parent joint.  A joint defines a variable offset either in rotation or translation (typically).  The "child" side of the joint is the origin of the child link.  The "parent" side of the joint is defined to be some fixed pose offset from the parent link.  So if you think of the "child" side of a joint as the "pose of the joint" then that pose is always the same as the pose of the child link.  However, probably because of the variable nature of joints, we don't often talk about the "pose of a joint" in moveit.  We just talk about the poses of links.

This is my impression, anyway. :)

Dave

[Carlos Maestre]

It makes sense. Cool answer Dave ;-)

Carlos