LECTURE Pavel Trutman Globally Optimal Solution to Inverse Kinematics of 7DOF Serial Manipulator 14.1.2020, 13:45 Project name: Intelligent Machine Perception Project Registration Number: CZ.02.1.01/0.0/0.0/15_003/0000468 Venue: CIIRC, B-670, Jugoslávských partyzánů 1580/3, Prague 6
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 January 14, 2020 P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator IMPACT seminar 1 / 16
Problem formulation Serial manipulator with 7 DOF ◮ 7 revolute joints → 7 DOF. θ 3 � x � y ◮ i -th joint is parametrized by θ 2 α angle θ i . Forward kinematics θ 1 x ◮ Rigid body in space has 6 DOF θ 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 IMPACT seminar 2 / 16
Problem formulation Denavit-Hartenberg convention ◮ Description of the manipulator by Denavit-Hartenberg (D-H) convention [HD55]. ◮ Parameters α i , d i and a i are found (fixed for given manipulator). ◮ D-H transformation matrices M i ( θ i ) ∈ R 4 × 4 from link i to i − 1 . Figure: DH convention. cos θ i − sin θ i 0 0 1 0 0 a i sin θ i cos θ i 0 0 0 cos α i − sin α i 0 M i ( θ i ) = (1) 0 0 1 d i 0 sin α i cos α i 0 0 0 0 1 0 0 0 1 P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator IMPACT seminar 3 / 16
Problem formulation Forward kinematics ◮ Transformation M from the end effector coordinate system to the base coordinate system 7 � M i ( θ i ) = M. (2) 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 = (3) 0 1 ◮ Known joint angles θ i → evaluation of Equation (2) gives the end effector pose M . ◮ Joint limits ( i = 1 , . . . , 7 ): ≤ θ i ≤ θ High θ Low . (4) i i P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator IMPACT seminar 4 / 16
Problem formulation Inverse kinematics (IK) problem r � θ 4 ◮ Known end effector pose M → joint angles θ i . X θ ′ 4 ◮ Solve � 7 i =1 M i ( θ i ) = M for θ i . θ ′ 3 ◮ For redundant manipulator there is an infinite θ 2 number of solution. ◮ Let us introduce an objective function to choose an optimal solution. θ ′ 2 7 θ 1 min max i =1 � θ i � (5) θ 3 θ ∈�− π ; π ) 7 θ ′ 1 Base ◮ Approximation by sum of squares. Figure: Two configurations of a planar 7 manipulator with different values of the θ 2 � min (6) objective function. i θ ∈�− π ; π ) 7 i =1 P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator IMPACT seminar 5 / 16
Problem formulation Optimization problem ◮ Optimization problem: 7 θ 2 � min i θ ∈�− π ; π ) 7 i =1 (7) 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 . (8) P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator IMPACT seminar 6 / 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) (9) − ( 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 IMPACT seminar 7 / 16
Polynomial optimization methods Polynomial optimization methods Polynomial problem: ◮ Objective function: polynomial. ◮ Constraints: polynomial inequalities and equations. ◮ Non-convex . Semidefinite program [Las01]: ◮ Each monomial is substituted by a new variable. ◮ Objective function: linear. ◮ Constraints: linear matrix inequalities, linear equations. ◮ Convex , but infinite-dimensional . P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator IMPACT seminar 8 / 16
Polynomial optimization methods Polynomial optimization methods Polynomial problem: ◮ Objective function: polynomial. ◮ Constraints: polynomial inequalities and equations. ◮ Non-convex . Relaxed semidefinite program [Las01]: ◮ Limit the degree of substituted monomials by degree r ∈ N . p ∗ r ≤ p ∗ r +1 ≤ p ∗ (10) ◮ Convex and finite-dimensional . r → + ∞ p ∗ r = p ∗ lim (11) ◮ Convergence is ensured. Implemented in Gloptipoly [HLL09]. P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla Globally Optimal Solution to IK of 7DOF Manipulator IMPACT seminar 8 / 16
Solving the IK problem Direct application of polynomial solver ◮ Direct application of Lasserre hierarchies [Las01] on the problem. ◮ Second order relaxation ◮ 14 variables, monomials up to degree 4 → SDP program with 3060 variables. ◮ Computation time in seconds. ◮ Solution not obtained in many cases. ◮ Third order relaxation ◮ 14 variables, monomials up to degree 6 → 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 IMPACT seminar 9 / 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 IMPACT seminar 10 / 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 [Las01] on the symbolically reduced problem with degree two polynomials. ◮ First order relaxation ◮ 14 variables, monomials up to degree 2 → SDP program with 120 variables. ◮ Solution typically not obtained. ◮ Second order relaxation ◮ 14 variables, monomials up to degree 4 → 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 IMPACT seminar 11 / 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. [Kuh+16]; new parameter δ is introduced to fix the left DOF. ◮ Dai et al. [DIT17] 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 IMPACT seminar 12 / 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 IMPACT seminar 13 / 16
Recommend
More recommend
