Robotics 1 Inverse kinematics

Robotics 1 Inverse kinematics Prof. Alessandro De Luca Robotics 1 1

Inverse kinematics what are we looking for? direct kinematics is always unique; how about inverse kinematics for this 6R robot? Robotics 1

Inverse kinematics problem! given a desired end-effector pose position + orientation), find the values of the joint variables that will realize it! a synthesis problem, with input data in the form R p! T = = 0 A n q) 000 1 classical formulation: inverse kinematics for a given end-effector pose! a typical nonlinear problem! existence of a solution workspace definition)! uniqueness/multiplicity of solutions r! R m, q!r n )! solution methods p " r = = f r q), or for any " other task vector generalized formulation: inverse kinematics for a given value of task variables Robotics 1 3

Solvability and robot workspace for tasks related to a desired end-effector Cartesian pose)! primary workspace WS 1 : set of all positions p that can be reached with at least one orientation " or R)! out of WS 1 there is no solution to the problem! when p! WS 1, there is a suitable " or R) for which a solution exists! secondary or dexterous) workspace WS : set of positions p that can be reached with any orientation among those feasible for the robot direct kinematics)! when p! WS, there exists a solution for any feasible " or R)! WS # WS 1 Robotics 1 4

Workspace of Fanuc R-000i/165F section for a constant angle $ 1 WS 1 R 3! WS for spherical wrist without joint limits) rotating the base joint angle $ 1 Robotics 1 5

Workspace of planar R arm y l q p orientations if p! intws 1 ) = WS 1 - WS 1 l 1 -l l 1 +l l 1! if l 1 l q 1! WS 1 = {p! R : l 1 -l &!p!& l 1 +l } R! WS = '! if l 1 = l = "! WS 1 = {p! R :!p!& "} R x WS 1 inner and outer boundaries 1 orientation! WS = {p = 0} infinite number of feasible orientations at the origin) Robotics 1 6

Wrist position and E-E pose inverse solutions for an articulated 6R robot LEFT DOWN Unimation PUMA 560 RIGHT DOWN 4 inverse solutions out of singularities for the position of the wrist center only) LEFT UP 8 inverse solutions considering the complete E-E pose spherical wrist: alternative solutions for the last 3 joints) RIGHT UP Robotics 1 7

Counting and visualizing the 8 solutions to the inverse kinematics of a Unimation Puma 560 RIGHT UP RIGHT DOWN LEFT UP LEFT DOWN Robotics 1 8

Inverse kinematic solutions of UR10 6-dof Universal Robot UR10, with non-spherical wrist video slow motion) desired pose #"0.373& p = "0.083 $ 1.34 ' [m] # 3 / 0.5 0& R = "0.5 3 / 0 $ 0 0 1' configuration at start q = " /3 #" /3 " /6 0 " / 0) [rad] Robotics 1 9

The 8 inverse kinematic solutions of UR10 Robotics 1 10 q = 1.047 "1.833 "0.7376 ".6915 "1.5708 3.1416 # $ & ' q = 1.047 "1.9941 0.7376.873 "1.5708 3.1416 # $ & ' q = 1.047 "1.5894 "0.536 0.54 1.5708 0 # $ & ' q = 1.047 ".0944 0.536 0 1.5708 0 # $ & ' q =.7686 "1.55 0.536.5994 "1.5708 1.40 # $ & ' q =.7686 "1.1475 "0.7376 0.3143 1.5708 "1.714 # $ & ' q =.7686 "1.8583 0.7376 "0.4501 1.5708 "1.714 # $ & ' q =.7686 "1.047 "0.536 3.1416 "1.5708 1.40 # $ & ' shoulderright wristdown elbowup shoulderright wristdown elbowdown shoulderright wristup elbowup shoulderright wristup elbowdown shoulderleft wristdown elbowdown shoulderleft wristdown elbowup shoulderleft wristup elbowdown shoulderleft wristup elbowup

Multiplicity of solutions some examples! E-E positioning m=) of a planar R robot arm! regular solutions in intws 1 )! 1 solution on WS 1! for l 1 = l : ) solutions in WS singular solutions! E-E positioning of an articulated elbow-type 3R robot arm! 4 regular solutions in WS 1 with singular cases yet to be investigated...)! spatial 6R robot arms! & 16 distinct solutions, out of singularities: this upper bound of solutions was shown to be attained by a particular instance of orthogonal robot, i.e., with twist angles * i = 0 or ±+/,i)! analysis based on algebraic transformations of robot kinematics! transcendental equations are transformed into a single polynomial equation of one variable! seek for an equivalent polynomial equation of the least possible degree Robotics 1 11

