10/18/2022

Inverse kinematics
(cont.)

Puma Robot 560

Different Configuration

Link Coordinate Parameters

PUMA 560 robot arm link coordinate parameters

1
 10/18/2022

Example 3: Puma 560

Example for Puma 560

2
 10/18/2022

• where

3
 10/18/2022

We have

• So

10

• We obtain

11

Or

12

4
 10/18/2022

Solve:

13

• Same:

14

We obtain

15

5
 10/18/2022

• We get

16

Example 2: Two-link Structure

• Two links connected by rotational joints

θ2
l2
l1

θ1 X=(x,y)
End Effector
Base

17

Example: Two-link Structure

• Animator specifies the joint angles: θ1θ2
• Computer finds the position of end-effector: x

θ2
l2
l1

θ1 X=(x,y)
End Effector
Base (0,0)

x=f(θ1, θ2)

18

6
 10/18/2022

F.K: Two-link Structure

• Animator specifies the joint angles: θ1θ2
• Computer finds the position of end-effector: x

θ2
l2
l1

θ1 X=(x,y)
End Effector
Base (0,0)
x = (l1cosθ1+l2cos(θ2+ θ2)
y = l1sinθ1+l2sin(θ2+ θ2))

19

Inverse Kinematics
• What if an animator specifies position of end-
effector?

θ2
l2
l1
End Effector
θ1 X=(x,y)

Base (0,0)

20

Inverse Kinematics
• Animator specifies the position of end-effector: x
• Computer finds joint angles: θ1θ2

θ2
l2
l1
End Effector
θ1 X=(x,y)

Base (0,0)

(θ1, θ2)=f-1(x)

21

7
 10/18/2022

Inverse Kinematics
• Animator specifies the position of end-effector: x
• Computer finds joint angles: θ1θ2

θ2
l2
l1
End Effector
θ1 X=(x,y)

Base (0,0)

22

Why Inverse Kinematics?

• Motion capture
• Basic tools in character animation
- key frame generation
- animation control
- interactive manipulation
• Computer vision (video-based mocap)
• Robotics
• Bioinfomatics (Protein Inverse Kinematics)
• Etc.

23

Inverse Kinematics
• Given end effector’s positions, compute
required joint angles

• In simple case, analytic solution exists

– Use trig, geometry, and algebra to solve

24

8
 10/18/2022

Inverse Kinematics
• Analytical solution only works for a fairly
simple structure

• Numerical/iterative solution needed for a

complex structure

25

Numerical Approaches
• Inverse kinematics can be formulated as
an optimization problem

26

Function Optimization
• Finding the minimum for nonlinear
functions

F(x)

27

9
 10/18/2022

Optimization As a Tool
• How to use optimization to solve the
following linear system?
3x + 2y = 5;
4x – 5y = 6

28

Optimization As a Tool
• How to use optimization to solve the
following linear system?
3x + 2y = 5;
4x – 5y = 6

Define a function:
F(x,y) = (3x+2y-5)2+(4x-5y-6)2

29

Optimization As a Tool
• How to use optimization to solve the
following linear system?
3x + 2y = 5;
Could be nonlinear equations!
4x – 5y = 6

Define a function:
F(x,y) = (3x+2y-5)2+(4x-5y-6)2

Finding the minimum for F(x,y) :

x+,y+= argminx,y (3x+2y-5)2+(4x-5y-6)2

30

10
 10/18/2022

Optimization As a Tool
• How to use optimization to solve the
following linear system?
f(x, y) = 0;
Could be nonlinear equations!
g(x,y) = 0

Define a function:
F(x,y) = f(x,y)2+g(x,y)2

Finding the minimum for F(x,y) :

x+,y+= argminx,y f(x,y)2+g(x,y)2

31

Formulation
• So how to convert the IK process into an
optimization function?

Find the pose satisfying:

θ2
l2
l1

θ1 C=(Cx,Cy)

Base (0,0)

32

Iterative Approaches
Find the joint angles θ that minimizes the distance between
the hypothesized character position and user specified
position

hypothesized specified
position position
θ2
l2
l1

θ1 C=(Cx,Cy)

Base (0,0)

33

