{'fraction_non_alphanumeric': 0.03914817171301321, 'fraction_numerical': 0.014334368801227591, 'mean_word_length': 4.459746628524724, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 5, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Global inverse kinematics via mixed-integer convex optimization

In this paper, we present a novel formulation of the inverse kinematics (IK) problem with generic constraints as a mixed-integer convex optimization program. The proposed approach can solve the IK problem globally with generic task space constraints: a major improvement over existing approaches, which either solve the problem in only a local neighborhood of the user initial guess through nonlinear non-convex optimization, or address only a limited set of kinematics constraints. Specifically, we propose a mixed-integer convex relaxation of non-convex SO ( 3 ) rotation constraints, and apply this relaxation on the IK problem. Our formulation can detect if an instance of the IK problem is globally infeasible, or produce an approximate solution when it is feasible. We show results on a seven-joint arm grasping objects in a cluttered environment, an 18-degree-of-freedom quadruped standing on stepping stones, and a parallel Stewart platform. Moreover, we show that our approach can find a collision free path for a gripper in a cluttered environment, or certify such a path does not exist. We also compare our approach against the analytical approach for a six-joint manipulator. The open-source code is available at http://drake.mit.edu .


Introduction
The inverse kinematics (IK) problem is one of the most fundamental problems in robotics. The problem is to find robot postures, so as to satisfy certain kinematic constraints. Traditionally the kinematic constraints are on the end-effectors, such as hand reaching a certain location with a given orientation. More recently applications have required more complex constraints in the task space, such as grasping a mug on the table while keeping the robot free from collision (Fig 1a).
Solving the IK problem is quite challenging, as it requires solving a set of nonlinear equations involving products of trigonometric functions (sin/cos). There have been tremendous efforts to solve the inverse kinematics problem [11,21,24,5], and they can be coarsely categorized as analytical or numerical. The analytical approach solves the kinematics equations as polynomials of sine and cosine and can produce closed-form solutions. It is widely known that many manipulators with 6 Degree of Freedoms (DoFs) allow analytical solution for the end effector to reach a specified position with a given orientation [26,20]. More generally, Diankov et al. introduced IKfast, which can find inverse kinematics solutions for more complicated robots, but the set of kinematics constraints it can handle is still limited [16]. Since the analytical approach solves the inverse kinematics problem by computing roots of polynomial equations, it does not permit inequality constraints on the link poses [2], which occur frequently when planning postures for certain tasks. This shortcoming makes it hard to handle general task space constraints involving inequalities, such as "putting the hand between the two boxes on the table". On the other hand, the numerical approach can solve inverse kinematics problems for complicated robots with generic constraints. This approach formulates an inverse kinematics problem as a general non-convex nonlinear optimization problem, and calls gradient-based nonlinear solvers to handle these non-convex constraints [25,13,17,4,9]. The drawback of this approach is that the solution heavily relies on the user-supplied initial seed. Because the nonlinear optimization typically solves the problem locally around the initial seed, hence it can get trapped in some locally infeasible region and never reach the solution far away. As a result, when these non-convex nonlinear solver reports the problem is infeasible, it only asserts local infeasibility, providing no guarantee on the global solution [6]. We will show that our approach can significantly improve the robustness and success rate of the IK solver, over the gradient-based nonlinear optimization approach.
We shall use a numerical approach to the IK problem with generic constraints, while also obtain the global solution. Instead of formulating the problem through non-convex nonlinear optimization, we instead consider a mixed-integer convex optimization formulation, which does not require an initial seed, and can provide a global solution [8], such as a certificate of global infeasibility. The non-convexity of the IK problem originates from the non-convixity of SO(3) constraints on rotation. If we choose to represent rotation with a matrix R = [u 1 u 2 u 3 ] ∈ R 3×3 , this rotation matrix should satisfy the following constraints where (i, j, k) = (1, 2, 3), (2, 3, 1) or (3, 1, 2) in (1c). We choose the rotation matrix as the orientation representation, since the position of a point attached to a body is a linear expression of the body rotation matrix; on the other hand, the position would be a nonlinear expression, if we were to represent orientation using other forms, such as unit quaternions or angle-axis.
Obviously the orientation constraint SO(3) (1a)-(1c) is non-convex. Take constraint (1a) as an example: geometrically, the convex combination of two points on the surface of a unit sphere, lies strictly in the interior of the unit sphere. Algebraically, an equality constraint is non-convex, if it includes quadratic terms such as u i (1) 2 in (1a), or bilinear terms like u i (1)u j (1) in (1b).
Various convex relaxations for SO(3) constraints have been proposed. Saunderson et al. proved that the convex hull of the set of rotation matrices can be described by a positive semidefinite constraint, a special type of convex constraint [27]. In [12] this convex relaxation is exploited to solve the IK problem through a sequence of convex optimizations. The drawback of this approach [12] is that the convex hull of the rotation matrix is a rather loose relaxation of SO(3). For example, the matrix u 1 , 0, 0 = 0.5 u 1 , u 2 , u 3 + 0.5 u 1 , −u 2 , −u 3 is in the convex hull for any rotation matrix u 1 , u 2 , u 3 . Hence quite often the relaxed convex program is too loose to detect global infeasibility of the original IK problem. To overcome this, we seek a tighter mixed-integer convex relaxation of the SO(3) constraint.
Our intuition is that instead of considering the convex hull of all rotation matrices as in [27], we divide the range of the rotation matrices into smaller intervals, and compute the convex hull of each small interval to obtain a tighter approximation. We can then constrain the approximated rotation matrix to be within one of the convex hulls. This approach is inspired by Deits et.al. [14], in which the orientation constraint SO(2) in 2D is approximated by mixed-integer linear constraints. In a similar fashion, we replace (1a)-(1c) with mixed-integer convex (quadratic) constraints, as a relaxation of the original non-convex SO(3) constraints. This relaxation allows the inverse kinematics problem to be formulated as a mixed-integer convex optimization program, which is amenable to the branch-and-bound algorithm by branching on integer variables, and can be solved efficiently by modern solvers [1]. Like convex optimization, mixed-integer convex optimization also does not rely on the initial seed, and warrants global solution [28,7]. With the rapid advancement in numerical solvers, mixed integer convex optimization becomes increasingly popular in robotics [30,22,15]. In this paper we show that our approach can either produce an approximate solution to the inverse kinematics problem, or prove the problem is globally infeasible for generic constraints.

Background
In this section we briefly review how to relax a non-convex quadratic constraint as mixed-integer convex constraints, which will be used to relax the non-convex SO(3) constraint on the rotation matrix (1a)-(1c).
Consider a non-convex quadratic constraint where the matrix Q is not necessarily positive semidefinite. This quadratic constraint contains bilinear product terms like xy, visualized in Fig 2a -2b. It is NP-hard to obtain an exact global optimal solution to this non-convex problem, but there has been a lot of research on obtaining an approximated one through mixed-integerconvex optimization [23,19]. The key idea is to partition the range of each variable into smaller intervals, and replace the bilinear term xy with a linear term which approximates xy inside each interval. To see this geometrically, we draw the surface of w = xy in Fig 2 within x ∈ [0, 1], y ∈ [0, 1]. We then partition the range [0, 1] into two intervals, with interval i as ] is a tetrahedron. We can then relax the constraint w = xy, with the constraint that the point (x, y, w) lies within one of the tetrahedrons. We will use some binary variables to determine within which tetrahedron the point lies.
Each convex hull is a tetrahedron drawn in distinct colors.
To enforce that (x, y, w) falls into one of the convex hulls, we rely on the special ordered set of type 2 (sos2) constraint [3], defined as follows Namely at most two entries in λ can be strictly positive, these two entries must be consecutive in their ordering.
The sos2 constraint is often used to formulate a piecewise linear approximation to a nonlinear function. For example in Fig 3, we approximate f (x) with a new variable w satisfying , λ is in sos2 (4) (4) forces the point (x, w) to be on the black lines in  We can formulate the sos2 (3) as mixed-integer linear constraints by introducing auxiliary binary variables. In this paper we adopt the formulation proposed in [31], which introduces log 2 n binary variables. We refer the readers to [31] for more details on sos2 constraint.
To summarize, for a non-convex quadratic constraint (2), we can replace the bilinear term xy with w satisfying (5), and obtain mixed-integer convex constraints as a relaxation of the original non-convex constraint.