y l 1 A planar 3R arm workspace and number/type of inverse solutions q 1 l q l 3 p 1. in WS 1 : ) 1 regular solutions except for. and 3.), at which the E-E can take a continuum of ) orientations but not all orientations in the plane!). if!p!= 3" : only 1 solution, singular 3. if!p!= " : ) 1 solutions, 3 of which singular q 3 l 1 = l = l 3 = ", n=3, m= x WS 1 = {p! R :!p!& 3"} R WS = {p! R :!p!& "} R any planar orientation is feasible in WS 4. if!p!< " : ) 1 regular solutions never singular) Robotics 1 1

Workspace of a planar 3R arm general case: different link lengths l min = l 3 = 0.3 R in = 0, Robotics 1 13

! if m = n!! solutions Multiplicity of solutions summary of the general cases! a finite number of solutions regular/generic case)! degenerate solutions: infinite or finite set, but anyway different in number from the generic case singularity)! if m < n robot is redundant for the kinematic task)!! solutions! ) n-m solutions regular/generic case)! a finite or infinite number of singular solutions! use of the term singularity will become clearer when dealing with differential kinematics! instantaneous velocity mapping from joint to task velocity! lack of full rank of the associated m"n Jacobian matrix Jq) Robotics 1 14

Dexter robot 8R arm)! m = 6 position and orientation of E-E)! n = 8 all revolute joints)! ) inverse kinematic solutions redundancy degree = n-m = ) video exploring inverse kinematic solutions by a self-motion Robotics 1 15

Solution methods ANALYTICAL solution in closed form) " preferred, if it can be found * " use ad-hoc geometric inspection " algebraic methods solution of polynomial equations) " systematic ways for generating a reduced set of equations to be solved * sufficient conditions for 6-dof arms 3 consecutive rotational joint axes are incident e.g., spherical wrist), or 3 consecutive rotational joint axes are parallel D. Pieper, PhD thesis, Stanford University, 1968 NUMERICAL solution in iterative form) " certainly needed if n>m redundant case), or at/close to singularities " slower, but easier to be set up " in its basic form, it uses the analytical) Jacobian matrix of the direct kinematics map J r q) = f r q) q " Newton method, Gradient method, and so on Robotics 1 16

y p y Inverse kinematics of planar R arm l q p direct kinematics p x = l 1 c 1 + l c 1 l 1 p y = l 1 s 1 + l s 1 q 1 p x x data q 1, q unknowns squaring and summing the equations of the direct kinematics p x + p y - l 1 + l ) = l 1 l c 1 c 1 + s 1 s 1 ) = l 1 l c and from this in analytical form c = p x + p y - l 1 - l )/ l 1 l, s = ±-1 - c q = ATAN {s, c } must be in [-1,1] else, point p is outside robot workspace!) solutions Robotics 1 17

y p y. Inverse kinematics of R arm cont d) q 1 * p q p x solutions one for each value of s ) x by geometric inspection q 1 = * -. q 1 = ATAN {p y, p x } - ATAN {l s, l 1 + l c } note: difference of ATAN needs to be re-expressed in -+, +]! q p {q 1,q } UP/LEFT q {q 1,q } DOWN/RIGHT q 1 q 1 q e q have same absolute value, but opposite signs Robotics 1 18

Algebraic solution for q 1 another solution method p x = l 1 c 1 + l c 1 = l 1 c 1 + l c 1 c - s 1 s ) p y = l 1 s 1 + l s 1 = l 1 s 1 + l s 1 c + c 1 s ) linear in s 1 and c 1 l 1 + l c - l s c 1 = p x l s l 1 + l c s 1 p y except for l 1 =l and c =-1 det = l 1 + l + l 1 l c ) > 0 being then q 1 undefined singular case: ) 1 solutions) q 1 = ATAN {s 1, c 1 } = ATAN {p y l 1 +l c )-p x l s )/det, p x l 1 +l c )+p y l s )/det} notes: a) this method provides directly the result in -+, +] b) when evaluating ATAN, det > 0 can be eliminated from the expressions of s 1 and c 1 Robotics 1 19

