updated to use new RBT+RigidBody API's
updated to use new RBT+RigidBody API's

sheim/drake,billhoffman/drake,sheim/drake,sheim/drake,billhoffman/drake,billhoffman/drake,billhoffman/drake,billhoffman/drake,sheim/drake,billhoffman/drake,billhoffman/drake,sheim/drake,sheim/drake,sheim/drake,sheim/drake,billhoffman/drake