Inverse Kinematics, Jacobians Kris Hauser ECE 383 / ME 442 Fall 2015 1

Articulated Robot Robot: usually a rigid articulated structure Geometric CAD models, relative to reference frames A configuration specifies the placement of those frames (forward kinematics) q1 q2

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

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

Inverse Kinematics Bring point xn on link n to target xT Find q s.t. xT = Tn(q)xn q4 q2 q3 q1

Solving a general equation Solve f(q) = 0 (vector valued nonlinear function) Can include rotation constraints, multiple IK targets q4 q2 q3 q1

Two Approaches for IK Analytical. Write out the equation in terms of q and “invert” it Sometimes there are simple solutions for certain kinematic structures (e.g., industrial robots). If so, computation is fast. Numerical. Iteratively move q to a solution point f(q)=0 General purpose Can incorporate joint limits easily May fall into local minima

Analytical endpoint positioning for a 2R robot arm Without loss of generality, let joint 2 and E.E. position lie on x axis at the reference frame, joint 1 at origin x(q)-xD = 0 L2 L1 xD q2 q1

Analytical endpoint positioning for a 2R robot arm Without loss of generality (why?), let joint 2 and E.E. position lie on x axis at the reference frame, joint 1 at origin x(q)-xD = 0 L2 ||x(q)||2 = ||xD||2 (lhs depends only on q2) ||x(q)||2 = (L1 + c2L2)2 + (s2L2)2 = L12 + 2c2L2L1 + c22L22 + s22L22 = L12 + L22 + 2c2L2L1 L1 xD q2 q1

Analytical endpoint positioning for a 2R robot arm Without loss of generality, let joint 2 and E.E. position lie on x axis at the reference frame, joint 1 at origin x(q)-xD = 0 L2 ||x(q)||2 = ||xD||2 (lhs depends only on q2) ||x(q)||2 = (L1 + c2L2)2 + (s2L2)2 = L12 + 2c2L2L1 + c22L22 + s22L22 = L12 + L22 + 2c2L2L1 c2 = (||xD||2 - L12 - L22)/(2L2L1) If rhs > 1, xD is out of reach If rhs < -1, xD is inside “inner circle” L1 xD q2 q1 c2(q2) Elbow up Elbow down

Analytical endpoint positioning for a 2R robot arm Without loss of generality, let joint 2 and E.E. position lie on x axis at the reference frame, joint 1 at origin x(q)-xD = 0 q2up ||x(q)||2 = ||xD||2 (lhs depends only on q2) ||x(q)||2 = (L1 + c2L2)2 + (s2L2)2 = L12 + 2c2L2L1 + c22L22 + s22L22 = L12 + L22 + 2c2L2L1 c2 = (||xD||2 - L12 - L22)/(2L2L1) If rhs > 1, xD is out of reach If rhs < -1, xD is inside “inner circle” L1 xD q2down q1 c2(q2) Elbow up Elbow down

Analytical endpoint positioning for a 2R robot arm Without loss of generality, let joint 2 and E.E. position lie on x axis at the reference frame, joint 1 at origin x(q)-xD = 0 q2 Once q2 is found, consider angle θD of xD w.r.t origin L1 xD q1

Analytical endpoint positioning for a 2R robot arm Without loss of generality, let joint 2 and E.E. position lie on x axis at the reference frame, joint 1 at origin x(q)-xD = 0 q2 Once q2 is found, consider angle θD of xD w.r.t origin Compute angle θ of E.E. at q=(0,q2) L1 xD q1

Analytical endpoint positioning for a 2R robot arm Without loss of generality, let joint 2 and E.E. position lie on x axis at the reference frame, joint 1 at origin x(q)-xD = 0 Once q2 is found, consider angle θD of xD w.r.t origin Compute angle θ of E.E. at q=(0,q2) Let q1 = θD-θ Done! L1 xD q1 q2

Analytical positioning and orienting for a 3R robot arm Left as an exercise Hint: consider solution in prior slides for choosing the first two joint angles so that the third joint is located at the correct location. Then pick the third.

