1. Inverse Kinematics Set goal configuration of end effector
calculate interior joint angles
Analytic approach – when linkage is simple enough, directly calculate joint angles in configuration that satifies goal
Numeric approach – complex linkages
At each time slice, determine joint movements that take you in direction of goal position (and orientation)

2. Forward Kinematics - review
Articulated linkage – hierarchy of joint-link pairs
Pose – linkage is a specific configuration
Pose Vector – vector of joint angles for linkage
Degrees of Freedom (DoF) – of joint or of whole figure
Types of joints: revolute, prismatic
Tree structure – arcs & nodes
Recursive traversal – concatenate arc matrices
Push current matrix leaving node downward
Pop current matrix traversing back up to node

3. Inverse Kinematics
End Effector L1 q3 q2 L3 L2 q1
Goal

4. Inverse Kinematics Underconstrained – if fewer constraints than DoFs
Many solutions
Overconstrained – too many constraints
No solution
Reachable workspace – volume the end effector can reach
Dextrous workspace – volume end effector can reach in any orientation

5. Inverse Kinematics - Analytic
Given arm configuration (L1, L2, …)
Given desired goal position (and orientation) of end effector: [x,y] or [x,y,z, y1,y2, y3]
Analytically compute goal configuration (q1,q2)
Interpolate pose vector from initial to goal

6. Analytic Inverse Kinematicsq2 L2 L1
Goal
(X,Y) q1

7. Analytic Inverse Kinematics
Multiple solutions
Goal
(X,Y)

8. Analytic Inverse Kinematics
180- q2 L1
(X,Y) q1 qT

9. Analytic Inverse Kinematics
180- q2 L1
(X,Y) q1 Y qT X

10. Law of Cosines C A a B

11. Analytic Inverse Kinematics
(X,Y)
180- q2 q1 qT Y X

12. Analytic Inverse Kinematics

13. Iterative Inverse Kinematics
When linkage is too complex for analytic methods
At each time step, determine changes to joint angles that take the end effector toward goal position and orientation
Need to recompute at each time step

14. Inverse Jacobian Method
d2=EF-J2
End Effector q2
a2 x d2
- Compute instantaneous effect of each joint
- Linear approximation to curvilinear motion
- Find linear combination to take end effector towards goal position

15. Instantaneous linear change in end effector for ith joint
Inverse Jacobian Method
Instantaneous linear change in end effector for ith joint
= (EF - Ji) x ai

16. Inverse Jacobian Method
What is the change in orientation of end effector induced by joint i that has axis of rotation a i
and position Ji?
Angular velocity

17. Inverse Jacobian Method
Solution only valid for an
instantaneous step
Angular affect is really
curved, not straight line
Once a step is taken, need
to recompute solution

18. Inverse Jacobian Method
Set up equations
yi: state variable
xi : system parameter
fi : relate system parameters to state variable
Inverse Jacobian Method
- Mathematics

19. Inverse Jacobian Method - Mathematics
Matrix Form

20. Inverse Jacobian Method - Mathematics
Use chain rule to differentiate equations to relate changes in system parameters to changes in state variables

21. Inverse Jacobian Method - Mathematics
Matrix Form

22. Inverse Jacobian Method
Change in position (and orientation) of end effector
Change in joint angles
Linear approximation that relates change in joint angle to change in end effector position (and orientation)

23. Inverse Jacobian Method

24. Inverse Jacobian Method

25. Inverse Jacobian Method
= (S - J1) x a1
= w1

26. The Matrices

27. The Matrices N DoFs V – desired linear and angular velocities 3x1, 6x1
J – Jacobian
Matrix of partials
3xN, 6xN
N x 1
q – change to joint angles (unknowns)

28. Pseudo Inverse of the Jacobian

29. Solving using the Pseudo Inverse
LU decomposition

30. Adding a Control Term A solution of this form
When put into this formula
Like this
After some manipulation, you can show that it…
…doesn’t affect the desired configuration
But it can be used to bias
The solution vector

31. Form of the Control Term
Desired angles and corresponding gains are input
Bias to desired angles
(not the same as hard joint limits)
‘z’ is H differentiated
Where the deviation is large, you bump up the solution vector in such a way that you don’t disturb the desired effect

32. Some Algebraic Manipulation
Include this in equation
Isolate vector of unknown
Rearrange to isolate the inverse

33. Solving the Equations
LU decomp.