Approach
As explained in Section 1, the non-convexity of the inverse kinematics problem originates from the non-convexity of the SO(3) constraint. In order to obtain a global solution to this non-convex problem, in sub-section 3.1, we first relax the SO(3) constraint (1a)-(1c), to a set of mixed-integer convex constraints, using the technique described in Section 2. Then in sub-section 3.2, we formulate the inverse kinematics problem by searching over the link poses (position/orientation) satisfying both the kinematic constraints, and the relaxed SO(3) constraint introduced in sub-section 3.1. Finally, we project the approximated rotation matrices to the SO(3) manifold to obtain the angle of each joint within the joint limits.

Rotation constraint relaxation
We aim to find a matrixR ∈ R 3×3 satisfying a relaxation of the SO(3) constraint (1a)-(1c). To this end, we cut the range [−1, 1] into n small intervals. The quadratic constraints (1b)-(1c) can be relaxed by replacing each bilinear term with a new variable approximating the bilinear term within each interval, described by (5).
To relax the unit length constraint u T Constraint (6) forces the point (u i ( j), w i ( j)) to be on the black lines in Fig 4, for n = 4 case. Namely w i ( j) is an upper-bound of u i ( j) 2 . We can thus relax the unit length constraint (1a) as Geometrically, constraint (7a) forces the rows/columns of the rotation matrix to be within the unit sphere, while constraint (7b) pushes the row/column vector from inside the unit sphere, such that the Euclidean length of the row/column vector is bounded from below.
While the generic technique introduced in section 2 can already approximate the orthogonality constraint, it is desirable to introduce an additional relaxation, because enforcing the solution to live in the intersection of different relaxations can only improve its approximation quality. We thus impose the following convex (quadratic) constraints as a relaxation to the orthorgonal constraint (1b) ) are connected with the black straight line. We introduce an auxiliary variable w i ( j) to be on the black lines, as an upper bound of the red curve lies within the shaded cyan region, as a relaxation of the red curve u i ( j) 2 .
where u i , u j are two distinctive rows/columns of the matrixR, | • | 2 is the l 2 norm of a vector. Constraint (8) When the vectors u i , u j are "close to" the unit sphere (due to the constraint (7a)-(7b)), constraint (8) approximates the orthogonal constraint u T i u j = 0. Similarly we can impose the convex constraint |u 1 ± u 2 ± u 3 | 2 2 ≤ 3 as an relaxation of the orthogonal constraint, where u 1 , u 2 , u 3 are all three rows/columns ofR.
The non-convex SO(3) constraint is thus relaxed to a set of mixed-integer convex quadratic constraints. These mixed-integer convex constraints are readily handled by modern numerical solvers such as Gurobi [1].

