Finished changes to RigidBodySimulator

-robot to super-robot
-methods take int i, the index of sub-robot in super-robot structure