motion planning

This work extends my own contributions at the tail end of my masters thesis with some excellent novel ideas by Yanhao Yang at Oregon State University. We perform an optimization of body orientation frame for mobile systems, key for motion planning using models for dynamics like constraint curvature. However, we address the curse of dimensionality by performing optimization in the region surrounding the current motion plan, rather than for the entire configuration space.