Improved Task and Motion Planning for Rearrangement Problems using Optimal Control

• Main Authors: Hellander, Anja, Bergman, Kristoffer, Axehill, Daniel
• Format: Conference Proceeding
• Language: English
• Subjects: Collision avoidance; Cost function; Costs; Intelligent vehicles; Optimal control; Planning; Task analysis
• ISSN: 2642-7214
• DOI: 10.1109/IV55156.2024.10588789

Optimal task and motion planning (TAMP) has seen an increase in interest in recent years. In this paper we propose methods for using numerical optimal control to improve upon a feasible solution to a TAMP rearrangement problem. The methods are extensions of existing improvement methods for pure motion planning. The first method poses an optimal control problem (OCP) to simultaneously improve all motions in the plan. The second method, which we denote multiple finite horizons (MFH), takes inspiration from finite horizon control and poses a sequence of finite horizon OCPs involving variables for the positions of temporary placements of movable objects as well as motions in the plan, such that after solving each problem a feasible plan is maintained and the plan cost is non-increasing after each step. The methods are evaluated on a TAMP problem for tractor-trailers in numerical experiments, and the results show that both methods improve the plan for the evaluated problems. The results also show that MFH can reduce the computation time compared to the first method, and that on one example problem it achieves plans of similar or better quality as when all the motions are optimized at the same time provided that the horizon length is sufficiently long.