Position and velocity frames for RBMs

Adds the ability to have nested multi-coordinate frames, and sets up the state frame of a rigid body manipulator to be composed of the state frames of all of the robots inside that manipulator, with each robot state frame made up of a position frame and velocity frame.
This will only return the id of the first sub-frame that matches. Would it be better to return a logical array, in case the caller is wants to know that multiple sub-frames match?