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

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

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

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

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

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

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

Newton-Raphson method

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)

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)

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

Newton-Raphson method

x Divergence x1x1 f(x)

x Divergence x1x1 x2x2 f(x)

x Divergence x1x1 x2x2 x3x3 f(x)

x Divergence x1x1 x2x2 x3x3 x4x4 f(x)

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

Newton-Raphson method

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

Gradient-descent methods

Relation to Newton-Raphson

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

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

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

Higher order optimization methods

Many local minima: need good initialization, or random restarts

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

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

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

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