Inverse kinematics of polar RRP) arm Note: q is NOT a DH variable! p z q 3 p x = q 3 c c 1 p y = q 3 c s 1 p z = d 1 + q 3 s q d 1 p x + p y + p z - d 1 ) = q 3 p y q 3 = + -p x + p y + p z - d 1 ) p x q 1 our choice: take here only the positive value... if q 3 = 0, then q 1 and q remain both undefined stop); else q = ATAN{p z - d 1 )/q 3, ± -p x + p y )/q 3 } if p x +p y = 0, then q 1 remains undefined stop); else q 1 = ATAN{p y /c, p x /c } regular solutions {q 1,q,q 3 }) if it stops, a singular case: ) or ) 1 solutions) Robotics 1 we have eliminated q 3 >0 from both arguments! 0

Inverse kinematics of 3R elbow-type arm {u, d} = elbow up, down L q q 3 L 3 d 1 {f, b} = facing, backing the point p=p x,p y,p z ) p z p y p x direct kinematics q 1 p x = c 1 L c + L 3 c 3 ) p y = s 1 L c + L 3 c 3 ) p z = d 1 + L s + L 3 s 3 WS 1 ={spherical shell centered at 0,0,d 1 ), with outer radius R out =#L +L 3 and inner radius R in = L -L 3 } symmetric structure without offsets e.g., first 3 joints of Mitsubishi PA10 robot four regular inverse kinematics solutions in WS 1 Note: more details e.g., full handling of singular cases) can be found in the solution of the Robotics 1 written exam of 11.04.017 Robotics 1 1

Inverse kinematics of 3R elbow-type arm L d 1 q q 3 L 3 p z p y p x = c 1 L c + L 3 c 3 ) p y = s 1 L c + L 3 c 3 ) p z = d 1 + L s + L 3 s 3 direct kinematics p x q 1 p x + p y + p z -d 1 ) = c 1 L c + L 3 c 3 ) + s 1 L c + L 3 c 3 ) + L s + L 3 s 3 ) =... = L + L 3 + L L 3 c c 3 +s s 3 ) = L + L 3 + L L 3 c 3 c 3 = p x + p y + p z -d 1 ) - L - L 3 ) / L L 3 [-1,1] else, p is out of workspace!) q {+} 3 = ATAN{s 3, c 3 } ±s 3 = ±-1 - c 3 two solutions q {-} 3 = ATAN{-s 3, c 3 } = - q {+} 3 Robotics 1

Inverse kinematics of 3R elbow-type arm L d 1 q q 3 L 3 p z p y p x = c 1 L c + L 3 c 3 ) p y = s 1 L c + L 3 c 3 ) p z = d 1 + L s + L 3 s 3 direct kinematics p x q 1 being p x + p y = L c + L 3 c 3 ) > 0) only when p x + p y > 0 else q 1 is undefined infinite solutions!) c 1 = p x /±#p x + p y s 1 = p y /±#p x + p y again, two solutions q 1 {+} = ATAN{p y, p x } q 1 {-} = ATAN{-p y, -p x } Robotics 1 3

Inverse kinematics of 3R elbow-type arm L from the first two equations in the direct kinematics q q 3 L 3 c 1 p x + s 1 p y = L c + L 3 c 3 = L +L 3 c 3 ) c L 3 s 3 s d 1 p z p y p z d 1 = L s + L 3 s 3 = L 3 s 3 c + L +L 3 c 3 ) s p x q 1 we can solve a linear system Ax = b in the algebraic unknowns x =c, s ) # {+,"} L +L 3 c 3 " L 3 s 3 & # {+,"} c & # = c {+,"} 1 p x + s {+,"} 1 p y & $ L 3 s 3 L +L 3 c 3 ' $ s ' $ p z " d 1 ' coefficient matrix A known vector b four regular solutions for q, depending on combinations of {+,-} from q 1 and q 3 provided det A = p x + p y + p z -d 1 ) > 0 else q is undefined infinite solutions!) q {{f,b},{u,d}} = ATAN{s {{f,b},{u,d}}, c {{f,b},{u,d}} } Robotics 1 4

Inverse kinematics for robots with spherical wrist j6 first 3 robot joints of any type RRR, RRP, PPP, ) W z 4 z 3 z 5 y 6 x 6 O 6 = p z 6 = a z 0 j5 j4 d 6 x 0 j1 y 0 find q 1,, q 6 from the input data: p origin O 6 ) R = [n s a] orientation of RF 6 ) 1. W = p - d 6 a / q 1, q, q 3 inverse position kinematics for main axes). R = 0 R 3 q 1, q, q 3 ) 3 R 6 q 4, q 5, q 6 ) given known, after step 1 Euler ZYZ or ZXZ rotation matrix / 3 R 6 q 4, q 5, q 6 ) = 0 R T 3 R / q 4, q 5, q 6 inverse orientation kinematics for the wrist) Robotics 1 5

