Package for whole body sampling based planning
rosrun robot_state_publisher robot_state_publisher
./ompl_test _is_model_floating_base:=false