Remove some functions in BodyNode
Merge branch 'refactoring' of github.com:dartsim/dart into refactoring