6R example: Unimation PUMA 600 spherical wrist n = 0 x 6 q) s = 0 y 6 q) a function of q 1, q, q 3 only! a = 0 z 6 q) p = 0 6 q) 8 different inverse solutions that can be found in closed form see Paul, Shimano, Mayer; 1981) here d 6 =0, Robotics 1 so that 0 6 =W directly 6

Numerical solution of inverse kinematics problems! use when a closed-form solution q to r d = f r q) does not exist or is too hard to be found! J r q) = f r analytical Jacobian) q! Newton method here for m=n)! r d = f r q) = f r q k ) + J r q k ) q - q k ) + o!q - q k! ) q k+1 = q k + J r -1 q k ) [r d - f r q k )] 0 neglected! convergence if q 0 initial guess) is close enough to some q * : f r q * ) = r d! problems near singularities of the Jacobian matrix J r q)! in case of robot redundancy m<n), use the pseudo-inverse J r# q)! has quadratic convergence rate when near to solution fast!) Robotics 1 7

Operation of Newton method! in the scalar case, also known as method of the tangent! for a differentiable function fx), find a root of fx * )=0 by iterating as x k+1 = x k " fx k ) f'x k ) an approximating sequence { x 1,x,x 3,x 4,x 5,...} " x * animation from http://en.wikipedia.org/wiki/file:newtoniteration_ani.gif Robotics 1 8

Numerical solution of inverse kinematics problems cont d)! Gradient method max descent)! minimize the error function Hq) = $!r d - f r q)! = $ [r d - f r q)] T [r d - f r q)] q k+1 = q k - * 1 q Hq k ) from 1 q Hq) = - J rt q) [r d - f r q)], we get q k+1 = q k + * J rt q k ) [r d - f r q k )]! the scalar step size * > 0 should be chosen so as to guarantee a decrease of the error function at each iteration too large values for * may lead the method to miss the minimum)! when the step size * is too small, convergence is extremely slow Robotics 1 9

Revisited as a feedback scheme r d + - r e J rt q) f r q). q q0) 3 q r d = cost * = 1) e = r d - f r q) / 0 4 closed-loop equilibrium e=0 is asymptotically stable V = $ e T e 5 0 Lyapunov candidate function... V = e T e = e T d q dt r d - f r q)) = - et J r = - e T J r J rt e & 0. V = 0 4 e! KerJ rt ) in particular e = 0 asymptotic stability Robotics 1 30

Properties of Gradient method! computationally simpler: Jacobian transpose, rather than its pseudo)-inverse! direct use also for robots that are redundant for the task! may not converge to a solution, but it never diverges! the discrete-time evolution of the continuous scheme q k+1 = q k + 6T J rt q k ) [r d - fq k )] * = 6T) is equivalent to an iteration of the Gradient method! scheme can be accelerated by using a gain matrix K>0. q = J rt q) K e note: K can be used also to escape from being stuck in a stationary point, by rotating the error e out of the kernel of J r T if a singularity is encountered) Robotics 1 31

A case study analytic expressions of Newton and gradient iterations! R robot with l 1 = l = 1, desired end-effector position r d = p d = 1,1)! direct kinematic function and error " f r q) = c 1 + c 1 " $ ' e = p # s 1 + s 1 & d - f r q) = $ 1 ' - f # 1& r q)! Jacobian matrix J r q) = " f rq) " q = # -s + s ) -s & 1 1 1 $ c 1 + c 1 c 1 '! Newton versus Gradient iteration det J r q) q k+1 = q k + J -1 r q k ) 1 " c 1 s 1 $ ' s #-c 1 + c 1 ) -s 1 + s 1 )& # " -s + s ) c + c & 1 1 1 1 $ -s 1 c 1 ' J r T q k ) " 1- c 1 + c 1 ) $ ' # 1- s 1 + s 1 )& Robotics 1 3 q=q k q=q k e k q=q k

Error function! R robot with l1=l=1, desired end-effector position pd = 1,1) e = pd - frq) plot of!e! as a function of q = q1,q) Robotics 1 two local minima inverse kinematic solutions) 33

Error reduction by Gradient method!! flow of iterations along the negative or anti-) gradient two possible cases: convergence or stuck at zero gradient) start one solution. local maximum stop if this is the initial guess). saddle point stop after some iterations) another start......the other solution q1,q) =0,!/) q1,q) =!/,-!/) Robotics 1 q1,q)max =-3!/4,0) q1,q)saddle =!/4,0) e! KerJrT)! 34