11
 10/18/2022

Iterative Approaches
Find the joint angles θ that minimizes the distance between
the hypothesized character position and user specified
position

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

34

Iterative Approaches
Mathematically, we can formulate this as an
optimization problem:

The above problem can be solved by many

nonlinear optimization algorithms:
- Steepest descent
- Gauss-newton
- Levenberg-marquardt, etc

35

Gradient-based Optimization

36

12
 10/18/2022

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles until the solution

converges:

37

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles until the solution

converges:

How can we decide the amount

of update?

38

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles:

39

13
 10/18/2022

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles:

Known!

40

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles:

Taylor series expansion

41

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles:

Taylor series expansion

rearrange

42

14
 10/18/2022

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles:

Taylor series expansion:
linear approximation
rearrange

Can you solve this optimization problem?

43

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles:

Taylor series expansion

rearrange

This is a quadratic function of

44

Gauss-Newton Approach
• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is

zero

45

15
 10/18/2022

Gauss-Newton Approach
• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is

zero

J Δθ b

Linear equation!

46

Gauss-Newton Approach
• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is

zero

J Δθ b

47

Gauss-Newton Approach
• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is

zero

J Δθ b

48

16
 10/18/2022

Jacobian Matrix
Dofs (N)
• Jacobian is a M by N matrix
that relates differential

The number of constraints (M)

changes of θ to changes of C
(Δθ-->Δf)

• Jacobian maps the velocity

in joint space to velocities in
Cartesian space

• Jacobian depends on current

state

49

Jacobian Matrix
• Jacobian maps the velocity in joint angle space to
velocities in Cartesian space

(x,y)

θ2

θ1

50

Jacobian Matrix
• Jacobian maps the velocity in joint angle space to
velocities in Cartesian space

(x,y)
A small change of θ1 and θ2 results in how
much change of end-effector position (x,y)
θ2

θ1

51

17
 10/18/2022

Jacobian Matrix
• Jacobian maps the velocity in joint angle space to
velocities in Cartesian space

(x,y)
A small change of θ1 and θ2 results in how
much change of end-effector’s position (x,y)
θ2

θ1

52

Jacobian Matrix
• Jacobian maps the velocity in joint angle space to
velocities in Cartesian space

(x,y)
A small change of θ1 and θ2 results in how
much change of end-effector position (x,y)
θ2

θ1

53

Jacobian Matrix
• Jacobian maps the velocity in joint angle space to
velocities in Cartesian space

(x,y)
A small change of θ2 results in how much
change of end-effector position f=(x,y)
θ2

θ1

54

18
 10/18/2022

Jacobian Matrix: 2D-link Structure

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

55

Jacobian Matrix: 2D-link Structure

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

56

Jacobian Matrix: 2D-link Structure

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

57

19
 10/18/2022

Jacobian Matrix: 2D-link Structure

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

58

Jacobian Matrix: 2D-link Structure

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

59

Jacobian Matrix: 2D-link Structure

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

60

20
 10/18/2022

Jacobian Matrix: 2D-link Structure

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)

61

Gauss-Newton Approach
Step 1: initialize the joint angles with

Step 2: update the joint angles:

Step size: specified by the user

62

When to stop
Option #1: When the change of solution from previous
iteration to the current one is below a user-specified
threshold.

Option #2: When the number of iteration reaches a

user-specified threshold.

Option #3: either #1 or #2 is satisfied.

63

21
 10/18/2022

Iterative Approaches for IK

Mathematically, we can formulate this as an
optimization problem:

The above problem can be solved by many

nonlinear optimization algorithms:
- Steepest descent
- Gauss-newton
- Levenberg-marquardt, etc

64

C/C++ Optimization Library

• levmar : Levenberg-Marquardt nonlinear least
squares algorithms in C/C+
- Works with/without analytical Jacobian matrix
- Speeds up the optimization process with analytical
Jacobian matrix.

• http://www.ics.forth.gr/~lourakis/levmar/

65

Example 2
• A 2D lamp character

66

66

22
 10/18/2022

Example 2
• A 2D lamp character

67

67

Forward Kinematics
• A 2D lamp character

Given , , how to compute the global position

