Add RBM methods to add shapes to bodies.
Add TSRBM pass-through methods for adding shapes
Fix calls to parseBodyID in RBM.
Add constructor args for geom-to-body transform

`RigidBodyGeometry` and its children now accept specification of the
geometry-to-body transform in their constructors. The transform can be
specified either with a 4x4 homogeneous transform or xyz, rpy values.
Calling the constructors without these arguments sets the transform to
identity (previous behavior).