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.