of the point A? 68

68

Forward Kinematics
• A 2D lamp with 6 degrees of freedom
lower arm

middle arm

?
Upper arm

base

69

23
 10/18/2022

Inverse Kinematics
• A 2D lamp with 6 degrees of freedom
lower arm

middle arm

Upper arm If you do inverse kinematics with

position of this point

base What’s the size of Jacobian matrix?

70

Inverse Kinematics
• A 2D lamp with 6 degrees of freedom
lower arm

middle arm

Upper arm If you do inverse kinematics with

position of this point

base What’s the size of Jacobian matrix?

2-by-6 matrix

71

Human Characters

72

72

24
 10/18/2022

Inverse Kinematics
• Analytical solution only works for a fairly
simple structure

• Iterative approach needed for a complex

structure

73

Inverse Kinematics
• Is the solution unique?
• Is there always a good solution?

74

Ambiguity of IK
• Multiple solutions

75

25
 10/18/2022

Ambiguity of IK
• Infinite solutions

76

Failures of IK
• Solution may not exist

77

Inverse Kinematics
• Generally ill-posed problem when the number of
Dofs is higher than the number of constraints

78

26
 10/18/2022

Inverse Kinematics
• Generally ill-posed problem when the number of
Dofs is higher than the number of constraints

• Additional objective
– Minimal Change from a reference pose θ0

79

Inverse Kinematics
• Generally ill-posed problem when the number of
Dofs is higher than the number of constraints

• Additional objective
– Minimal Change from a reference pose θ0

80

Inverse Kinematics
• Generally ill-posed problem when the number of
Dofs is higher than the number of constraints

• Additional objective
– Minimal Change from a reference pose θ0

Satisfy the constraints Minimize the difference between the

solution and a reference pose

81

27
 10/18/2022

Inverse Kinematics
• Generally ill-posed problem when the number of
Dofs is higher than the number of constraints

• Additional objective
– Minimal Change from a reference pose θ0
– Naturalness g(θ) (particularly for human characters)

82

Inverse Kinematics
• Generally ill-posed problem when the number of
Dofs is higher than the number of constraints

• Additional objective
– Minimal Change from a reference pose θ0
– Naturalness g(θ) (particularly for human characters)

83

Inverse Kinematics
• Generally ill-posed problem when the number of
Dofs is higher than the number of constraints

• Additional objective
– Minimal Change from a reference pose θ0
– Naturalness g(θ) (particularly for human characters)

Satisfy the constraints How natural is the solution pose?

84

28
 10/18/2022

Summary of IK
• Very simple structure allows an analytic
solution

• Most of complex articulated figures

requires a numerical solution

• May not always get the “right” solution

- need additional objectives

85

Support Materials

86

3-link 2D arm

Jacobian inverse is underdetermined

L3 q 3

L2
q2 y
L1
q1
x

87

87

29
 10/18/2022

Recall least square problem

Projection onto a Space
• Ax=b may not have
solution (if b is not in
C(A)) b

• Solve instead e=b–p

a2
where p is the p
projection of b onto
C(A) a1
Summary

88

88

Projection (cont)

Known as the “normal equation”

P: projection matrix

89

89

Extra material

Jacobian transpose
Constraint dynamics

90

30
 10/18/2022

Ref: all-about-taylor.ppt
Statement of the IK Problem
g x(q)
Want *

Error at ith iteration

i i

Seek correction Dq to fix the error q: joint angles

g=

Omit high order terms…

Jacobian matrix
Note: here xRn, qRm, Jnm
Dq are in m: joint space dimension
radians! n: space where end effector91is in

91

Error at End Effector

c: current, d: destination

92

92

IK General (Jacobian Transpose)

pc

L2
q2 y
L1 pd
q1
x

93

93

31
 10/18/2022

Jacobian Transpose
L2
q2 y
L1
q1
x

94

94

Principle
. .

95

95

96

96

32
 10/18/2022

97

97

98

98

Differences

99

99

33
 10/18/2022

P2
[Constraint Dynamics]
L2
q2
P1
L1
q1

100

100

Properties of Pseudo Inverse A+ (From

Wikipedia)

101

101

34