Optimizing Non-Convex Objectives to Plan More Optimal Motion for Manipulators

Abstract

Non-convex optimization is essential to tackle increasingly complex and practical problems in kinematic motion planning. Although introducing non-convexity often sacrifices guarantees of feasibility and optimality–making solutions more susceptible to local minima or failure to converge–many robotic systems and tasks are non-convex by nature, necessitating at least somewhat non-convex formulations. In this thesis, we aim to mostly constrain non-convexity to the objective. This optimization structure helps preserve certain feasibility guarantees in theory and usability in practice while enhancing optimality of solutions, even if global optimality is not achieved. In the first chapter, we demonstrate the effectiveness of non-convex objectives in scenarios where motion planning involves a non-convex parameterization of the configuration space. We keep constraints strictly convex, with the non-convexity quarantined to the objective. This structure guarantees a feasible solution given a feasible initial guess. We primarily use our method to post-process Graphs of Convex Sets solutions in three domains: constrained bimanual motion, motion with guaranteed non-collision, and planning in SO(3). In each case, the non-convex objective compensates for distortion introduced by the parameterization, resulting in more efficient and natural motion. In the second chapter, we propose teleoperation scheme with full-body motion planning for non-holonomic mobile manipulators. Our key contribution is a Differential Inverse Kinematics (DiffIK) formulation that crafts non-convex objectives to avoid singularities and joint limits leading to more robust feasible motion. Unlike before, the constraints are not strictly convex, so the optimization has no guarantees of feasibility. However, we mitigate the non-convexity in the constraints as much as we can by linearizing around the robot’s current position and approximating the highly non-convex non-holonomic constraint. We explore multiple formulations for singularity avoidance and empirically demonstrate that integrating these objectives into DiffIK improves motion quality for teleoperation for the RBY-1 robot.