Adding partial implementation of trajectory optimization tools.