An Optimization-based Approach to Dual-Arm Motion Planning with Closed Kinematics

This paper addresses the optimization-based planning of collision-free motions for a dual-arm robot with kinematic constraints. Such problems arise, for example, when the robot has to move an object with both arms, whereby the two arms and the gripped object form a closed kinematic chain. Such constrained problems are hard to solve with sampling-based planners, because the probability that a random sample satisfies the closure constraint is practically zero. In contrast, the solution of optimization problems with equality constraints is a well-understood field of research. This paper formulates the motion planning task as optimization problem and proposes a numerical solution using the augmented Lagrangian method for handling constraints. The planner is compared to RRTs, CHOMP and TrajOpt on a set of randomly generated problems for a dual-arm robot with twelve degrees of freedom highlighting the advantages of optimization-based planning.