Inverse kinematics formulation
We aim to solve the inverse kinematics problem by finding the pose of each link. The pose of link i can be represented by rigidly attaching a frame to the link, with the position W p i ∈ R 3 and the rotation matrix W R i , where the right subscript i denotes it is link i's frame we are interested in, the left superscript W indicates the quantity is expressed in the world frame. In Fig 5, we illustrate the kinematics relationship between two links connected by a revolute joint. We will express this kinematics relation as constraints on the position W p i and orientation W R i of link i relative to and expressed in the world frame, and the position/orientation of its parent link i−1. A joint frame J is rigidly attached to the parent link i − 1, and coincides with the child link frame, when the joint angle q i = 0. We denote the fixed translation of joint frame to the parent link frame as i−1 p J . The joint axis is expressed as i−1ẑ i in the parent link (i − 1)'s frame, and iẑ i in the child link i's frame. Since the position and direction of joint axis is invariant under rotation about this axis, we can impose the following constraints on the poses of the parent and child links We also handle the angle limits on the revolute joint. Without loss of generality, we assume that angle limits for joint i are −α ≤ q i ≤ α. In order to enforce this joint limit constraint with only link poses as decision variables, we consider a unit length vectorx J , perpendicular to the joint axisẑ i , and how it relates to itself before and after joint rotation. For example in Fig 5, rotating the i'th joint by q i transforms the unit length vectorx J tox i . The joint limit constraint |q i | ≤ α can be imposed as where ∠(·, ·) is the angle between two vectors. i−1x J is the given unit length vector fixed and expressed in the parent i − 1 frame, and ix i is the given unit length vector fixed and expressed in the child i frame. Equation (10) is a convex (quadratic) constraint on the decision variables (parent and child link orientations). The joint limit constraint (10) would be tight if W R i , W R i−1 satisfied SO(3) constraints exactly. Fig. 6: The visual illustration of the joint limit constraint (10). The red chord is the difference betweenx J and another vector, obtained by rotatingx J about an axis by angle α. The chord's length is 2 sin α/2.x J −x i should have shorter length than the red chord, since the angle q i between these two vectors is smaller than α.
To impose collision avoidance constraints, we use a similar formulation as [15]. We segment the free space into convex polytopes P i , i = 1, . . . , N p , with the vertices of P i denoted as v i,1 , . . . , v i,m i . A point is collision free if it is in one of the polytopes. We impose the following mixed-integer convex constraints to enforce the point Q, on link k with coordinates k p Q in the link frame, to be collision free z i is the binary variable indicating within which polytope the link point Q stays.
With the mixed-integer convex relaxation on SO(3) (sub-section 3.1), and the kinematic constraints in sub-section 3.2, we can solve the IK problem through mixed-integer convex (quadratic) optimization, to find the body positions and an approximated solution to the body orientations. If the mixed-integer convex program is infeasible, it proves that the original unrelaxed IK problem is globally infeasible.

