Add more body pos and vel tests
Add body force getter/setter
Add body ang_vel getter/setter
Change order of methods
Add another test