Globally Optimal Solution to Inverse Kinematics of 7DOF Serial Manipulator Pavel Trutman 1 Mohab Safey El Din 2 Didier Henrion 3 Tomas Pajdla 1 1 CIIRC CTU in Prague 2 Sorbonne Universit´ e, Inria, LIP6 CNRS 3 LAAS-CNRS, FEE CTU in Prague December 4, 2019 P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 1 / 16
Problem formulation Serial manipulator with 7DOF ◮ 7 revolute joints → 7 DOF. θ 3 � x � y ◮ i -th joint is parametrized by θ 2 α angle θ i . Direct kinematics θ 1 x ◮ Rigid body in space has 6DOF θ 2 y Inverse kinematics θ 3 α → redundant manipulator. θ 1 ◮ One DOF left → self-motion. Figure: Example of planar manipulator. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 2 / 16
Problem formulation Direct kinematics ◮ Description of the manipulator by Denavit-Hartenberg (D-H) convention [2]. ◮ D-H transformation matrices M i ( θ i ) ∈ R 4 × 4 from link i to i − 1 . ◮ Transformation M from the end effector coordinate system to the base coordinate system 7 � M i ( θ i ) = M. (1) i =1 ◮ M represents the end effector pose w.r.t. the base coordinate system � � R t , t ∈ R 3 and R ∈ SO (3) . M = (2) 0 1 ◮ Known joint angles θ i → evaluation of Equation (1) gives the end effector pose M . ◮ Joint limits ( i = 1 , . . . , 7 ): ≤ θ i ≤ θ High θ Low . (3) i i P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 3 / 16
Problem formulation Inverse kinematics (IK) problem ◮ Known end effector pose M → joint angles θ i . ◮ Solve � 7 i =1 M i ( θ i ) = M for θ i . ◮ For redundant manipulator there is an infinite number of solution. ◮ Let us introduce an objective function to choose r � θ 4 an optimal solution. X θ ′ 4 θ ′ 3 7 θ 2 min max i =1 � θ i � (4) θ ∈�− π ; π ) 7 ◮ Approximation by sum of squares. θ ′ 2 θ 1 θ 3 θ ′ 7 1 Base � θ 2 min (5) i θ ∈�− π ; π ) 7 Figure: Two configurations of a planar i =1 manipulator with different values of the objective function. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 4 / 16
Problem formulation Optimization problem ◮ Optimization problem: 7 θ 2 � min i θ ∈�− π ; π ) 7 i =1 (6) s.t. � 7 i =1 M i ( θ i ) = M ≤ θ i ≤ θ High θ Low ( i = 1 , . . . , 7) i i ◮ Not polynomial, contains trigonometric functions. ◮ We remove them by rewriting the problem in new variables c = [ c 1 , . . . , c 7 ] ⊤ and s = [ s 1 , . . . , s 7 ] ⊤ , which represent the cosines and sines of the joint angles θ = [ θ 1 , . . . , θ 7 ] ⊤ respectively. ◮ To preserve the structure, we need to add the trigonometric identities: q i ( c , s ) = c 2 i + s 2 i − 1 = 0 , i = 1 , . . . , 7 . (7) P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 5 / 16
Problem formulation Polynomial optimization problem ◮ Polynomial optimization problem equivalent to the original optimization problem: c ∈�− 1 , 1 � 7 , s ∈�− 1 , 1 � 7 || c − 1 || 2 min s.t. p j ( c , s ) = 0 ( j = 1 , . . . , 12) q i ( c , s ) = 0 ( i = 1 , . . . , 7) (8) − ( c i + 1) tan θ Low + s i ≥ 0 ( i = 1 , . . . , 7) i 2 ( c i + 1) tan θ High − s i ≥ 0 ( i = 1 , . . . , 7) i 2 ◮ In 14 variables ( c and s ). ◮ Contains polynomials up to degree four. ◮ When solved, θ are recovered from c and s by function atan2. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 6 / 16
Solving the IK problem Direct application of polynomial solver ◮ Direct application of Lasserre hierarchies [4] on the problem. ◮ Second order relaxation ◮ 14 variables, degree 4 polynomials → SDP program with 3060 variables. ◮ Computation time in seconds. ◮ Solution not obtained in many cases. ◮ Third order relaxation ◮ 14 variables, degree 6 polynomials → SDP program with 38 760 variables. ◮ Computation time in hours. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 7 / 16
Solving the IK problem Symbolic reduction Generic manipulator Generic pose M Polynomial constraints p j , q i Theorem The ideal generated by the kinematics constraints p j G ← Gr¨ obner basis of � p j , q i � for generic serial manipulator with seven revolute joints and for generic pose M with addition of the trigonometric identities q i can be generated by a set of S = { f ∈ G | deg( f ) = 2 } degree two polynomials. G ′ ← Gr¨ obner basis of S Proof. G = G ′ � The proof is computational. See the diagram. � p j , q i � = � S � P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 8 / 16
Solving the IK problem Solving the reduced polynomial optimization problem Corollary Polynomials p j and q i up to degree four in POP can be replaced by degree two polynomials. ◮ Application of Lasserre hierarchies [4] on the symbolically reduced problem with degree two polynomials. ◮ First order relaxation ◮ 14 variables, degree 2 polynomials → SDP program with 120 variables. ◮ Solution typically not obtained. ◮ Second order relaxation ◮ 14 variables, degree 4 polynomials → SDP program with 3060 variables. ◮ Computation time in seconds. ◮ Gives solution for almost all poses. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 9 / 16
Experiments Experiments with KUKA LBR iiwa ◮ Special structure: for fixed end effector pose the joint angle θ 4 is constant. ◮ Previous work: ◮ Geometrical derivation of a closed form solution by Kuhlemann et al. [3]; new parameter δ is introduced to fix the left DOF. ◮ Dai et al. [1] proposed mix-integer convex relaxation of the non-convex rotational constraints; approximation introduces errors in units of centimeters and degrees. ◮ Synthetic dataset: Figure: Manipulator KUKA ◮ 10 000 randomly chosen poses. LBR iiwa. ◮ From within and outside of the working space of the manipulator. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 10 / 16
Experiments Degree four polynomials Feasible poses Infeasible poses ◮ Solve the polynomial optimization problem with Poses failed to compute degree four polynomials. ◮ For relaxation order two. 1000 900 800 700 600 800 ◮ Using polynomial optimization toolbox GloptiPoly z [mm] 500 600 400 400 300 with MOSEK as the semidefinite problem solver. 200 200 100 0 y [mm] 0 -200 -800-600-400-200 0 200 400 600 800-800 -400 -600 ◮ For 29 . 3 % poses we failed to compute the x [mm] solution or report infeasibility. Figure: Poses of the manipulator solved from degree four polynomials. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 11 / 16
Experiments Degree two polynomials ◮ Advantage of special structure of KUKA LBR: Feasible poses eliminate variables c 4 and s 4 . Infeasible poses Poses failed to compute ◮ Symbolically reduce the degree four polynomials to degree two polynomials (Maple). 1000 900 800 700 ◮ Solve for relaxation order two. 600 800 z [mm] 500 600 400 400 300 200 200 ◮ Using polynomial optimization toolbox GloptiPoly 100 0 y [mm] 0 -200 -800-600-400-200 0 200 400 600 800-800 -400 with MOSEK as the semidefinite problem solver. -600 x [mm] ◮ For 0 . 1 % poses we failed to compute the solution Figure: Poses of the manipulator solved or report infeasibility. from degree two polynomials. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 12 / 16
Experiments Numerical stability and execution time evaluation ◮ End effector poses have been computed ◮ Execution time of on-line phase of by direct kinematics from estimated θ . GloptiPoly and of the symbolic reduction of the polynomials. ◮ Pose error w.r.t. desired poses measured in 3D space. 10 5 Translation error [mm] Feasible poses Rotation error [deg] Infeasible poses 10 4 10 4 Poses failed to compute Frequency 10 3 10 3 Frequency 10 2 10 2 10 1 10 1 10 0 10 0 2 4 6 8 10 12 1 . 6 1 . 8 2 2 . 2 2 . 4 2 . 6 2 . 8 3 10 − 6 10 − 5 10 − 4 10 − 3 10 − 2 10 − 1 10 0 GloptiPoly execution time [s] Maple execution time [s] Pose error Figure: Histogram of pose errors. Figure: Histograms of execution time. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 13 / 16
Recommend
More recommend