Reconstruct joint angle by projecting back to SO(3)
After obtaining the approximated solution to the body orientations through the mixed-integer convex optimization, we need to recover the value of each joint angle. Since the solution to the body orientation does not satisfy the SO(3) constraint exactly, we project the approximated solution to the rotation constraint, starting from the root link. If the link is floating, then the optimal projected solution is obtained as UV T , whereR = UΣV T is the SVD ofR, as proved in [18]. On the other hand, if the link is connected to its parent link through a revolute joint, we project W R T i . Algebraically, the optimal q i is the solution to the following program In the objective function | · | F is the matrix Frobenius norm, |X| F = trace(X T X). R( i−1ẑ i , q i ) can be computed from the Rodriguez Formula below where i−1ẑ i × ∈ R 3×3 is the skew-symmetric matrix, representing the cross product with the vector i−1ẑ i . Substituting equation (13) into the optimization objective (12a), we can analytically obtain the optimal q i as where i . Note that at value π/2 + 2kπ − β the derivative of the objective (12a) is zero, i.e., it is the optimal solution to (12a) without the joint limits (12b). The optimal solution is truncated to the joint bounds if π/2+2kπ −β is outside of the angle limits. q i in (14) is the optimal joint angle within the joint limits, that minimizes the projection error from the mixed-integer convex optimization solution, to SO(3) with a given rotation axis and angle bounds. We thus obtain a posture as the approximated solution to the inverse kinematics problem.

Results
We present the results on the SO(3) relaxation and the inverse kinematics algorithm. The programs run on 64 bit Intel Xeon CPUs with Gurobi 7.5 [1] as the solver.

Tightness of rotation matrix relaxation
We first show the tightness of our mixed-integer convex relaxation of the SO(3) constraint on the rotation matrix. IfR ∈ R 3×3 satisfies the relaxed constraints, then we know that |Rv| 2 ≈ |v| 2 ∀v ∈ R 3 , and ∠(Rv 1 ,Rv 2 ) ≈ ∠(v 1 , v 2 ). Geometrically the relaxation allows the transformation v →Rv to change the length of a vector, and to perturb the angle between two vectors. In this section we show how much shorter the transformed vector can be, and also derive a bound on the angle error.
We find the global optimum to two mixed-integer convex optimization programs, to determine the tightness of the relaxation. First we compute the global minimum of minR ,i |Re i | 2 , where e i is the unit length vector with e i (i) = 1. To determine the angle error, we compute the global minimum minR in relaxation,i = j |Re i +Re j | 2 . If we denote this global minimum as d, we obtain loose bounds on the angle after transformation as 180 • − arccos (d 2 /2 − 1) ≤ ∠(Re i ,Re j ) ≤ arccos(d 2 /2 − 1). We compute the error bounds on the relaxation, by partitioning the range [−1, 1] into N = 2, 4 or 6 intervals, with φ i = −1 + 2i/N. The results are in Table 1. The last column with infinite number of binary variables is the idealR ∈ SO(3) case without any relaxation. The bounds on |Re i | 2 and |Re i +Re j | 2 are tight, but the bound on the angle in the last row of the  As the number of intervals increases, the convex hull of xy in each interval is closer to the bilinear product. As a result in Table 1, the minimal length ofRe i increases, and the bound on the angle error shrinks, leading to a tighter relaxation of SO(3). In the inverse kinematics problem, we choose to cut [−1, 1] into 4 intervals, as a compromise between computation speed and relaxation accuracy.

