KinematicTree needs to keep track of links about exotica

[VladimirIvan]

All segments/links are stored in the KinematicTree, in Tree as a vector and in TreeMap as a map. Their transforms in the world frame (after updating) can be accessed through there as well, e.g.:

for(std::shared_ptr<KinematicElement> element : Tree)
{
  KDL::Frame link = element->Frame;
  ...
}

Do you need any other interface for this?

[wxmerkt]

Not needed anymore, exposed through Scene. A convenience function that retrieves the parent joint of a link may be necessary in the future.