Sequential quadratic programming for task plan optimization

We consider the problem of refining an abstract task plan into a motion trajectory. Task and motion planning is a hard problem that is essential to long-horizon mobile manipulation. Many approaches divide the problem into two steps: a search for a task plan and task plan refinement to find a feasible trajectory. We apply sequential quadratic programming to jointly optimize over the parameters in a task plan (e. g. , trajectories, grasps, put down locations). We provide two modifications that make our formulation more suitable to task and motion planning. We show how to use movement primitives to reuse previous solutions (and so save optimization effort) without trapping the algorithm in a poor basin of attraction. We also derive an early convergence criterion that lets us quickly detect unsatisfiable constraints so we can re-initialize their variables. We present experiments in a navigation amongst movable objects domain and show substantial improvement in cost over a backtracking refinement algorithm.

Authors

• Dylan Hadfield-Menell
• Christopher Lin
• Rohan Chitnis
• Stuart Russell 0001
• Pieter Abbeel

Keywords

• Planning
• Robots
• Trajectory optimization
• Quadratic programming
• Collision avoidance
• Task Planning
• Sequential Quadratic Programming
• Path Planning
• Object Motion
• Joint Optimization
• Previous Solution
• Basin Of Attraction
• Explanatory Variables
• Action Plan
• Intermediate State
• Parametrized
• Local Optimum
• Bounding Box
• Experimental Variables
• Configuration Space
• Sequence Of States
• State Trajectories
• Robot Pose
• Trust Region
• Object Domain
• Norm Minimization
• Collision Detection
• Continuous Parameters
• Problem In Robotics