Spherical robot Consider the 2RP manipulator -> The origin is at the center of the three intersecting axes 𝑇 1,𝑙𝑜𝑐𝑎𝑙 𝑞 1 = 𝑅 𝑧 𝑞 1 0 0 1 𝑇 2,𝑙𝑜𝑐𝑎𝑙 𝑞 2 = 𝑅 𝑦 𝑞 2 0 0 1 𝑇 3,𝑙𝑜𝑐𝑎𝑙 𝑞 3 = 𝐼 𝑒 1 𝑞 3 0 1 𝑇 3 𝑞 = 𝑅 𝑧 𝑞 1 𝑅 𝑦 𝑞 2 𝑅 𝑧 𝑞 1 𝑅 𝑦 𝑞 2 𝑒 1 𝑞 3 0 1

Spherical robot Consider the 2RP manipulator -> Workspace: end effector position 𝑥 = 𝑅 𝑧 𝑞 1 𝑅 𝑦 𝑞 2 𝑒 1 𝑞 3 = 𝑐 1 − 𝑠 1 0 𝑠 1 𝑐 1 0 0 0 1 𝑐 2 0 𝑠 2 0 1 0 − 𝑠 2 0 𝑐 2 𝑞 3 0 0 = 𝑐 1 − 𝑠 1 0 𝑠 1 𝑐 1 0 0 0 1 𝑐 2 𝑞 3 0 − 𝑠 2 𝑞 3 = 𝑐 1 𝑐 2 𝑞 3 𝑠 1 𝑐 2 𝑞 3 − 𝑠 2 𝑞 3

Spherical robot Consider the 2RP manipulator -> Workspace: end effector position 𝑥 = 𝑐 1 𝑐 2 𝑞 3 𝑠 1 𝑐 2 𝑞 3 − 𝑠 2 𝑞 3 Geometric approach: 𝑥 = 𝑞 3 𝑠 2 =− 𝑥 3 / 𝑞 3 implies 𝑞 2 =± sin −1 (− 𝑥 3 / 𝑞 3 ) if 𝑞 3 ≠0 𝑞 1 =𝑎𝑡𝑎𝑛2( 𝑥 2 / 𝑐 2 , 𝑥 1 / 𝑐 2 ) if 𝑐 2 ≠0 What happens when 𝑞 3 =0 or 𝑐 2 =0?

6R Industrial Robot Common 6R industrial robot -> The origin is at the point of intersection of the first 3 axes Workspace is end effector translation 𝑡 and orientation 𝑅 Given 𝑡 and 𝑅 we can determine where 𝑇 6 (𝑞) needs to be (why?) 𝑞 4 is the only joint that affects the distance between origin and 𝑇 6 , compute using elbow formula for planar 2R robot 𝑞 1 , 𝑞 2 , and 𝑞 3 allow unrestricted orientation, so focus on 𝑞 5 and 𝑞 6 . Treating 𝑇 6 as center, choose them to match 4,5,6 with local coordinates of origin 𝑇 6 −1 ⋅ 0 (3D, 2R robot) Determine 𝑇 6→3 𝑞 4 , 𝑞 5 , 𝑞 6 , and find the required 𝑇 3 = 𝑇 6→3 𝑇 6 . Find 𝑞 1 , 𝑞 2 , 𝑞 3 to match 𝑇 3 .

Simultaneous polynomial equation method Consider 2RP manipulator constraints 𝑐 1 𝑐 2 𝑞 3 −𝑥 𝑠 1 𝑐 2 𝑞 3 −𝑦 − 𝑠 2 𝑞 3 −𝑧 =0 Consider variables 𝑐 1 , 𝑠 1 , 𝑐 2 , 𝑠 2 , 𝑞 3 related by constraints s 1 2 + 𝑐 1 2 −1=0, 𝑠 2 2 + 𝑐 2 2 −1=0 5 simultaneous polynomial equations of degree at most 3 Start eliminating variables (like Gaussian elimination)… ultimately get a degree 4 polynomial in 1 variable

Comments on Analytical IK Most 6DOF robots encountered in practice are designed to have convenient analytical IK solutions (e.g., 3 intersecting orthogonal axes) General methods for 6DOF robots require solving a high-degree (16) polynomial, which is computationally expensive and suffers from numerical difficulties What about a redundant manipulator (> 6 DOF)?

