Optimal control for geometric motion planning of a robot diver | IEEE Conference Publication | IEEE Xplore