Rosanswers logo

In python I was able to read in a urdf calculate the end effector pose coming from joint angles and later manipulating the urdf by accessing the urdf.joints directly:

urdf = URDF.from_xml_file(PATH)
kdl_kin = KDLKinematics(urdf, urdf.links[0].name, urdf.links[-1].name)
pose = kdl_kin.forward(joints.position)

urdf.joints[0].origin.xyz = [1, 2, 3]

Now I would like to achieve the same in C++. I am currently trying to use the KDL library to do this:

KDL::Tree tree;

As I see it, I should extract the chain from the tree and then use the ChainFkSolverPos to get a pose. But for extracting the chain I need the names of segments, which I can not as easily access as I did in Python. How do you get the first and last sections' names? Or is my approach totally wrong?

Originally posted by Beaucay on ROS Answers with karma: 43 on 2017-02-02

Post score: 1


1 Answer 1


Rosanswers logo

I found a solution, but I doubt it's the right one. It looks more like a workaround to me. Anyway:

    KDL::SegmentMap::const_iterator current_segment = tree.getRootSegment();
    std::vector<KDL::SegmentMap::const_iterator> children_vector = current_segment->second.children;

        current_segment = current_segment->second.children[0];
        children_vector = current_segment->second.children;

This means, I build up the Chain elementwise instead of using the getChain-function from tree.

Originally posted by Beaucay with karma: 43 on 2017-02-03

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.