start to work on a planning for CoM trajectory and contact force only