added dependency on full_body_controller.
EnvInterfaces now sends a controller message to execute trajectory on robot
added BFS3D with rotation heuristic.