CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

80
CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai
  • date post

    18-Dec-2015
  • Category

    Documents

  • view

    270
  • download

    17

Transcript of CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Page 1: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

CSCE 641: Forward kinematics and inverse

kinematicsJinxiang Chai

Page 2: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Outline

Kinematics– Forward kinematics– Inverse kinematics

Page 3: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Kinematics

The study of movement without the consideration of the masses or forces that bring about the motion

Page 4: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Degrees of freedom (Dofs)

The set of independent displacements that specify an object’s pose

Page 5: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Degrees of freedom (Dofs)

The set of independent displacements that specify an object’s pose

• How many degrees of freedom when flying?

Page 6: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Degrees of freedom (Dofs)

The set of independent displacements that specify an object’s pose

• How many degrees of freedom when flying?

• So the kinematics of this airplane permit movement anywhere in three dimensions

• Six• x, y, and z positions• roll, pitch, and yaw

Page 7: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Degrees of Freedom

• How about this robot arm?

• Six again

• 2-base, 1-shoulder, 1-elbow, 2-wrist

Page 8: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Work Space vs. Configuration Space

• Work space– The space in which the object exists– Dimensionality

• R3 for most things, R2 for planar arms

• Configuration space– The space that defines the possible object

configurations– Degrees of Freedom

• The number of parameters that are necessary and sufficient to define position in configuration

Page 9: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Forward vs. Inverse Kinematics

• Forward Kinematics– Compute configuration (pose) given individual

DOF values

• Inverse Kinematics– Compute individual DOF values that result in

specified end effector position

Page 10: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Example: Two-link Structure

• Two links connected by rotational joints

BaseEnd Effector

θ2

θ1 X=(x,y)

l2l1

Page 11: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Example: Two-link Structure

• Animator specifies the joint angles: θ1θ2

• Computer finds the position of end-effector: x

BaseEnd Effector

θ2

θ1

x=f(θ1, θ2)

l2l1

(0,0)

X=(x,y)

Page 12: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Example: Two-link Structure

• Animator specifies the joint angles: θ1θ2

• Computer finds the position of end-effector: x

BaseEnd Effector

θ2

θ1

x = (l1cosθ1+l2cos(θ2+ θ2)

y = l1sinθ1+l2sin(θ2+ θ2))

(0,0)

θ2 l2l1

X=(x,y)

Page 13: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Forward kinematics

BaseEnd Effector

θ2

θ1

(0,0)

θ2 l2l1

• Create an animation by specifying the joint angle trajectories

θ1

θ2

X=(x,y)

Page 14: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

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

Base

End Effector

θ2

θ1

(0,0)

θ2 l2l1

X=(x,y)

Page 15: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

• Animator specifies the position of end-effector: x

• Computer finds joint angles: θ1θ2

Base

End Effector

θ2

θ1

(0,0)

θ2 l2l1

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

X=(x,y)

Page 16: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

• Animator specifies the position of end-effector: x

• Computer finds joint angles: θ1θ2

Base

End Effector

θ2

θ1

(0,0)

θ2 l2l1

X=(x,y)

Page 17: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Why Inverse Kinematics?

• Basic tools in character animation - key frame generation

- animation control - interactive manipulation

• Computer vision (vision based mocap)• Robotics• Bioinfomatics (Protein Inverse Kinematics)

Page 18: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

• Given end effector position, compute required joint angles

• In simple case, analytic solution exists– Use trig, geometry, and algebra to solve

Page 19: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

• Analytical solution only works for a fairly simple structure

• Iterative approach needed for a complex structure

Page 20: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Iterative approach

• Inverse kinematics can be formulated as an optimization problem

Page 21: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Iterative approaches

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

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(Cx,Cy)

i

