ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

From URDF to forward kinematics and modifying the urdf

asked 2017-02-02 08:42:25 -0500

Beaucay gravatar image

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-02-03 02:39:08 -0500

Beaucay gravatar image

updated 2017-02-03 02:40:35 -0500

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.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2017-02-02 08:42:25 -0500

Seen: 861 times

Last updated: Feb 03 '17