Inverse Kinematics

2

The Problem

Robotic applications: cutting/welding

Forward Kinematics:Joint angles End effector

Inverse Kinematics:End effector Joint angles

Inverse Kinematics:End effector Joint angles

Fall 2012

3

Animation Applications (more)

Fall 2012

4

IK Solutions

Analytical solutions are desirable because of their speed and exactness of solution. For complex kinematics problems, analytical solutions may not be possibleUse iterative methods Optimization methods (e.g., minimize the

distance between end effector and goal point)

Fall 2012

5

Case Study: Two-Link Arm

1

2

L1

L2

x

y

21

22

21

221

2

22122

21

21211

2121121

22

21

22

2122

221121122

12

2122

221121122

12

21211

21211

2cos

cos2

sincoscossinsin

sinsincoscoscos2

)(sin)sin(sin2sin

)(cos)cos(cos2cos

Inverse

)sin(sin

)cos(cos

LL

LLyx

LLLL

LLLLyx

LLLLy

LLLLx

KinematicsSolving

LLy

LLx

KinematicsForward

Analytic (closed form) solution

Case:3-link armFall 2012

6

2-link Arm (analytic solution2)

1

43

2

L2

L1

22122

2212211

22122

22122

cossin

cossin

1

431

122

224

3

sincossincos

tan

sincossincos

1tan

cossin

tan

tan

122

22

122

22

LyLLxLxLLy

LyLLxLxLLy

LLL

LLL

xy

LLL

xy

xy

ba

baba

tantan1

tantantan

ba

baba

tantan1

tantantan

y

xFall 2012

Unreachable Targets

No joint angles can satisfy the target

7

22122

2212211

21

22

21

221

2

sincossincos

tan

2cos

LyLLxLxLLy

LL

LLyx

If this value is > 1 or < -1,

no solution exists

Fall 2012

8

Statement of the IK ProblemWant

Error at ith iteration

q: joint angles

*

i i

g =

Jacobian matrixxRn, qRm, Jnm

Seek correction q to fix the error

Omit high order terms…

Fall 2012

m: joint space dimensionn: space where end effector is in

Note: here q are in radians!

Error at End Effector

9

jjdjcorient

cdpos

uuqE

ppqE2

2

1)(

)(

:Errors

33

321n orientatio

position

ionconfigurateffector endCurrent

cccc

Tcccc

uuuo

zyxp

Fall 2012

10

Jacobian Inverse

1

2

L1

L2

x

y

small is until iterate ,

coscoscos

sinsinsin

)sin(sin

)cos(cos

21221211

21221211

21

21

21211

21211

qxqJ

LLL

LLL

yy

xxJ

qJx

LLy

LLx

KinematicsForward

Simple case: Cramer’s rule sufficesGeneral case: pseudo inverse

Fall 2012

11

3-link 2D arm

1

2

L1

L2

x

y

L3 3

32133213212321321211

32133213212321321211

coscoscoscoscoscos

sinsinsinsinsinsin

321

321

LLLLLL

LLLLLL

J

qJx

yyy

xxx

Jacobian inverse is underdetermined

Fall 2012

)sin()sin(sin

)cos()cos(cos

321321211

321321211

LLLy

LLLx

KinematicsForward

12

Pseudo Inverse and Ax=b

Full rank: A-1 exist; A+ is the same as A-1

Underdetermined case: many solutions; will find the one with the smallest magnitude |x|Overdetermined case: find the solution that minimize the error r=||Ax–b||, the least square solution

A panacea for Ax=bA panacea for Ax=b

bUVbAx

bAxT

bUVbAx

bAxT

Fall 2012

About Pseudo Inverse A+

Fall 2012 13

32...

...

A

32...

...

A

C(A

T )N

(A)

R3

C(A

)N

(AT )

R2

A

A+

A: row space → column spaceA+: column space → row space

14

Using GSL (Gnu Scientific Library)

4

4

22

31

2

1

x

x

2

2

110

011

3

2

1

x

x

x

1

2

1

10

11

01

2

1

x

x

3

8

11

22

2

1

x

x

Full rank

Full row rank

Full column rank

Fall 2012

15

Forward Kinematics

1

2

L1

L2

x

y

Rotate(z,1)Translate(L1,0)

Rotate(z,2)Translate(L2,0)

1

0

0

100

010

01

100

0

0

100

010

01

100

0

0

1

2

22

221

11

11 L

cs

scL

cs

sc

y

x

pLTRLTRx 22221111

End effector at the local origin

Using Coordinate Transformation

Fall 2012

17reference

Computing Jacobian

Fall 2012i: unit vector of rotation axis

Example

18

1

2

L1

L2

x

y

x

y

yx

kji

rJ

JJJ

0

100111

21

)sin(sin

)cos(cos

21211

21211

LLy

LLxr1

212

212

212212

222

cos

sin

0sincos

100

L

L

LL

kji

rJ

1

2

L1

L2

x

y

r2

Fall 2012

19

About Joint Space Redundancy

goalssecondary

redundancy Exploiting

zJJIxJq

Fall 2012

mRz

If m > nRedundant manipulator

Joint Space End Effectorm n

Example

20

1

2

L1

L2

L33

zJJIq

z T

32233313

maxmax1

:correction goalSecondary

limit angleJoint :0,0,

max

Fall 2012

21

IK Solution (CCD)

CCD: cyclic coordinate descent; initially from C. Welman (1993)From the most distal joint, solve a series of one-dimensional minimization analytically to satisfy the goal (one joint at a time)

Fall 2012

22

CCD-1 (Cyclic Coordinate Descent)

Fall 2012

23

CCD-2

Fall 2012

24

CCD-3

Implementing joint limits in CCD is straight forward:

simply clamp the joint angle

Implementing joint limits in CCD is straight forward:

simply clamp the joint angle

Fall 2012

25

CCD (2-link arm)

1

2

L1

L2

Pcurrent

PdestP1 dc

d

d

c

c

ppppsign

pp

pp

pp

ppsign

11

1

1

1

11

by determined :

cos

dc

d

d

c

c

ppppsign

pp

pp

pp

ppsign

00

0

0

0

01

by determined :

cos

1

2

L1

L2

Pc

Pd

P0

Fall 2012

26

IK Solutions (DLS)DLS (damped least square)

Position of k end effectors

target of end effectors

error of end effectors

Joint angles

Jacobian matrix (mn)

For 3-link 2D arm: m = 2, n =

3

Fall 2012

27

DLS (Damped Least Square)

Joint angle to correct error

Minimize damped least square

Rewrite error as

Normal equation for least square problem

Simplifying

and getFall 2012

28

[Details]

222xeJ

ReJ

x

R

ReJ

nm

n

m

2

2

0

minimize

0

e

I

Jx

e

I

JeJx

bAxAA

xAb

TT

ˆ

solvingby

ˆ minimize2

Summary bA x

0

e

I

J

I

J

I

J

EquationNormalTT

Fall 2012

29

DLS (cont)(continued)

Shown next page

Therefore

Instead of computing inverse, solve

For 3-link 2D arm: m = 2, n =

32x2

How to choose

?Affect convergence rate

Fall 2012

Damping Effects

30Fall 2012

Properties of Pseudo Inverse A+ (From Wikipedia)

31Fall 2012

32

Challenging IK Cases:Multiple Targets

Fall 2012

Types of IK

33Fall 2012

Types of IK (cont)

34Fall 2012

35

Inverse Kinetics (Boulic96)

The constraint on the position of the center of mass is treated as any other task, and solved at the differential level with a special-purpose Jacobian matrix that relates differential changes of the joint coordinates to differential changes of the Cartesian coordinates of the center of mass.

Fall 2012

Kinematic chain

36Fall 2012

37Fall 2012

38Fall 2012

Support Materials

40

3-link 2D arm

1

2

L1

L2

x

y

L3 3

32133213212321321211

32133213212321321211

321321211

321321211

coscoscoscoscoscos

sinsinsinsinsinsin

)sin()sin(sin

)cos()cos(cos

321

321

LLLLLL

LLLLLL

J

qJx

LLLy

LLLx

KinematicsForward

yyy

xxx

Jacobian inverse is underdetermined

Fall 2012

41

Projection onto a Space

Ax=b may not have solution (if b is not in C(A))Solve instead where p is the projection of b onto C(A)

a1

a2

b

pe=b–p

pxA ˆ

Recall least square problem

||

||

21 aaA

bAxAA

xAb

TT

ˆ

solvingby

ˆ minimize2

Summary

Fall 2012

42

Projection (cont)

PbbAAAAxAp

bAAAx

bAxAA

xAba

a

xAba

xAba

axAbaxAb

xAaxaxp

TT

TT

TT

T

T

T

T

1

1

2

1

2

1

21

2211

ˆ

ˆ

ˆ

0

0ˆ

0ˆ

0ˆ

ˆ ˆ

ˆˆˆ

P: projection matrix

Known as the “normal equation”

Fall 2012

Extra material

Jacobian transposeConstraint dynamics

44

IK General (Jacobian Transpose)

1

2

L1

L2

x

y

pc

pd

FJq

mqm

q

fJFJ

qfx

mmmfffF

T

nn

nT

n

n

Tzyxzyx

1let and dynamics Aristotole

,

work virtualof principle From

)(

Kinematics Forward

effector endat applied Force

11

6166

116

Fall 2012

45

Jacobian Transpose

1

2

L1

L2

x

y

21221211

21221211

21

21

21211

21211

coscoscos

sinsinsin

)sin(sin

)cos(cos

LLL

LLL

yy

xxJ

qJx

LLy

LLx

KinematicsForward

Fall 2012

46Fall 2012

47Fall 2012

48Fall 2012

49Fall 2012

50

Differences

xJq

qJx

qfx

133212

)(

inverseJacobian

xJq

qJx

qfx

T

133212

)(

ransposeJacobian t

Fall 2012

51

[Constraint Dynamics]

1

2

L1

L2

P1

P2

aT

Tc

ca

ca

JWfJWJ

Jf

ffJW

qqJqC

qC

ffQ

WQq

work virtualof Principle

0

Combining

0)()(

0)(

Constraint

:motion ofEqn

12122121

11

22

212

212

21

21

21

2222

0022

0

0

2

2

2

2

1

2

1

2

2

1

2

1

1

1

1

1

yyxxyyxx

yx

J

Lyyxx

Lyx

yC

xC

yC

xC

yC

xC

yC

xC

Fall 2012

52Fall 2012

53

1

2

L1

L2g

All joints rotate w.r.t. local z

CCD step 1: Find 2g; project og to XY plane (as og’); rotate OE to og’

FBF1

F2

CCD step 2: Find 1g; project og to XY plane (as og’); rotate OE to og’

)0,0,(),(

),(

12

1

2

1

1

12

1

2

11

LtranszRotT

zRotT

gTTg

gTg

FF

FF

FFF

FF

F

FFF

F

B

B

B

B

B

frameB frameA to taking :TT

xTxAB

BAB

A

Fall 2012