Multiplicity issue Let n = # of DOFs, m = # of constraints Roughly, in common cases If n < m, there will be 0 IK solutions If n = m, there may be 1 or more solution If n > m, there will either be 0 or infinite number of solutions (redundant robot) Singularities: the uncommon case

Redundant robots n degrees of freedom > m workspace dimensions E.g. 4R with 2D position constraint

Redundant robots The infinity of solutions can often be parameterized by choosing n-m independent joints with the remainder m joints being dependent. Independent joints can be varied freely with the dependent joints “following along” 𝑞 2 𝑞 1

Redundant robots The infinity of solutions can often be parameterized by choosing n-m independent joints with the remainder m joints being dependent. Independent joints can be varied freely with the dependent joints “following along” 𝑞 2 𝑞 1

Multiple solutions => different charts of solution manifold Redundant robots The infinity of solutions can often be parameterized by choosing n-m independent joints with the remainder m joints being dependent. Independent joints can be varied freely with the dependent joints “following along” 𝑞 2 𝑞 1 Multiple solutions => different charts of solution manifold

Simple example: planar 2R, constrained to vertical line Constraint: 𝑥= 𝐿 1 cos 𝑞 1 + 𝐿 2 cos 𝑞 1 + 𝑞 2 L2 L1 We can choose 𝑞 1 somewhat freely, (1 independent joint). So, 𝑞 2 =± cos −1 𝑥− 𝐿 1 cos 𝑞 1 𝐿 2 − 𝑞 1 q2 q1 x 𝑞 2 Chart 1 Chart 2 𝑞 1 Solution manifold

Simple example: planar 2R, constrained to vertical line Constraint: 𝑥= 𝐿 1 cos 𝑞 1 + 𝐿 2 cos 𝑞 1 + 𝑞 2 L2 L1 We can choose 𝑞 1 somewhat freely, (1 independent joint). So, 𝑞 2 =± cos −1 𝑥− 𝐿 1 cos 𝑞 1 𝐿 2 − 𝑞 1 q2 q1 x 𝑞 2 Domain of 𝑞 1 : for a solution to exist, we need −1≤ 𝑥− 𝐿 1 cos 𝑞 1 𝐿 2 ≤1 => − 𝐿 2 ≤𝑥− 𝐿 1 cos 𝑞 1 ≤ 𝐿 2 => − 𝐿 2 −𝑥≤− 𝐿 1 cos 𝑞 1 ≤ 𝐿 2 −𝑥 => 𝑥− 𝐿 2 𝐿 1 ≤ cos 𝑞 1 ≤ 𝑥+ 𝐿 2 𝐿 1 Chart 1 Chart 2 𝑞 1 Solution manifold

Simple example: planar 2R, constrained to vertical line Constraint: 𝑥= 𝐿 1 cos 𝑞 1 + 𝐿 2 cos 𝑞 1 + 𝑞 2 L2 L1 We could have chosen 𝑞 2 as the independent joint! So, 𝑥= 𝐿 1 + 𝐿 2 𝑐 2 𝑐 1 − 𝐿 2 𝑠 2 𝑠 1 And from box, we have 𝑞 1 =± cos −1 𝑥 𝐿 1 2 + 𝐿 2 2 +2 𝐿 1 𝐿 2 𝑐 2 +𝑎𝑡𝑎𝑛2(− 𝐿 2 𝑠 2 , 𝐿 1 + 𝐿 2 𝑐 2 ) q2 q1 x 𝑞 2 General solution to 𝑎 sin 𝜃 + 𝑏 cos 𝜃 = 𝑐 Consider cos (𝜃−𝑧) = cos 𝜃 cos 𝑧 + sin 𝜃 sin 𝑧 Normalize coefficients 𝑎 𝑎 2 + 𝑏 2 sin 𝜃 + 𝑏 𝑎 2 + 𝑏 2 cos 𝜃 = 𝑐 𝑎 2 + 𝑏 2 Solve for 𝑧=𝑎𝑡𝑎𝑛2(𝑎,𝑏) cos 𝜃+𝑧 = 𝑐 𝑎 2 + 𝑏 2 𝜃=± cos −1 𝑐 𝑎 2 + 𝑏 2 +𝑎𝑡𝑎𝑛2(𝑎,𝑏) Chart 1 Chart 2 𝑞 1 Solution manifold

