CPSC 441: Forward kinematics and inverse kinematics

CPSC 441: Forward kinematics and inverse

kinematicsJinxiang Chai

Outline

Kinematics– Forward kinematics– Inverse kinematics

Kinematics

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

Degrees of freedom (Dofs)

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



• How many degrees of freedom when flying?



• 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

Degrees of Freedom

• How about this robot arm?

• Six again

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

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

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

Example: Two-link Structure

• Two links connected by rotational joints

BaseEnd Effector

θ2

θ1 X=(x,y)

l2l1


• 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)


• 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)

Forward kinematics

BaseEnd Effector

θ2

θ1

(0,0)

θ2 l2l1

• Create an animation by specifying the joint angle trajectories

θ1

θ2

X=(x,y)

Inverse kinematics

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

Base

End Effector

θ2

θ1

(0,0)

θ2 l2l1

X=(x,y)

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)

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)

Why Inverse Kinematics?

• Basic tools in character animation - key frame generation

- animation control - interactive manipulation

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

Inverse kinematics

• Given end effector position, compute required joint angles

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

Inverse kinematics

• Analytical solution only works for a fairly simple structure

• Iterative approach needed for a complex structure

Iterative approach

• Inverse kinematics can be formulated as an optimization problem

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

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

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

Gauss-newton approach

Step 1: initialize the joint angles with

Step 2: update the joint angles:

0

kkk 1




0

kkk 1

How can we decide the amount of update?




ii

kki cf

2)(minarg

0

kkk 1




ii

kki cf

2)(minarg

0

kkk 1

Known!




ii

kki cf

2)(minarg

0

kkk 1

Taylor series expansion




ii

kki cf

2)(minarg

2

)(minarg

ii

kkik

i cf

f

0

kkk 1


rearrange




ii

kki cf

2)(minarg

2

)(minarg

ii

kkik

i cf

f

0

kkk 1


rearrange

Can you solve this optimization problem?




ii

kki cf

2)(minarg

2

)(minarg

ii

kkik

i cf

f

2

))((minarg

i

kii

kki fcf

0

kkk 1


rearrange

This is a quadratic function of


• 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




),...,(

),...,(

),...,(

...

...

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!




),...,(

),...,(

),...,(

...

...

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 Δθ




),...,(

),...,(

),...,(

...

...

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)(

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)

Jacobian matrix

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

(x,y)

θ1

θ2

Jacobian matrix


(x,y)

θ1

θ2

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

Jacobian matrix


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)

Jacobian matrix: 2D-link structure

2221211

2121211


21

cllcll

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)


2221211

2121211


21

cllcll

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)

f1f2


2221211

2121211


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

...

...


2221211

2121211


21

cllcll

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

Base

θ2

θ1

(0,0)

θ2 l2l1

C=(c1,c2)


2221211

2121211


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


2221211

2121211


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




0

kkk 1

Step size: specified by the user

Another Example

• A 2D lamp with 6 degrees of freedom

1

2

3

3c

2c

1c

0p

),,( 0yx0c

base

Upper arm

lower arm

middle arm

How to compute forward kinematics?

Another Example


1

2

3

3c

2c

1c

0p

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

base

Upper arm

lower arm

middle arm

Another Example


1

2

3

3c

2c

1c

0p


base

Another Example


1

2

3

3c

2c

1c

0p


base

Upper arm

Another Example


1

2

3

3c

2c

1c

0p


base

Upper arm

middle arm

Another Example


1

2

3

3c

2c

1c

0p


base

Upper arm

lower arm

middle arm

More Complex Characters?


1

2

3

3c

2c

1c

0p

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

base

Upper arm

lower arm

middle arm



1

2

3

3c

2c

1c

0p


base

Upper arm

lower arm

middle arm

If you do inverse kinematics with position of this point

What’s the size of Jacobian matrix?



1

2

3

3c

2c

1c

0p


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

Human Characters

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

321

21110

1000000 57

Inverse kinematics

• Analytical solution only works for a fairly simple structure

• Iterative approach needed for a complex structure

Inverse kinematics

• Is the solution unique?

• Is there always a good solution?

Ambiguity of IK

• Multiple solutions

Ambiguity of IK

• Infinite solutions

Failures of IK

• Solution may not exist

Inverse kinematics

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

Inverse kinematics


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

Inverse kinematics



2

0

2)(minarg

iii cf

Inverse kinematics



2

0

2)(minarg

iii cf

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

Inverse kinematics



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

Inverse kinematics




)()(minarg2

gcf

iii

Inverse kinematics




)()(minarg2

gcf

iii

Satisfy the constraints How natural is the solution pose?

Interactive Human Character Posing

• video

CPSC 441: Forward kinematics and inverse kinematics

Documents

Transcript of CPSC 441: Forward kinematics and inverse kinematics