1. Numerical Methods for Inverse Kinematics Kris Hauser ECE 383 / ME 442

2. Agenda Numerical methods for IK Root finding Optimization Other IK objective functions Axis constraints Hinge constraints Jacobians and null space motions

3. Inverse Kinematics Problem: given desired position of point on robot (end effector), what joint angles reach it?

4. Solving a general equation Solve f(q) = 0 (vector valued nonlinear function) Can include rotation constraints, multiple IK targets q1q1 q2q2 q3q3 q4q4

5. Numerical Inverse Kinematics: General Idea For the root finding problem f(q)=0, given a seed q 0 Take steps q 1,…,q k s.t. the error ||f(q)|| shrinks towards 0 Stop when error is “small enough” or progress stops

6. 3 numerical methods (of many) Cyclic Coordinate Descent Newton-Raphson root finding method Gradient descent & related methods

7. Cyclic Coordinate Descent method On each iteration, solve min ||f(q)|| for a single joint Derive fast analytical solutions for each joint Loop through joints, repeat until convergence

8. Newton-Raphson method

9. x Newton’s method In a neighborhood of a root, the line tangent to the graph crosses the x axis near the root x0x0 f(x)

10. x In a neighborhood of a root, the line tangent to the graph crosses the x axis near the root… iterate! x1x1 Newton’s method f(x)

11. x In a neighborhood of a root, the line tangent to the graph crosses the x axis near the root… iterate! x2x2 Newton’s method

12. Newton-Raphson method

13. x Divergence x1x1 f(x)

14. x Divergence x1x1 x2x2 f(x)

15. x Divergence x1x1 x2x2 x3x3 f(x)

16. x Divergence x1x1 x2x2 x3x3 x4x4 f(x)

17. x Figure 11 x1x1 x2x2 x3x3 x4x4 x5x5 f(x)

18. Newton-Raphson method

19. Pseudoinverses A pseudoinverse A # of an m x n matrix A is an n x m matrix such that: A # AA # =A # AA # A=A If A is square and has an inverse, A # is A -1 Can derive a pseudoinverse for all matrices Typically we talk about the Moore Penrose pseudoinverse A + which is a unique matrix with the properties: If A is underconstrained, x=A + b is the solution to Ax=b that has minimum norm ||x|| If A is overconstrained, x=A + b is the solution that minimizes the norm of ||Ax-b|| Can be calculated in all major math packages, typically via the Singular Value Decomposition A=UWV T

20. Gradient-descent methods

30. Relation to Newton-Raphson

31. Line search: pick step size to lead to decrease in function value

32. (Use your favorite univariate optimization method)  f(x-  f(x)) 

33. Gradient Descent Pseudocode Input: f, starting value x 1, termination tolerances For t=1,2,…,maxIters: Compute the search direction d t = -  f(x t ) If ||d t ||< ε g then: return “Converged to critical point”, output x t Find  t so that f(x t +  t d t ) < f(x t ) using line search If ||  t d t ||< ε x then: return “Converged in x”, output x t Let x t+1 = x t +  t d t Return “Max number of iterations reached”, output x maxIters

39. Higher order optimization methods

40. Many local minima: need good initialization, or random restarts

41. Null Space A motion from q->q’ that maintains f(q)=0 is known as a motion in the null space.

42. Null Space Null space velocity dq must satisfy J(q)dq = 0 => dq lies in the null space of J(q) For any vector y, (I-J + J)y lies in the null space Recall J + is the pseudoinverse of J A basis of the null space can be found by SVD J = UWV T Let the last k diagonal entries of W be 0, first n-k nonzero WV T dq = 0 First n-k entries of V T dq must be zero Last k entries of V T dq may be non zero => Last k columns of V are a basis for null space

43. Recap Two general ways of solving inverse kinematics: analytical and numerical With nonredundant manipulators, there are a finite number of solutions (usually > 1, without joint limits) With redundant manipulators, there are an infinite number of solutions Null space motions

44. Next class Klamp’t lab Robots, objects, and worlds Configurations Forward kinematics Inverse kinematics Reading: Klamp't inverse kinematics tutorialKlamp't inverse kinematics tutorial Bring your laptop