Parallel robots Only some joints are actuated (active) Closed chain constraints imply that passive joints move in response to motions of active joints Active : passive :: independent : dependent Active joints should be set up so passive joints have nonredundant IK solutions, given positions of active joints Problems at singularities! Avoid them (e.g., using joint stops) 24 passive revolute joints 6 actuated prismatic joints

The Jacobian MatriX

Jacobian matrix What if we want to: The Jacobian comes to our rescue Relate joint velocities to workspace velocities? Estimate the effect of joint positioning accuracy to workspace accuracy? Obtain IK solutions for arbitrary devices? The Jacobian comes to our rescue

The Jacobian Matrix The Jacobian of a function 𝑥 = 𝑓( 𝑞 ), with 𝑥 ∈ 𝑅 𝑚 and 𝑞 ∈ 𝑅 𝑛 is the m x n matrix of partial derivatives Typically written 𝐽( 𝑞 ) (Note the dependence on q) f1/q1 … f1/qn … … fm/q1 … fm/qn

Single Joint Robot Example (x,y) 𝑥 𝑦 𝑞 = 𝐿 cos 𝑞 𝐿 sin 𝑞 L q

Single Joint Robot Example (x,y) 𝑥 𝑦 𝑞 = 𝐿 cos 𝑞 𝐿 sin 𝑞 L q 𝐽 𝑞 = 𝜕𝑥/𝜕𝑞 𝜕𝑦/𝜕𝑞 𝑞 = −𝐿 sin 𝑞 𝐿 cos 𝑞

Single Joint Robot Example (x,y) 𝐽 𝑞 𝑥 𝑦 𝑞 = 𝐿 cos 𝑞 𝐿 sin 𝑞 L q 𝐽 𝑞 = 𝜕𝑥/𝜕𝑞 𝜕𝑦/𝜕𝑞 𝑞 = −𝐿 sin 𝑞 𝐿 cos 𝑞

Significance Let 𝑥 be an end effector position. Multiplying J(q) with a joint velocity vector 𝑞 gives the end effector velocity 𝑥 =𝐽(𝑞) 𝑞 𝐽 𝑞 𝑥 𝑦 =𝐽 𝑞 𝑞 𝑥 𝑦 =𝐽 𝑞 𝑞 (x,y) 𝐽 𝑞 𝑞 L 𝑞 q

Multi-dimensional analogue of Taylor expansion 1D Taylor expansion: 𝑓 𝑥+Δ𝑥 =𝑓 𝑥 + 𝑓 ′ 𝑥 Δx+ 1 2 𝑓 ′′ 𝑥 Δ x 2 + 1 2⋅3 f ′′′ x Δ x 3 +… First order approximation for small Δ𝑥: 𝑓 𝑥+𝛥𝑥 ≈𝑓 𝑥 + 𝑓 ′ 𝑥 Δx+O(Δ 𝑥 2 ) N-D First order Taylor expansion: 𝑓 𝑥 +Δ 𝑥 ≈𝑓 𝑥 +𝐽( 𝑥 )Δ 𝑥 +O( Δ𝑥 2 ) (approximations assume 1st and 2nd derivatives exist and are bounded)

Relating joint accuracy to workspace accuracy Assume joint i is disturbed by a (small) amount ±𝜀. Let’s ask how far the disturbed position 𝑥 ′ might lie from the original position Let 𝑒 𝑖 be the vector of all 0’s except for 1 in the i’th entry 𝑥 ′ − 𝑥 = 𝑓 𝑞 +𝜀 𝑒 𝑖 −𝑓( 𝑞 ) ≈ 𝜀𝐽 𝑞 𝑒 𝑖 =𝜀 𝐽 𝑞 𝑒 𝑖

Relating joint accuracy to workspace accuracy Assume joint i is disturbed by a (small) amount ±𝜀. Let’s ask how far the disturbed position 𝑥 ′ might lie from the original position Let 𝑒 𝑖 be the vector of all 0’s except for 1 in the i’th entry 𝑥 ′ − 𝑥 = 𝑓 𝑞 +𝜀 𝑒 𝑖 −𝑓( 𝑞 ) ≈ 𝜀𝐽 𝑞 𝑒 𝑖 =𝜀 𝐽 𝑞 𝑒 𝑖 Principle: errors caused by errors in the i’th joint are proportional to the magnitude of the i’th column of the Jacobian