ii Cf 2)(minarg

Page 22: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Iterative approaches

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

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

Page 23: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Iterative approach

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

i

ii cf 2)(minarg

Page 24: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

0

kkk 1

Page 25: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

0

kkk 1

How can we decide the amount of update?

Page 26: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

ii

kki cf

2)(minarg

0

kkk 1

Page 27: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

ii

kki cf

2)(minarg

0

kkk 1

Known!

Page 28: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

ii

kki cf

2)(minarg

0

kkk 1

Taylor series expansion

Page 29: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

ii

kki cf

2)(minarg

2

)(minarg

ii

kkik

i cf

f

0

kkk 1

Taylor series expansion

rearrange

Page 30: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

ii

kki cf

2)(minarg

2

)(minarg

ii

kkik

i cf

f

0

kkk 1

Taylor series expansion

rearrange

Can you solve this optimization problem?

Page 31: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

ii

kki cf

2)(minarg

2

)(minarg

ii

kkik

i cf

f

2

))((minarg

i

kii

kki fcf

0

kkk 1

Taylor series expansion

rearrange

This is a quadratic function of

Page 32: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is zero

),...,(

),...,(

),...,(

...

...

1

122

11

2

1

2

2

2

2

1

2

1

2

1

1

1

NiN

N

Ni

N

N

MM

N

M

N

N

fc

fc

fc

fff

fff

fff

2

))((minarg

i

kii

kki fcf

Page 33: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is zero

),...,(

),...,(

),...,(

...

...

1

122

11

2

1

2

2

2

2

1

2

1

2

1

1

1

NiN

N

Ni

N

N

MM

N

M

N

N

fc

fc

fc

fff

fff

fff

2

))((minarg

i

kii

kki fcf

bJ Δθ

Linear equation!

Page 34: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is zero

),...,(

),...,(

),...,(

...

...

1

122

11

2

1

2

2

2

2

1

2

1

2

1

1

1

NiN

N

Ni

N

N

MM

N

M

N

N

fc

fc

fc

fff

fff

fff

2

))((minarg

i

kii

kki fcf

bJ Δθ

Page 35: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

• Optimizing an quadratic function is easy

• It has an optimal value when the gradient is zero

),...,(

),...,(

),...,(

...

...

1

122

11

2

1

2

2

2

2

1

2

1

2

1

1

1

NiN

N

Ni

N

N

MM

N

M

N

N

fc

fc

fc

fff

fff

fff

2

))((minarg

i

kii

kki fcf

bJ Δθ

bJJJ TT 1)(

Page 36: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix

• Jacobian is a M by N matrix that relates differential changes of θ to changes of C

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

• Jacobian depends on current state

N

MMM

N

N

fff

fff

fff

21

2

2

2

1

2

1

2

1

1

1

...

...

Dofs (N)

The

num

ber

of c

onst

rain

ts (

M)

Page 37: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix

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

(x,y)

θ1

θ2

Page 38: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix

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

(x,y)

θ1

θ2

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

Page 39: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix

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

2

1

2

1

1

2

2

1

1

1

ff

ff

y

x

(x,y)

θ1

θ2

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

Page 40: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix: 2D-link structure

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

Page 41: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix: 2D-link structure

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

f1f2

Page 42: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix: 2D-link structure

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

)sin(sin

)cos(cos

212112

212111

llf

llf

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

N

MMM

N

N

fff

fff

fff

21

2

2

2

1

2

1

2

1

1

1

...

...

Page 43: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix: 2D-link structure

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

Page 44: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix: 2D-link structure

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

Page 45: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix: 2D-link structure

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

1

21211

1

1 ))cos(cos(

llf

Page 46: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Jacobian matrix: 2D-link structure

2221211

2121211

,))sin(sin())cos(cos(minarg

21

cllcll

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

2

21211

2

2 ))sin(sin(

llf

Page 47: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

0

kkk 1

Step size: specified by the user

Page 48: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Another Example

• A 2D lamp character

1

2

3

),,( 0yx

3c

2c

1c

0c

A

48

Page 49: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Another Example

• A 2D lamp character3l

A2l

1l

0l

49

Page 50: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Another Example

• A 2D lamp character

1

2

3

3c

2c

1c

A

Given , , how to compute the global position of the point A?

),,,,,( 3210 yx

?

),,( 0yx0c

50

Page 51: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s local coordinate ?

1

2

3

3c

2c

1c

A

?

A

),,( 0yx0c

51

Page 52: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s local coordinate ?

1

2

3

3c

2c

1c

A

?

A

1

03

0

l

p

),,( 0yx0c

03221100 )()0,()()0,()()0,()(),( pRlTRlTRlTRyxTp 52

Page 53: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

?

),,( 0yx0c

1

03

0

l

p

03221100 )()0,()()0,()()0,()(),( pRlTRlTRlTRyxTp

100

0cossin

0sincos

)( 33

33

3

R

53

Page 54: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

?

),,( 0yx0c

1

00

l

p

03221100 )()0,()()0,()()0,()(),( pRlTRlTRlTRyxTp

100

010

01

)0,(2

2

l

lT

54

Page 55: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

?

),,( 0yx0c

1

00

l

p

03221100 )()0,()()0,()()0,()(),( pRlTRlTRlTRyxTp

100

0cossin

0sincos

)( 22

22

2

R

55

Page 56: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

?

),,( 0yx0c

1

00

l

p

03221100 )()0,()()0,()()0,()(),( pRlTRlTRlTRyxTp

100

010

01

)0,(1

1

l

lT

56

Page 57: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

?

),,( 0yx0c

1

00

l

p

03221100 )()0,()()0,()()0,()(),( pRlTRlTRlTRyxTp

100

0cossin

0sincos

)( 11

11

1

R

57

Page 58: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

?

),,( 0yx0c

1

00

l

p

03221100 )()0,()()0,()(),0()(),( pRlTRlTRlTRyxTp

100

10

001

),0( 00 llT

58

Page 59: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

?

03221100 )()0,()()0,()(),0()(),( pRlTRlTRlTRyxTp

),,( 0yx0c

1

00

l

p

100

cossin

sincos

)(),( 00

00

0 y

x

RyxT

59

Page 60: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

03221100 )()0,()()0,()(),0()(),( pRlTRlTRlTRyxTp

),,( 0yx0c

60

Page 61: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

03221100 )()0,()()0,()(),0()(),( pRlTRlTRlTRyxTp

),,( 0yx0c

61

Page 62: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

03221100 )()0,()()0,()(),0()(),( pRlTRlTRlTRyxTp

),,( 0yx0c

62

Page 63: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

03221100 )()0,()()0,()(),0()(),( pRlTRlTRlTRyxTp

),,( 0yx0c

63

Page 64: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Composite 2D Transformation

• What’s the current coordinate A ?

1

2

3

3c

2c

1c

A

03221100 )()0,()()0,()(),0()(),( pRlTRlTRlTRyxTp

),,( 0yx0c

64

Page 65: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

More Complex Characters?

• A 2D lamp with 6 degrees of freedom

1

2

3

3c

2c

1c

0p

032211003210 )()0,()()0,()(),0()(),(),,,,,( pRlTRlTRlTRyxTyxf ),,( 0yx0c

base

Upper arm

lower arm

middle arm

If you do inverse kinematics with position of this point

What’s the size of Jacobian matrix?

Page 66: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

More Complex Characters?

• A 2D lamp with 6 degrees of freedom

1

2

3

3c

2c

1c

0p

032211003210 )()0,()()0,()(),0()(),(),,,,,( pRlTRlTRlTRyxTyxf ),,( 0yx0c

base

Upper arm

lower arm

middle arm

If you do inverse kinematics with position of this point

What’s the size of Jacobian matrix?

2-by-6 matrix

Page 67: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Human Characters

xRRTRTRRRTRRRzyxTf )()()()()()()()()(),,( 332

321

21110

1000000 67

Page 68: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

• Analytical solution only works for a fairly simple structure

• Iterative approach needed for a complex structure

Page 69: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

• Is the solution unique?

• Is there always a good solution?

Page 70: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Ambiguity of IK

• Multiple solutions

Page 71: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Ambiguity of IK

• Infinite solutions

Page 72: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Failures of IK

• Solution may not exist

Page 73: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

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

Page 74: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

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

Page 75: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

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

2

0

2)(minarg

iii cf

Page 76: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

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

2

0

2)(minarg

iii cf

Satisfy the constraints Minimize the difference between the solution and a reference pose

Page 77: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

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)

Page 78: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

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)

)()(minarg2

gcf

iii

Page 79: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Inverse kinematics

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

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

– Naturalness g(θ) (particularly for human characters)

)()(minarg2

gcf

iii

Satisfy the constraints How natural is the solution pose?

Page 80: CSCE 641: Forward kinematics and inverse kinematics Jinxiang Chai.

Interactive Human Character Posing

• video