KinematicTree: getter for joints

[christian-rauch]

This adds getter for the joint elements of the KinematicTree.

At the moment, the KinematicElements returned by GetControlledJointsMap have empty parent_name and the segment.getName() returns the joint name instead of the link name, i.e. segment.getName() and segment.getJoint().getName() both return the same joint name.

From http://docs.ros.org/jade/api/orocos_kdl/html/classKDL_1_1Segment.html:

This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body inertia) with a joint and with "handles", root and tip to connect to other segments.

I would assume that the KDL::Segment always refers to a frame/link and not a joint.

Is this intended?

Edit:
And closest_robot_link.lock() is NULL which also appears to be wrong.

[VladimirIvan]

Each KDL::Segment has to be associated with a joint. It can be a fixed joint with no transform but it has to have joint by convention. I don't remember how joint and segment names map onto the URDF ones. Our convention was following KDL.

Is there a mismatch between the joint/segment naming between KDL and exotica?

[christian-rauch]

Yes.

In plain KDL:

kdl_parser::treeFromUrdfModel(*urdf_model, robot_tree);
for(const std::pair<std::string, KDL::TreeElement>& segm : robot_tree.getSegments()) {
    const std::string segm_name = segm.second.segment.getName();
    const std::string jnt_name = segm.second.segment.getJoint().getName();
    std::cout << segm_name << " -> " << jnt_name << std::endl;
}

gives the link name for the segment and its attached joint name (here examplarily for a Kinova Jaco):

j2n6s300_end_effector -> j2n6s300_joint_end_effector
j2n6s300_link_1 -> j2n6s300_joint_1
j2n6s300_link_2 -> j2n6s300_joint_2
j2n6s300_link_3 -> j2n6s300_joint_3
j2n6s300_link_4 -> j2n6s300_joint_4
j2n6s300_link_5 -> j2n6s300_joint_5
j2n6s300_link_6 -> j2n6s300_joint_6
j2n6s300_link_base -> j2n6s300_joint_base
j2n6s300_link_finger_1 -> j2n6s300_joint_finger_1
j2n6s300_link_finger_2 -> j2n6s300_joint_finger_2
j2n6s300_link_finger_3 -> j2n6s300_joint_finger_3
j2n6s300_link_finger_tip_1 -> j2n6s300_joint_finger_tip_1
j2n6s300_link_finger_tip_2 -> j2n6s300_joint_finger_tip_2
j2n6s300_link_finger_tip_3 -> j2n6s300_joint_finger_tip_3

In EXOTica, the KinematicTree::controlled_joints_map_ contains segments whose name is the joint name. E.g. it would print:

j2n6s300_joint_1 -> j2n6s300_joint_1
j2n6s300_joint_2 -> j2n6s300_joint_2
j2n6s300_joint_3 -> j2n6s300_joint_3
j2n6s300_joint_4 -> j2n6s300_joint_4
...

The model_tree_ contains the correct names.

[VladimirIvan]

Good spot.
Can you add the fix for the segment names and check if it breaks anything?

For the closest_robot_link, this is used only for scene elements to track when they get attached to the robot (e.g., in pick and place). It is then used in the collision scene to filter out self-collisions. After attaching an element to a robot link closest_robot_link is set. Robot links have the is_robot_link flag set at creation instead. This flag doesn't get changed at any point.

[christian-rauch]

There seem to be two sets of KinematicElements. One set with the correct segment names (1) and one set with the wrong segment names (2). Elements from set 1 are referenced by tree_, model_tree_, tree_map_ and probably other members. Set 2 is at least referred by controlled_joints_map_.

Do we need different sets of KinematicElements or should all vectors, map, etc. point to elements of the same set?
I think there should be one vector/list that holds the superset of elements of the KinematicTree and all other model/controlled links/joints should define a subset of elements. I.e. controlled_joints_map_ should point to the same elements that collision_tree_map_ is pointing to.

[VladimirIvan]

There are two sets of elements so that we can keep track of model elements separately from the world elements. Every time you load a new scene, all world elements are destroyed and recreated while the robot elements are kept as they are. The robot model therefore doens't have to be reloaded. It's not the prettiest solution but it removes some complexity of tracking different parts of the scene. Changing this would be great but it would also probably break all the API and it would require a lot of work on code that a lot of our experiment code heavily depends. Especially collision checking, which is why we introduced a lot of this machinery in the first place.

[christian-rauch]

I have to revert my statement partially. Only the world frames are named after their joints. The actual robot frames are named correctly.

I got confused because EXOTica is using the virtual world joint name as link name, e.g.

<virtual_joint name="world_joint" type="floating" parent_frame="world" child_link="root" />

gives:

world_joint/trans_x -> world_joint/trans_x
world_joint/trans_y -> world_joint/trans_y
world_joint/trans_z -> world_joint/trans_z
world_joint/rot_x -> world_joint/rot_x
world_joint/rot_y -> world_joint/rot_y
world_joint/rot_z -> world_joint/rot_z
j2n6s300_joint_1 -> j2n6s300_link_1
j2n6s300_joint_2 -> j2n6s300_link_2
j2n6s300_joint_3 -> j2n6s300_link_3
...

i.e. the 6 frames for the 6D world pose are named after the joints instead of using the parent_frame like world/trans_x, world/rot_x, ... etc.

A naive fix like replacing root_joint->getName() by world_frame_name gives me a segfault somewhere in UpdateJointLimits.
It's a little bit confusing but it can be neglected. We can go ahead with this PR without fixing this.

[christian-rauch]

I included a fix that correctly names the world frames:

world_joint/trans_x -> world/trans_x
world_joint/trans_y -> world/trans_y
world_joint/trans_z -> world/trans_z
world_joint/rot_x -> world/rot_x
world_joint/rot_y -> world/rot_y
world_joint/rot_z -> world/rot_z

[wxmerkt]

I included a fix that correctly names the world frames:

Agree with the fix, I introduced the bug when I fixed the wide-spread floating-base issue (or non-existing support) about two years ago. It will break user code, though.
[Edit: I updated my code in anticipation of this breaking change, i.e., non-blocking]