Fix off-by-one error on body indices.

This was causing the last link in the chain to be ignored in
collisionDetection.
Fix output method for TSRBM with multiple robots.

Use the transformation we already have from TSRBM state frame to RBM state
frame to extract the manipulator state. We'll need to add transforms from TSRBM
state to the sensor states as well.