Use structured bindings for `std::map` objects

[Woolfrey]

Instead of using something like:

std::map<std::string,RigidBody> rigidBodyList;
auto container = rigidBodyList.begin();
// container->first gives the string
// container->second gives the RigidBody object

it can be replaced with structured bindings for readability:

auto &[name, rigidBody] = rigidBodyList.begin;
// name is the string
// rigidBody is the object

Particularly useful in the KinematicTree class.

[Woolfrey]

Replaced everything in KinematicTree.h.