Inner-Approximation of Manipulable and Reachable Regions using Bilinear Matrix Inequalities

Abstract

Given an articulated robot arm, we present a method to identify two regions with non-empty interiors. The first region is a subset of the configuration space where every point in the region is manipulable. The second region is a subset of the workspace where every point in the region is reachable by the end-effector. Our method expresses the kinematic state of the robot arm using the maximal coordinates, so that the kinematic constraints take polynomial forms. We then reformulate the optimization-based inverse kinematics (IK) algorithm as gradient flows. Finally, we use sum-of-squares (SOS) programming to certify the convergence of each gradient flow. Our main result shows that the feasibility of an SOS programming problem is a sufficient condition for the manipulability and reachability of the sublevel sets of polynomial functions. Our method can be used to certify manipulable or reachable regions by solving a set of linear matrix inequalities (LMIs) or to maximize the volume of a region by solving a set of bilinear matrix inequalities (BMIs). These identified regions can then be used in various motion planning problems as hard safety constraints.