This is a numerical solver for kinematic planar rigid body robot in the plane. For any specified error tolerance, this solver is guaranteed to find an approximately optimal trajectory.
The detail of this solver can be found in our paper: Yu-Han Lyu, Devin Balkcom, "Optimal Trajectories for Kinematic Planar Rigid Bodies with Switching Costs," IJRR