1: start/end points ,
2: compare with to check whether it needs to go
 backwards, if yes, go backwards and restart from step 1
3: solve TPBVP for input levels to get , and
4:
5:  case 1: apply input levels with durations
6:
7:  case 2: solve TPBVP for input levels
   and apply them with obtained durations
8: else (i.e., )
9:  case 3: solve TPBVP for input levels and apply
   them with obtained durations
10:
Algorithm 1: Motion planning algorithm. Goal: To generate a sequence of control signals for the propulsive force.