Globally Optimal Solution to Inverse Kinematics of 7DOF Serial - - PowerPoint PPT Presentation

▶
globally optimal solution to inverse kinematics of 7dof
SMART_READER_LITE
LIVE PREVIEW

Globally Optimal Solution to Inverse Kinematics of 7DOF Serial - - PowerPoint PPT Presentation

Jan 16, 2024 278 likes •455 views

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

slide-1
SLIDE 1

Globally Optimal Solution to Inverse Kinematics of 7DOF Serial Manipulator

Pavel Trutman1 Mohab Safey El Din2 Didier Henrion3 Tomas Pajdla1

1CIIRC CTU in Prague 2Sorbonne Universit´

e, Inria, LIP6 CNRS

3LAAS-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

slide-2
SLIDE 2

Problem formulation

Serial manipulator with 7DOF

◮ 7 revolute joints → 7 DOF. ◮ i-th joint is parametrized by angle θi. ◮ Rigid body in space has 6DOF → redundant manipulator. ◮ One DOF left → self-motion.

x y

  • α

θ1 θ2 θ3   x y α     θ1 θ2 θ3  

Direct kinematics Inverse kinematics

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

slide-3
SLIDE 3

Problem formulation

Direct kinematics

◮ Description of the manipulator by Denavit-Hartenberg (D-H) convention [2]. ◮ D-H transformation matrices Mi(θi) ∈ R4×4 from link i to i − 1. ◮ Transformation M from the end effector coordinate system to the base coordinate system

7

  • i=1

Mi(θi) = M. (1) ◮ M represents the end effector pose w.r.t. the base coordinate system M =

  • R

t 1

  • , t ∈ R3 and R ∈ SO(3).

(2) ◮ Known joint angles θi → evaluation of Equation (1) gives the end effector pose M. ◮ Joint limits (i = 1, . . . , 7): θLow

i

≤ θi ≤ θHigh

i

. (3)

  • P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla

Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 3 / 16

slide-4
SLIDE 4

Problem formulation

Inverse kinematics (IK) problem

◮ Known end effector pose M → joint angles θi. ◮ Solve 7

i=1 Mi(θi) = M for θi.

◮ For redundant manipulator there is an infinite number of solution. ◮ Let us introduce an objective function to choose an optimal solution. min

θ∈−π;π)7 7

max

i=1 θi

(4) ◮ Approximation by sum of squares. min

θ∈−π;π)7 7

  • i=1

θ2

i

(5)

Base X

  • r

θ1 θ2 θ3 θ4 θ′

1

θ′

2

θ′

3

θ′

4

Figure: Two configurations of a planar manipulator with different values of the

  • bjective function.
  • P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla

Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 4 / 16

slide-5
SLIDE 5

Problem formulation

Optimization problem

◮ Optimization problem: min

θ∈−π;π)7 7

  • i=1

θ2

i

s.t. 7

i=1 Mi(θi) = M

θLow

i

≤ θi ≤ θHigh

i

(i = 1, . . . , 7) (6) ◮ Not polynomial, contains trigonometric functions. ◮ We remove them by rewriting the problem in new variables c = [c1, . . . , c7]⊤ and s = [s1, . . . , s7]⊤, which represent the cosines and sines of the joint angles θ = [θ1, . . . , θ7]⊤ respectively. ◮ To preserve the structure, we need to add the trigonometric identities: qi(c, s) = c2

i + s2 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

slide-6
SLIDE 6

Problem formulation

Polynomial optimization problem

◮ Polynomial optimization problem equivalent to the original optimization problem: min

c∈−1,17, s∈−1,17 ||c − 1||2

s.t. pj(c, s) = 0 (j = 1, . . . , 12) qi(c, s) = 0 (i = 1, . . . , 7) −(ci + 1) tan θLow

i

2

+ si ≥ 0 (i = 1, . . . , 7) (ci + 1) tan θHigh

i

2

− si ≥ 0 (i = 1, . . . , 7) (8) ◮ 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

slide-7
SLIDE 7

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

slide-8
SLIDE 8

Solving the IK problem

Symbolic reduction

Theorem

The ideal generated by the kinematics constraints pj for generic serial manipulator with seven revolute joints and for generic pose M with addition of the trigonometric identities qi can be generated by a set of degree two polynomials.

Proof.

The proof is computational. See the diagram.

Generic manipulator Generic pose M Polynomial constraints pj, qi G ← Gr¨

  • bner basis of pj, qi

S = {f ∈ G | deg(f) = 2} G′ ← Gr¨

  • bner basis of S

G = G′

  • pj, qi = S
  • P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla

Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 8 / 16

slide-9
SLIDE 9

Solving the IK problem

Solving the reduced polynomial optimization problem

Corollary

Polynomials pj and qi 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

slide-10
SLIDE 10

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:

◮ 10 000 randomly chosen poses. ◮ From within and outside of the working space of the manipulator. Figure: Manipulator KUKA LBR iiwa.

  • P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla

Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 10 / 16

