The propulsion of C. elegans on agar substrate has been investigated and analysed with regard to the kinematics of the worm [1] and the substrate material properties [2]. However, the modelling of the substrate-body mechanics and actual movement that allows observed worm displacements remains unexplored. In order to elucidate the mechanisms that allow the locomotion of small worm-like bodies, we here investigate the solution of optimal solution for the locomotion of a slender three-dimensional body on a frictional substrate, subjected to an optimal contraction field distribution. We integrate the optimality conditions by resorting to an iterative Forward-Backward Sweep Method, which requires the solution of a forward dynamical problem, a linear ODE (adjoint equations), and the update of the control equations [3]. We investigate the convergence of the method in linear simple problems [4], and apply to simulate the locmotion of a three-dimensional body on a flat substrate, with unequal normal and tangential friction. [1] G. J. Stephens, B. J. Kerner, W. Bialek, and W. S. Rhu. Dimensionality and Dynamics in the Behavior of C. elegans. PLoS Comp. Biol., 4:e1000028, 2008. [2] B.C. Petzold, S.-J. Park, B. L. Pruitt. Caenorhabditis elegans body mechanics are regulated by body wall muscle tone. Biophys. J. 100:1977–1985, 2011. [3] A. Bijalwan, J.J. Mu˜noz. A control Hamiltonian preserving discretisation for optimal control. Mulitbody System Dynamics. Accepted. [4] A. Bijalwan, J.J,. Mu˜noz. On the numerical stability of discretised Optimal Control Problems. IUTAM Bookseries. Accepted.