Introducing Multi-Convexity in Path Constrained Trajectory Optimization for Mobile Manipulators

Mobile manipulators have a highly non-linear and non-convex mapping between the end-effector path and the manipulator's joints and position and orientation of the mobile base. As a result, trajectory optimization with end-effector path constraints takes the form of a difficult non-linear optimization problem. In this paper, we present the first multi-convex approximation to this difficult optimization problem that eventually reduces to solving a sequence of globally valid convex quadratic programs (QPs). The proposed optimizer rests on two novel building blocks. First, we introduce a set of auxiliary variables in which the non-linear constraints that arise out of manipulator kinematics and its coupling with the mobile base have a multi-affine form. Projecting the auxiliary variables to the space of actual configuration variables of the mobile manipulator involves a non-convex optimization. Thus, the second building block involves computing a convex surrogate for this non-convex projection. We show how large parts of the proposed optimizer can be solved in parallel providing the possibility of exploiting multi-core CPUs. We validate our trajectory optimization on different benchmark examples. Specifically, we highlight how it solves the cyclicity problem and provides a holistic approach where a diverse set of trajectories can be obtained by trading-off different aspects of manipulator and mobile base motion.