Computing Jacobians in general How do we compute it? 𝑇 𝑘 𝑞 𝑥 𝑘 = 𝑖∈𝐴 𝑘 𝑇 𝑖→𝑝 𝑖 𝑟𝑒𝑓 𝑅 𝑞 𝑖 𝑥 𝑘 Consider derivative w.r.t. qj 𝜕 𝜕 𝑞 𝑗 𝑇 𝑘 𝑞 𝑥 𝑘 = 𝑇 𝑝 𝑗 𝑞 𝑇 𝑗→𝑝 𝑗 𝑟𝑒𝑓 𝑑 𝑑 𝑞 𝑗 𝑅 𝑞 𝑗 𝑇 𝑘→𝑗 𝑞 𝑥 𝑘

Derivatives in 2D… 𝑅(𝜃) = cos 𝜃 − sin 𝜃 0 sin 𝜃 cos 𝜃 0 0 0 1 𝑑 𝑑𝜃 𝑅 𝜃 = −sin 𝜃 − cos 𝜃 0 cos 𝜃 − sin 𝜃 0 0 0 0 =𝑅(𝜃) 0 −1 0 1 0 0 0 0 0 𝜕 𝜕 𝑞 𝑗 𝑇 𝑘 𝑞 𝑥 𝑘 = 𝑇 𝑗 𝑞 0 −1 0 1 0 0 0 0 0 𝑇 𝑘→𝑗 𝑞 𝑥 𝑘

Derivatives in 2D… 𝑑 𝑑𝜃 𝑅 𝜃 = −sin 𝜃 − cos 𝜃 0 cos 𝜃 sin 𝜃 0 0 0 0 =𝑅(𝜃) 0 −1 0 1 0 0 0 0 0 𝜕 𝜕 𝑞 𝑗 𝑇 𝑘 𝑞 𝑥 𝑘 = 𝑇 𝑗 𝑞 0 −1 0 1 0 0 0 0 0 𝑇 𝑘→𝑗 𝑞 𝑥 𝑘 𝜕 𝜕 𝑞 𝑗 𝑥 T2(q1,q2) T3(q1,..,q3) xk L2 T4(q1,…,q4) T0 L3 T1(q1) L1

Consequences… Column j of position Jacobian Jx(q) is the speed at which x would change if joint j rotated alone Perpendicular and equal in magnitude to vector from x to joint axis Larger when x is farther from the joint axis T2(q1,q2) T3(q1,..,q3) xk L2 T4(q1,…,q4) T0 L3 T1(q1) L1

Consequences At singularities, some columns become linearly dependent => The rank of the Jacobian may change => If 𝑟𝑎𝑛𝑘(𝐽(𝑞)) drops below the workspace dimension, then at 𝑞 the robot cannot instantaneously move in certain workspace dimensions The number of IK solutions changes from the usual case xk L2 T0 L3 L1

Orientation Jacobian, 2D Consider end effector orientation θ(q) in plane All entries of Jθ(q) corresponding to revolute joints are 1! T2(q1,q2) T3(q1,..,q3) xk L2 T4(q1,…,q4) T0 L3 T1(q1) L1

Total Jacobian Total Jacobian J(q) is the matrix formed by stacking Jx(q), Jθ(q) 3 rows in 2D, 6 rows in 3D 3D: Relates joint velocities to translational velocity v, angular velocity 𝜔

In Klamp’t Forward kinematics is automatically computed upon setting a RobotModel’s configuration: world.robot(0).setConfig(q) #updates all link transforms world.robot(0).getLink(10).getTransform() #retrieves the transforms for link 10 Jacobians are available too: world.robot(0).getLink(10).getPositionJacobian(p) #retrieves the 3xn position Jacobian for the point p=[px,py,pz] given in link 10’s local frame world.robot(0).getLink(10).getOrientationJacobian() #retrieves the 3xn orientation Jacobian of link 10 world.robot(0).getLink(10).getJacobian(p) #retrieves the 6xn total Jacobian

Readings Principles, Appendix C.5