0
$\begingroup$

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;
kdl_parser::treeFromFile(PATH);

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

$\endgroup$

1 Answer 1

0
$\begingroup$

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();
    chain.addSegment(current_segment->second.segment);
    std::vector<KDL::SegmentMap::const_iterator> children_vector = current_segment->second.children;

    while(children_vector.size()>0){
        current_segment = current_segment->second.children[0];
        chain.addSegment(current_segment->second.segment);
        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

$\endgroup$

Your Answer

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