improved simulation

+ Lidar model
+ Robot Model Inertia Fixes
add ODE enviromental objects