34. Control Term Use to bias to desired mid-angle
Does not enforce joint angles
Does not address “human-like” or “natural” motion
Only kinematic control – no forces involved

35. Other ways to numerically IK
Jacobian transpose
Alternate Jacobian – use goal position
HAL – human arm linkage
Damped Least Squares
CCD

36. Jacobian Transpose
Use projection of effect vector onto desired movement

37. Jacobian Transpose S

38. Jacobian Transpose …

39. Alternate Jacobian G
Use the goal postion instead of the end-effector!!?? !? …

40. Damped Least Squares G
substitution
Solve

41. Hueristic Human-Like Linkage (HAL)
7 DoF linkage
RA (q1,q2 ,q3) , RB ( q4), RC(q5,q6 ,q7)
3 DoF G
Decompose into simpler subproblems
1 DoF
Set hand position and rotation based on relative position of Goal to shoulder
3 DoF
Fix wrist position – use as Goal

42. Hueristic Human-Like Linkage (HAL)
Set q4 based on distance between shoulder and wrist w e
Assume axis of elbow is perpendicular to plane defined by s, e, w use law of cosines

43. Hueristic Human-Like Linkage (HAL)q4 w e
Elbow lies on circle defined by w, s & q4
Determine elbow position based on heuristics
For example:
project forearm straight from hand orientation
if arm intersects torso or a shoulder angle exceeds joint limit (or exceeds comfort zone) –
Clamp to inside of limits

44. Hueristic Human-Like Linkage (HAL)q4 w e
From e and s, determine RA
From e and w and hand orientation, determine RB

45. Cyclic-Coordinate Descent
Traverse linkage from distal joint inwards
Optimally set one joint at a time
Update end effector with each joint change
At each joint, minimize difference between end effector and goal
Easy if only trying to match position; heuristic if orientation too
Use weighted average of position and orientation.

46. Cyclic-Coordinate Descent.

47. Cyclic-Coordinate Descent
Rotational joint: .

48. Cyclic-Coordinate Descent
Rotational joint: .

49. Cyclic-Coordinate Descent
Rotational joint: .

50. Cyclic-Coordinate Descent
Rotational joint: .

51. Cyclic-Coordinate Descent
Translational joint: ..

52. IK w/ constraints Basic idea:
Chris Welman, “Inverse Kinematics and Geometric Constraints for Articulated Figure Manipulation,” M.S. Thesis, Simon Fraser University, 2001.
Basic idea:
Constraints are geometric, e.g., point-to-point, point-to-plan, specific orientation, etc.
Assume starting out in satisfied configuration
Forces are applied to system
Detect, and cancel out, force components that would violate constraints.

53. IK w/ constraints Point-on-a-plane constraint Fa Fc Ft
Given: geometric constraints & applied forces
Determine: what constraints will be violated & what (minimal) forces are needed to counteract the components of the applied forces responsible for the violations.

54. Constraints
To maintain constraints, need:
Notation: .

55. Constraints
IK Jacobian .
Generalized force
Constraint Jacobian

56. example constraint
Geometric constraint on point that is function of pose
Usually sparse .

57. Computing the constraint force
Applied force
Yet to be determined constraint force
To counteract ga’s affect on constraints:
g should lie in the nullspace of JcK .

58. Computing the constraint force
The system is usually underconstrained
Restrict gc to move the system in a direction it may not go
Solve linear system to find Lagrange multiplier vector. .

59. Solving for Lagrange Multipliers
gradient
Shortest distance from A to B passing through a point P
Direction of gradients are equal A
Constrain P to lie on g B g
Points on ellipse are set of points for which sum of distances to foci is equal to some constant

60. Solving for Lagrange Multipliers
Use truncated SVD with backsubstitution on
diagonal
Nullspace basis.
Range basis

61. Solving for Lagrange Multipliers

62. Feedback term
Spring that penalizes deviation from constraints.

63. Implementation Handles on skeletons Point handle orientation handle
Center-of-mass handle
Each handle must know how to its value from q
Each handle must know how to compute the Jacobian.

64. Constraints on handles
Constraining a point handle to a location
Constraining a point handle to a plane
Constraining a point handle to a line
Constraining an orientation handle to an orientation .

65. Dataflow approach Constraint function block Knows how to compute
Its function in term of x
Knows its Jacobian wrt x ..

66. Example network Jc C c1 c2 h1 h2 h3 h4 .. q