add ode utility functions
build bodyarray for vehicle
store numGrousers
add some OMPL code