Initial work on kinodynamicFMT