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! T = R p 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! solution methods p " r =, or any other task vector! generalized formulation: inverse kinematics for a given value of task variables Robotics 1 3
Solvability and robot workspace! 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! for p " WS 1 and a suitable! (or R) there is at least one solution! secondary (or dexterous) workspace WS : set of positions p that can be reached with any orientation (among those feasible for the robot direct kinematics)! for p " WS there is at least one 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 (! WS for spherical wrist without joint limits) rotating the base joint angle $ 1 Robotics 1 5
Workspace of planar R arm y p orientations l 1 +l l q l 1 -l l 1! if l 1 % l q 1! WS 1 = {p " R : l 1 -l &!p!& l 1 +l }! WS = '! if l 1 = l = "! WS 1 = {p " R :!p!& "} 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
Multiplicity of solutions some examples! E-E positioning of a planar R robot arm! regular solutions in WS 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! 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 9
A planar 3R arm workspace and number/type of inverse solutions y q 3 l1 = l = l 3 = " p l 1 l q l 3 WS 1 = {p " R :!p!& 3"} WS = {p " R :!p!& "} q 1 x any planar orientation is feasible in WS 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 4. if!p!< " : ) 1 regular solutions (never singular) Robotics 1 10
! 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 J(q) Robotics 1 11
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 1
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 13
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 14
y p y. Inverse kinematics of R arm (cont d) q 1 * q p 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 } q 1 q p note: difference of ATAN needs to be re-expressed in (-+, +]! {q 1,q } UP/LEFT {q q 1,q } DOWN/RIGHT q 1 q e q have same absolute value, but opposite signs Robotics 1 15
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 16
Inverse kinematics of polar (RRP) arm q is NOT a DH variable! p z q 3 p x = q 3 c c 1 p y = q 3 c s 1 d 1 q p z = d 1 + q 3 s 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! 17
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 ) / 3 R 6 (q 4, q 5, q 6 ) = 0 R T 3 R / q 4, q 5, q 6 (inverse orientation given known, Euler ZYZ or ZXZ kinematics for wrist) after 1. rotation matrix Robotics 1 18
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 19
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 0
Operation of Newton method! in the scalar case, also known as method of the tangent! for a differentiable function f(x), find a root of f(x)=0 by iterating as x k+1 = x k " f(x k ) f'(x k ) an approximating sequence { x 1,x,x 3,x 4,x 5,...} animation from http://en.wikipedia.org/wiki/file:newtoniteration_ani.gif Robotics 1 1
Numerical solution of inverse kinematics problems (cont d)! Gradient method (max descent)! minimize the error function H(q) = #!r d - f r (q)! = # [r d - f r (q)] T [r d - f r (q)] q k+1 = q k - * 1 q H(q k ) from 1H(q) = - 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
Revisited as a feedback scheme r d + - r e J rt (q) f r (q). q q(0) 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 " Ker(J rt ) in particular e = 0 asymptotic stability Robotics 1 3
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 - f(q 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 4
A case study analytic expressions of Newton and gradient iterations! R robot with l 1 =l =1, desired end-effector position r d = (1,1)! direct kinematic function and error " f r (q) = c 1 + c 1 % " $ ' e = r # s 1 + s 1 & d - f r (q) = $ 1% ' 1 - f # & r (q)! Jacobian matrix J r (q) = " f r(q) " 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 5 q=q k q=q k e k q=q k
Error function! R robot with l1=l=1, desired end-effector position rd = (1,1) e = rd - fr(q) plot of!e! as a function of q = (q1,q) Robotics 1 two local minima (inverse kinematic solutions) 6
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 " Ker(JrT)! 7
! 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 - f(q k )! $ #! understanding closeness to singularities algorithm increment!q k+1 -q k! $ # q $ min {J(q k )} % $ 0 numerical conditioning of Jacobian matrix (SVD) (or a simpler test on its determinant, for m=n) Robotics 1 8
Numerical tests on RRP robot! RRP/polar robot: desired E-E position r d = p d = (1, 1,1) see slide 17, d 1 =0.5! the two (known) analytic 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), det(j r ) $ 10-4 (closeness to singularity)! numerical performance of Gradient (with different *) vs. Newton! test 1: q 0 = (0, 0, 1) as initial guess! test : q 0 = (-+/4, +/, 1) singular start, since c =0 (see slide 17)! test 3: q 0 = (0, +/, 0) double singular start, since also q 3 =0! solution and plots with Matlab code Robotics 1 9
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 30
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 solution q * = (0.7854, 0.3398, 1.5) Robotics 1 31
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 Ker(J rt ) joint variables!! Robotics 1 3
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)!! Cartesian errors joint variables Robotics 1 33
Final remarks! an efficient iterative scheme can be devised by combining! initial iterations with Gradient method ( sure but slow, having linear convergence rate)! switch then to Newton method (quadratic terminal convergence rate)! joint range limits are considered only at the end! check if the found solution is feasible, as for analytical methods! 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 q 0 at t k is the solution of the previous problem at t k-1 (gives continuity, needs only 1- Newton iterations)! crossing of singularities and handling of joint range limits need special care in this case! Jacobian-based inversion schemes are used also for kinematic control, along a continuous task trajectory r d (t) Robotics 1 34