An SDP Optimization Formulation for the Inverse Kinematics Problem

Abstract

Inverse kinematics (IK) is an important problem in robot control and motion planning; however, the nonlinearity of the map from joint angles to robot configurations makes the problem nonconvex. In this paper, we propose an inverse kinematics solver that works in the space of rotation matrices of the link reference frames rather than joint angles. To overcome the nonlinearity of the manifold of rotation matrices SO(3), we propose a semidefinite programming (SDP) relaxation of the kinematic constraints followed by a fixed-trace rank minimization via maximization of a convex function. Along the way, we show that the feasible set of an IK problem is exactly the intersection of a convex set and fixed-trace rank-1 matrices. Thanks to the use of matrices with fixed trace, our algorithm to obtain rank-1 solutions has guaranteed local convergence. Unlike some traditional solvers, our method does not require an initial guess, and can be applied to robots with closed kinematic chains without ad-hoc modifications such as splitting the kinematic chain. Compared to other work that performs SDP relaxation for IK problems, our formulation is simpler, and uses variables with smaller sizes. We validate our approach via simulations on a closed kinematic chain constituted by two robotic arms holding a box, comparing against a standard IK method.