Revision history [back]

Hi,

It seems to be a bit late but if anyone else need this. I followed your method and after getting bad results too, I changed two things.

The first, which I think cause your problem is the condition !segmnt.getJoint().getName().compare(initJoints->name[i]), which I transform as segmnt.getJoint().getName().compare(initJoints->name[i] == 0), because String1.compare(String2) does not return a bool.

The second, which is not important for good results (but I'm telling it to understand my code below), is the order of your loops. If your joint_state message, like me, is not necessarily ordered like the chain and could include other joints, it is more "optimized" i think to invert these two loops "for" and break when you find the match. Anyway, this doesn't really matter and this is my code which seems to work :

    urdf::Model model;
if (!model.initParam("robot_description"))
    return -1;

KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(model, tree)) {
    ROS_ERROR("Failed to extract kdl tree from xml robot description");
    return -1;
}

const sensor_msgs::JointStateConstPtr initJoints = ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states", n);

KDL::Chain chain;
tree.getChain("base_link", "xtion_wrist_link", chain);

KDL::JntArray joint_pos= KDL::JntArray(chain.getNrOfJoints());

int k = 0;
for(int i = 0; i < chain.getNrOfSegments(); i++)
{   
    KDL::Segment segment = chain.getSegment(i);
    for (int j = 0; j < initJoints->name.size(); j++)
    {
        if (segment.getJoint().getName().compare(initJoints->name[j]) == 0)
        {
            if(segment.getJoint().getType() != KDL::Joint::None)
            {
                joint_pos(k) = initJoints->position[j];
                k++;
                break;
            }
        }
    }
}

KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain);

KDL::Frame frame;    
bool kinematics_status;

kinematics_status = fksolver.JntToCart(joint_pos, frame);
if (kinematics_status >= 0)
{
    cout << "fk ->    x : " << frame.p.x() << " y : " << frame.p.y() << " z : " << frame.p.z() << endl;
}
else
    cout << "JntToCart did not work" << kinematics_status << endl;

code output : fk -> x : 0.426696 y : -0.426238 z : 0.887562

rrun tf tf echo output : At time 122.154 - Translation: [0.427, -0.426, 0.888]