[ros-users] kdl_parser error?

[Kristof Berx]

Hello all,

We are a student team working on the pr2-robot and want to estimate the forces on the right gripper tool through the efforts sent out on the joint-states topic.

We are trying to construct a kdl::chain to use for the calculation of a jacobian.

But when we print out the names of all the joints in our chain, some of the joint names seem wrong since they aren't mentioned on the joint states topic.

This is the code we use to construct the chain and print out it's segments and joint:

    Tree modeltree;
    std::string robot_desc_string;
    n->param("robot_description", robot_desc_string, string());
    if (!kdl_parser::treeFromString(robot_desc_string, modeltree))
    {
      ROS_ERROR("Failed to construct kdl tree");
      return false;
    }

    cout << "file parsed" << endl;

    const string root_name = "torso_lift_link";
    const string tip_name  = "r_gripper_tool_frame";
    if (!modeltree.getChain(root_name,tip_name,chain_))
    {
        ROS_ERROR("could not find a chain from '%s' to '%s'",root_name.c_str(), tip_name.c_str());
        return false;
    }

    cout << "Chain built" << endl;

    for(int i=0;i<chain_.getNrOfJoints();++i)
    {
        cout<<"Segment "<<i<<": "<<chain_.getSegment(i).getName()<<" Joint "<<i<<": "<<chain_.getSegment(i).getJoint().getName()<<endl;
    }

and we receive the following printout:

file parsed
Chain built
Segment 0: r_shoulder_pan_link       Joint 0: r_shoulder_pan_joint
Segment 1: r_shoulder_lift_link          Joint 1: r_shoulder_lift_joint
Segment 2: r_upper_arm_roll_link     Joint 2: r_upper_arm_roll_joint
Segment 3: r_upper_arm_link           Joint 3: r_upper_arm_joint
Segment 4: r_elbow_flex_link            Joint 4: r_elbow_flex_joint
Segment 5: r_forearm_roll_link          Joint 5: r_forearm_roll_joint
Segment 6: r_forearm_link                Joint 6: r_forearm_joint

The r_upper_arm_joint and r_forearm_joint are not mentioned in the joint_states topic, so we are unable to determine which effort to assign to these joints.

The right-arm joints (not including the fingers) mentioned in the joint_states are the following:
'r_upper_arm_roll_joint'
'r_shoulder_pan_joint'
'r_shoulder_lift_joint'
'r_forearm_roll_joint'
'r_elbow_flex_joint'
'r_wrist_flex_joint'
'r_wrist_roll_joint'
'r_gripper_joint'

Why don't these names match?
Did we do something wrong in constructing the tree or extracting the chain?
How do we now know which effort to assign to which joint in our chain?

Greetz,
Kristof & Nico