kinematics_model doesn't load end effector info properly from srdf

[jonbinney]

In planning_models::KinematicModel::buildGroupInfo, there's some logic to figure out the parent groups of each end effector group. It basically says that if the parent link of the end effector group belongs to exactly 1 group, then that is the parent group of that end effector group. It then sets attached_end_effector_group_name_ in the parent group, and end_effector_parent_ in the end effector group.

The problem is that the parent link of the pr2's right gripper, for instance, actually belongs to 3 groups: "right_arm", "arms", and "whole_body". So the function doesn't know what the parent group should be, and doesn't set anything.

What is the right way to correct this?

[isucan]

Not sure what the correct way is, but I put some code that selects the smallest group (in terms of number of joints) as the parent. This sounds like it would do something reasonable for the cases I thought of. We might have to revisit this at some point.