monolithic planner simulation version to copy to the robot