Inverse Kinematics
We first show that our inverse kinematics approach can find postures for complicated kinematics constraints including inequalities, such as those arising from the task "grasping a mug on the table in the cluttered environment" in Fig 7. We show two distinct postures found by Gurobi on the mixed-integer convex optimization programs. These two postures have different sets of active binary variables. We use a KUKA IIWA arm and a Schunk gripper with 7 total joints. The kinematics constraints are • The middle point between the two fingers should be on the axis of the mug. The middle point should be above the table, and lower than the top rim of the mug. • The y axis on the hand should be horizontal.
• The links and the gripper should be collision free. We also show that our approach can prove the global infeasibility of some kinematics constraints. In Fig 8, we impose the task constraint that the hand should grasp the wine bottle, by keeping its center on the green line segment, and its orientation aligned with the bottle's longitudinal axis. In the left plot the mixed-integer program is solved successfully, while in the right plot with the fridge on the table, the additional collision avoidance constraint causes the mixed-integer convex program to be globally infeasible. The solver takes 0.75 seconds to detect the global infeasibility.
For more complicated robots like LittleDog (Fig 9), we show that our approach can find a posture of the robot with each leg on one of the stepping stones. This constraint is imposed similar to the collision avoidance constraint (11a)-(11c), but  To get some statistics on our mixed-integer convex IK approach, we test its performance on an ABB IRB140 arm with 6 DoF, for which the IK problem can be solved analytically [11]. In Fig 10, we show the results of running both analytical and mixed-integer convex IK on the robot. In each column of the figures, an IK problem is solved to determine whether the end effector will reach a sampled location with the given orientation. There are three categories of the solutions • Green dot. Both analytical and mixed-integer convex IK find the solution.
• Blue dot. Both analytical and mixed-integer convex IK prove global infeasiblity.
• Red dot. Analytical IK proves the problem is infeasible, but mixed-integer convex IK thinks the problem is feasible, due to the relaxation.
We first analyze the tightness of the relaxation. The red dots, which imply loose relaxation on the SO(3), form a thin layer between the unreachable blue dots, and the reachable green dots. Only 2.43% of all the samples are red. For all the kinematically unreachable points (red and blue), 94.72% of them are detected as global infeasible in our mixed-integer IK solver; on the other hand, SDP relaxation of the SO(3) constraint, proposed in [27,12], can detect only 42.23% of infeasibility. This huge difference demonstrates that our mixed-integer convex relaxation of SO(3) constraint is a lot tighter than the SDP relaxation.
We also examine the computation time of the mixed-integer convex IK, together with the quality of the approximated solution. In Fig 11 we show the histogram of the mixed-integer convex IK computation time for the green dots (both IK approaches can find the solution). 95.1% of samples are solved within 10s, and the average computation time is 4.2s. In Fig 12 after reconstructing the posture from the mixed-integer convex IK, we compute the end effector pose error from the reconstructed posture to the sampled pose, to demonstrate how much relaxation error is brought into the approximated IK solution. Most of the solutions have less than 5cm of position error, and less than 2 • of orientation error. The solutions with large pose error all have some joints with active joint limits. This large error occurs because the joint limit constraint (10) would be tight only if the link orientation matrices W R i−1 , W R i were on SO(3). With our mixed-integer convex relaxation on SO (3), the angle π/2 + 2kπ − β in (14), where the derivative of the objective function (12a) would be zero, falls outside of the joint limits. So the reconstructed joint angle is instead truncated to its angle limits ±α. This truncation can cause large cost in the projection error (12a), resulting in significant deviation in the end effector pose.
In Fig 13a, 13b we draw the histograms of the computation time, when the mixedinteger convex IK proves global infeasibility. In 86.05% cases the computation takes less than 0.1s.
Finally, we solve the inverse kinematics program for the samples in Fig 10a, 10b through nonlinear optimization [17]. We first set the initial seed as the postures in 10a and 10b, and then solve the nonlinear optimization again, but replace the initial seed with the reconstructed posture from the mixed-integer convex IK. The success rate of the nonlinear IK solver increases from 85.67% to 100%. This improvement demonstrates that the approximate solution from the mixed-integer convex IK can always serve as a good initial seed for the nonlinear optimization program.

Conclusion and Discussion
In this paper we propose a mixed-integer convex relaxation of the non-convex SO(3) constraint, and formulate a mixed-integer convex optimization program to solve the inverse kinematics problem globally with generic constraints. We show that this relaxation is relatively tight, and our IK approach can either produce an approximate solution, or prove that the solution does not exist globally. We demonstrate results when applying our approach on manipulators and quadruped robots.
Currently the computation time is a lot slower than the gradient-based nonlinear optimization approach, and the reconstructed posture can violate the kinematic con- arm. We take 9321 sample points in an 1 m 3 cube. The robot end effector is required to reach each sampled location with two different given orientations. The desired orientation of the end effector is shown in silver color next to the coordinate axes in the bottom left of the figure. The green dots correspond to positions for which both IK approaches obtain the solution. The blue dots correspond to positions for which both IK approaches guarantee global infeasibility. The red dots correspond to the gap for which the analytical IK proves that the problem is infeasible, while the mixed-integer convex IK thinks the problem is feasible under relaxation. In the first row of the figures we show all three colors of dots. In the last two rows we highlight each color of the dots separately.  straints with large errors. We are working on improving the accuracy of the solution, and reducing the computation time.
We would like to thank our colleagues at Toyota Research Institute and Robot Locomotion Group for the valuable discussions.