9. For k←1 to (Length-1)*2 do

10. if k == Length and Reverse == False then

11. $L_{j}^{P_{i}}$ ← $L_{j}^{P_{i}}$ .Reverse()

12. Reverse←True

13. end

14. $a_{g o a l}$ ← $L_{j}^{P_{i}}$ (end)// $L_{j}^{P_{i}}$ (end)代表第i个任务场景的第j条可行路径的终点

15. if Reverse==False then

16. $a_{i n i t}$ ← $L_{j}^{P_{i}}$ (k)// $L_{j}^{P_{i}}$ (k)代表第i个任务场景的第j条可行路径的k时刻节点

17. $a_{n e x t}$ ← $L_{j}^{P_{i}}$ (k + 1)

18. else

19. $a_{i n i t}$ ← $L_{j}^{P_{i}}$ (k- Length + 1)

20. $a_{n e x t}$ ← $L_{j}^{P_{i}}$ (k + 2- Length)

21. end

22. $f_{t a s k}$ ←Contact ( $h_{i}$ , $a_{i n i t}$ , $a_{g o a l}$ )

23..Append (( $f_{t a s k}$ , $a_{n e x t}$ ))

24. end

2 25. return