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

2. 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

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

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

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

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

8. 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

9. 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

10. 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

11. 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

12. 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

13. 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

14. 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

15. 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

16. 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.

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

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

19. 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?

20. 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 .

21. 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=0, 𝑠 𝑐 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

22. 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)?

23. 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

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

25. 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

26. 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

27. 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

28. 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

29. 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

30. 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 ) 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(𝑎,𝑏)
Chart 1
Chart 2
𝑞 1
Solution manifold

31. 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

32. The Jacobian MatriX

33. 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

34. 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

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

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

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

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

39. Multi-dimensional analogue of Taylor expansion
1D Taylor expansion:
𝑓 𝑥+Δ𝑥 =𝑓 𝑥 + 𝑓 ′ 𝑥 Δx 𝑓 ′′ 𝑥 Δ x ⋅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)

40. 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
𝑥 ′ − 𝑥 = 𝑓 𝑞 +𝜀 𝑒 𝑖 −𝑓( 𝑞 ) ≈ 𝜀𝐽 𝑞 𝑒 𝑖 =𝜀 𝐽 𝑞 𝑒 𝑖

41. 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

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

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

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

45. 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

46. 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

47. 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

48. 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 𝜔

49. 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

50. Readings
Principles, Appendix C.5