From URDF to forward kinematics and modifying the urdf
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?