slide-11
SLIDE 11

Experiments

Degree four polynomials

◮ Solve the polynomial optimization problem with degree four polynomials. ◮ For relaxation order two. ◮ Using polynomial optimization toolbox GloptiPoly with MOSEK as the semidefinite problem solver. ◮ For 29.3 % poses we failed to compute the solution or report infeasibility.

  • 800-600-400-200 0 200 400 600 800-800
  • 600
  • 400
  • 200

200 400 600 800 100 200 300 400 500 600 700 800 900 1000 Feasible poses Infeasible poses Poses failed to compute x [mm] y [mm] z [mm]

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

slide-12
SLIDE 12

Experiments

Degree two polynomials

◮ Advantage of special structure of KUKA LBR: eliminate variables c4 and s4. ◮ Symbolically reduce the degree four polynomials to degree two polynomials (Maple). ◮ Solve for relaxation order two. ◮ Using polynomial optimization toolbox GloptiPoly with MOSEK as the semidefinite problem solver. ◮ For 0.1 % poses we failed to compute the solution

  • r report infeasibility.
  • 800-600-400-200 0 200 400 600 800-800
  • 600
  • 400
  • 200

200 400 600 800 100 200 300 400 500 600 700 800 900 1000 Feasible poses Infeasible poses Poses failed to compute x [mm] y [mm] z [mm]

Figure: Poses of the manipulator solved 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

slide-13
SLIDE 13

Experiments

Numerical stability and execution time evaluation

◮ End effector poses have been computed by direct kinematics from estimated θ. ◮ Pose error w.r.t. desired poses measured in 3D space.

100 101 102 103 104 10−6 10−5 10−4 10−3 10−2 10−1 100 Frequency Pose error Translation error [mm] Rotation error [deg]

Figure: Histogram of pose errors.

◮ Execution time of on-line phase of GloptiPoly and of the symbolic reduction

  • f the polynomials.

100 101 102 103 104 105 2 4 6 8 10 12 Frequency GloptiPoly execution time [s] 1.6 1.8 2 2.2 2.4 2.6 2.8 3 Maple execution time [s] Feasible poses Infeasible poses Poses failed to compute

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

slide-14
SLIDE 14

Conclusions

Conclusions

◮ We proved that the variety of IK solutions of all generic 7DOF revolute serial manipulators can be generated by second degree polynomials only. ◮ We presented a practical method for globally solving 7DOF IK problem with polynomial

  • bjective function.

◮ Our solution is accurate and can solve/decide infeasibility in 99.9 % cases tested on KUKA LBR iiwa manipulator. ◮ The code is open-sourced at https://github.com/PavelTrutman/Global-7DOF-IKT.

Execution time [s] Median error % of failed poses Reduction step GloptiPoly Translation [mm] Rotation [deg]

  • Deg. 4 polynomials

— 18.4 2.12 · 10−4 3.32 · 10−5 29.3 %

  • Deg. 2 polynomials

2.3 5.5 6.28 · 10−5 5.57 · 10−3 0.1 %

Table: Overview of execution times and accuracy of the presented methods.

  • P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla

Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 14 / 16

slide-15
SLIDE 15

Acknowledgement

Acknowledgement

  • P. Trutman was supported by the EU Structural and Investment Funds, Operational Programe Research,

Development and Education under the project IMPACT (reg. no. CZ.02.1.01/0.0/0.0/15 003/0000468) and Grant Agency of the CTU Prague project SGS19/173/OHK3/3T/13.

  • T. Pajdla was suppoted by IMPACT Project CZ.02.1.01/0.0/0.0/15 003/0000468 & EU Structural and

Investment Funds, Operational Programe Research, Development and Education and ARtwin - An AR cloud and digital twins solution for industry and construction 4.0 (GA No 856994) H-2020 project.

  • P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla

Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 15 / 16

slide-16
SLIDE 16

Bibliography

Bibliography

[1] Hongkai Dai, Gregory Izatt, and Russ Tedrake. Global inverse kinematics via mixed-integer convex optimization. The International Journal of Robotics Research, page 0278364919846512, 2017. [2] Richard S Hartenberg and Jacques Denavit. A kinematic notation for lower pair mechanisms based on matrices. Journal of applied mechanics, 77(2):215–221, 1955. [3] I Kuhlemann, A Schweikard, P Jauer, and F Ernst. Robust inverse kinematics by configuration control for redundant manipulators with seven dof. In 2016 2nd International Conference on Control, Automation and Robotics (ICCAR), pages 49–55. IEEE, 2016. [4] Jean B. Lasserre. Global optimization with polynomials and the problem of moments. Society for Industrial and Applied Mathematics Journal on Optimization, 11:796–817, 2001.

  • P. Trutman, M. Safey El Din, D. Henrion, T. Pajdla

Globally Optimal Solution to IK of 7DOF Manipulator PGMO Days 2019 16 / 16