Add inertiaPlanningTest
update