Convergence analysis when does the gradient method get stuck?! lack of convergence occurs when! the Jacobian matrix J r q) is singular the robot is in a singular configuration )! AND the error is in the null space of J rt q) " p d = $ 1 ' # 1& p d p # e =p d "p = 1- & $ 1- ' q q 1 p d " J T r q) = -s + s ) c + c " 1 1 1 1 $ ' = # -s 1 c $ ' 1 & q=q saddle # & q 1,q ) saddle =!/4,0) e! KerJ rt ) p e! KerJ rt ) q 1,q ) max =-3!/4,0) p d e KerJ rt )!! p the algorithm will proceed in this case, moving out of the singularity q 1,q ) =!/9,0) Robotics 1 35

! initial guess q 0 Issues in implementation! only one inverse solution is generated for each guess! multiple initializations for obtaining other solutions! optimal step size * in Gradient method! a constant step may work good initially, but not close to the solution or vice versa)! an adaptive one-dimensional line search e.g., Armijo s rule) could be used to choose the best * at each iteration! stopping criteria Cartesian error possibly, separate for position and orientation)!r d - fq k )! #! understanding closeness to singularities algorithm increment!q k+1 -q k! # q $ min {Jq k )} & $ 0 numerical conditioning of Jacobian matrix SVD) or a simpler test on its determinant, for m=n) Robotics 1 36

Numerical tests on RRP robot! RRP/polar robot: desired E-E position r d = p d = 1, 1,1) see slide 0, with d 1 =0.5! the two known) analytical solutions, with q 3 & 0, are: q * = 0.7854, 0.3398, 1.5) q ** = q 1* +, + q *, q 3* ) = -.356,.8018, 1.5)! norms # = 10-5 max Cartesian error), # q =10-6 min joint increment)! k max =15 max # iterations), detj r ) 10-4 closeness to singularity)! numerical performance of Gradient with different steps *) vs. Newton! test 1: q 0 = 0, 0, 1) as initial guess! test : q 0 = -+/4, +/, 1) singular start, since c =0 see slide 0)! test 3: q 0 = 0, +/, 0) double singular start, since also q 3 =0! solution and plots with Matlab code Robotics 1 37

Numerical test! -1 test 1: q0 = 0, 0, 1) as initial guess; evolution of error norm Gradient: * = 0.5 Gradient: * = 1 too large, oscillates around solution slow, 15 max) iterations good, converges in 11 iterations 0.57䏙10-5 Newton very fast, converges in 5 iterations Gradient: * = 0.7 Cartesian errors component-wise ex ey ez Robotics 1 0.15䏙10-8 38

Numerical test - 1! test 1: q 0 = 0, 0, 1) as initial guess; evolution of joint variables Gradient: * = 1 Gradient: * = 0.7 not converging to a solution converges in 11 iterations Newton converges in 5 iterations both to the same solution q * = 0.7854, 0.3398, 1.5) Robotics 1 39

Numerical test -! test : q 0 = -+/4, +/, 1): singular start error norms Gradient * = 0.7 Newton with check of singularity: blocked at start without check: it diverges! starts toward solution, but slowly stops in singularity): when Cartesian error vector e KerJ rt ) joint variables!! Robotics 1 40

Numerical test - 3! test 3: q 0 = 0, +/, 0): double singular start error norm 0.96 10-5 Gradient with * = 0.7) starts toward solution exits the double singularity slowly converges in 19 iterations to the solution q * =0.7854, 0.3398, 1.5) Newton is either blocked at start or w/o check) explodes! NaN in Matlab Cartesian errors joint variables Robotics 1 41

Final remarks! an efficient iterative scheme can be devised by combining! initial iterations using Gradient sure but slow, linear convergence rate)! switch then to Newton method quadratic terminal convergence rate)! joint range limits are considered only at the end! check if the solution found is feasible, as for analytical methods! in alternative, an optimization criterion can be included in the search! driving iterations toward an inverse kinematic solution with nicer properties! if the problem has to be solved on-line! execute iterations and associate an actual robot motion: repeat steps at times t 0, t 1 =t 0 +T,..., t k =t k-1 +T e.g., every T=40 ms)! the good choice for the initial guess q 0 at t k is the solution of the previous problem at t k-1 provides continuity, needs only 1- Newton iterations)! crossing of singularities/handling of joint range limits need special care! Jacobian-based inversion schemes are used also for kinematic control, along a continuous task trajectory r d t) Robotics 1 4