Inverse Kinematics

filercaliforniaΜηχανική

14 Νοε 2013 (πριν από 3 χρόνια και 8 μήνες)

111 εμφανίσεις

Inverse Kinematics

2

The Problem

Robotic applications: cutting/welding

Forward Kinematics:

Joint angles


End effector

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 possible

Use iterative methods


Optimization methods (e.g., minimize the distance
between end effector and goal point)

Fall 2012

5

Case Study: Two
-
Link Arm

q
1

q
2

L
1

L
2

x

y




















































2
1
2
2
2
1
2
2
1
2
2
2
1
2
2
2
1
2
1
2
1
1
2
1
2
1
1
2
1
2
2
2
1
2
2
2
1
2
2
2
2
1
1
2
1
1
2
2
1
2
2
1
2
2
2
2
1
1
2
1
1
2
2
1
2
2
1
2
1
1
2
1
2
1
1
2
cos
cos
2


sin
cos
cos
sin
sin
sin
sin
cos
cos
cos
2
)
(
sin
)
sin(
sin
2
sin
)
(
cos
)
cos(
cos
2
cos

Inverse

)
sin(
sin
)
cos(
cos

L
L
L
L
y
x
L
L
L
L
L
L
L
L
y
x
L
L
L
L
y
L
L
L
L
x
Kinematics
Solving
L
L
y
L
L
x
Kinematics
Forward
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
Analytic (closed form) solution

Case:3
-
link arm

Fall 2012

6

2
-
link Arm (analytic solution
2
)

q
1

q
4

q
3

q
2

L
2

L
1









2
2
1
2
2
2
2
1
2
2
1
1
2
2
1
2
2
2
2
1
2
2
cos
sin
cos
sin
1
4
3
1
1
2
2
2
2
4
3
sin
cos
sin
cos
tan
sin
cos
sin
cos
1
tan
cos
sin
tan
tan
1
2
2
2
2
1
2
2
2
2
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
L
y
L
L
x
L
x
L
L
y
L
y
L
L
x
L
x
L
L
y
L
L
L
L
L
L
x
y
L
L
L
x
y
x
y























b
a
b
a
b
a
tan
tan
1
tan
tan
tan




y

x

Fall 2012

Unreachable Targets

No joint angles can satisfy the target

7





2
2
1
2
2
2
2
1
2
2
1
1
2
1
2
2
2
1
2
2
1
2
sin
cos
sin
cos
tan
2
cos
q
q
q
q
q
q
L
y
L
L
x
L
x
L
L
y
L
L
L
L
y
x



















If this value is


> 1 or <
-
1,

no solution exists

Fall 2012

8

Statement of the IK Problem

Want

Error at i
th

iteration

q
: joint angles

*

i

i

g =

Jacobian matrix

x

R
n
,
q

R
m
, J
n

m

Seek correction
D
q

to fix the error

Omit high order terms…

Fall 2012

m
: joint space dimension

n
: space where end effector is in

Note: here
D
q

are in
radians!

Error at End Effector

9









j
jd
jc
orient
c
d
pos
u
u
q
E
p
p
q
E
2
2
1
)
(
)
(
:
Errors


3
3
3
2
1
n
orientatio
position
ion
configurat
effector

end
Current













c
c
c
c
T
c
c
c
c
u
u
u
o
z
y
x
p
Fall 2012

10

Jacobian Inverse

q
1

q
2

L
1

L
2

x

y









small

is


until

iterate

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

2
1
2
2
1
2
1
1
2
1
2
2
1
2
1
1
2
1
2
1
2
1
2
1
1
2
1
2
1
1
q
x
q
J
L
L
L
L
L
L
y
y
x
x
J
q
J
x
L
L
y
L
L
x
Kinematics
Forward
D
D

D






























D

D






q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
Simple case: Cramer

s rule suffices

General case:
pseudo inverse

Fall 2012

11

3
-
link 2D arm

q
1

q
2

L
1

L
2

x

y

L
3

q
3






























































D

D












3
2
1
3
3
2
1
3
2
1
2
3
2
1
3
2
1
2
1
1
3
2
1
3
3
2
1
3
2
1
2
3
2
1
3
2
1
2
1
1
cos
cos
cos
cos
cos
cos
sin
sin
sin
sin
sin
sin
3
2
1
3
2
1
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
L
L
L
L
L
L
L
L
L
L
L
L
J
q
J
x
y
y
y
x
x
x
Jacobian inverse is
underdetermined

Fall 2012

)
sin(
)
sin(
sin
)
cos(
)
cos(
cos

3
2
1
3
2
1
2
1
1
3
2
1
3
2
1
2
1
1
q
q
q
q
q
q
q
q
q
q
q
q












L
L
L
y
L
L
L
x
Kinematics
Forward
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=b

b
U
V
b
A
x
b
Ax
T






Fall 2012

About Pseudo Inverse A+

Fall 2012

13

3
2
.
.
.
.
.
.








A
R
3

R
2

A

A
+

A: row space


column space

A
+
: column space


row space

14

Using GSL

(Gnu Scientific Library)




















4
4
2
2
3
1
2
1
x
x























2
2
1
1
0
0
1
1
3
2
1
x
x
x




























1
2
1
1
0
1
1
0
1
2
1
x
x



















3
8
1
1
2
2
2
1
x
x
Full rank

Full row rank

Full column rank

Fall 2012

15

Forward Kinematics

q
1

q
2

L
1

L
2

x

y

Rotate(z,
q
1
)

Translate(L
1
,0)

Rotate(z,
q
2
)

Translate(L
2
,0)
































































1
0
0
1
0
0
0
1
0
0
1
1
0
0
0
0
1
0
0
0
1
0
0
1
1
0
0
0
0
1
2
2
2
2
2
1
1
1
1
1
L
c
s
s
c
L
c
s
s
c
y
x
q
q
q
q
q
q
q
q








p
L
T
R
L
T
R
x
2
2
2
2
1
1
1
1
q
q

End effector at
the local origin

Using Coordinate Transformation

Fall 2012

17

reference

Computing Jacobian

Fall 2012

w
i
: unit vector of
rotation axis

Example

18

q
1

q
2

L
1

L
2

x

y



























x
y
y
x
k
j
i
r
J
J
J
J
0
1
0
0
1
1
1
2
1
w
)
sin(
sin
)
cos(
cos
2
1
2
1
1
2
1
2
1
1
q
q
q
q
q
q






L
L
y
L
L
x
r
1




































2
1
2
2
1
2
2
1
2
2
1
2
2
2
2
cos
sin
0
sin
cos
1
0
0
q
q
q
q
q
q
q
q
w
L
L
L
L
k
j
i
r
J
q
1

q
2

L
1

L
2

x

y

r
2

Fall 2012

19

About Joint Space Redundancy








goals
secondary


redundancy

Exploiting
z
J
J
I
x
J
q
D


D

D


Fall 2012

m
R
z

D
If m > n

Redundant


manipulator

Joint Space

End Effector

m

n

Example

20





z
J
J
I
q
z
T
D


D


D





3
2
2
3
3
3
1
3
max
max
1
:
correction

goal
Secondary
limit

angle
Joint
:
0
,
0
,
q
q
q
q
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

Fall 2012

25

CCD (2
-
link arm)

q
1

q
2

L
1

L
2

P
current

P
dest

f

P
1

d
c
d
d
c
c
p
p
p
p
sign
p
p
p
p
p
p
p
p
sign
1
1
1
1
1
1
1
by

determined

:
cos













f
d
c
d
d
c
c
p
p
p
p
sign
p
p
p
p
p
p
p
p
sign
0
0
0
0
0
0
1
by

determined

:
cos













f
q
1

q
2

L
1

L
2

P
c

P
d

f

P
0

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

n)

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 get

Fall 2012

28

[Details]

2
2
2
x
e
J
R
e
J
x
R
R
e
J
n
m
n
m

D


D







D

D


D


D

q

q
q

q
q

q
2
2
0
minimize
0









D
























D















D

D

e
I
J
x
e
I
J
e
J
x
q

q

q

q
b
A
x
A
A
x
A
b
T
T


ˆ
solving
by
ˆ

minimize
2
Summary

b

A

x


















D
















0

e
I
J
I
J
I
J
Equation
Normal
T
T

q


Fall 2012

29

DLS (cont)

(continued)

Shown next page

Therefore

Instead of computing
inverse, solve

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

2x2

How to
choose

?

Affect convergence rate

Fall 2012

Damping Effects

30

Fall 2012

Properties of Pseudo Inverse A
+

(From
Wikipedia
)

31

Fall 2012

32

Challenging IK Cases:

Multiple Targets

Fall 2012

Types of IK

33

Fall 2012

Types of IK (cont)

34

Fall 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

36

Fall 2012

37

Fall 2012

38

Fall 2012

Support Materials

40

3
-
link 2D arm

q
1

q
2

L
1

L
2

x

y

L
3

q
3























































































3
2
1
3
3
2
1
3
2
1
2
3
2
1
3
2
1
2
1
1
3
2
1
3
3
2
1
3
2
1
2
3
2
1
3
2
1
2
1
1
3
2
1
3
2
1
2
1
1
3
2
1
3
2
1
2
1
1
cos
cos
cos
cos
cos
cos
sin
sin
sin
sin
sin
sin
)
sin(
)
sin(
sin
)
cos(
)
cos(
cos

3
2
1
3
2
1
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
L
L
L
L
L
L
L
L
L
L
L
L
J
q
J
x
L
L
L
y
L
L
L
x
Kinematics
Forward
y
y
y
x
x
x


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)

a
1

a
2

b

p

e=b

p

p
x
A

ˆ
Recall least square problem












|
|
|
|
2
1
a
a
A
b
A
x
A
A
x
A
b
T
T


ˆ
solving
by
ˆ

minimize
2
Summary

Fall 2012

42

Projection (cont)











Pb
b
A
A
A
A
x
A
p
b
A
A
A
x
b
A
x
A
A
x
A
b
a
a
x
A
b
a
x
A
b
a
a
x
A
b
a
x
A
b
x
A
a
x
a
x
p
T
T
T
T
T
T
T
T
T
T




































1
1
2
1
2
1
2
1
2
2
1
1
ˆ
ˆ
ˆ
0
0
ˆ
0
ˆ
0
ˆ
ˆ

ˆ
ˆ
ˆ
ˆ
P
: projection matrix

Known as the “normal equation”

Fall 2012

Extra material

Jacobian transpose

Constraint dynamics

44

IK General (Jacobian Transpose)

q
1

q
2

L
1

L
2

x

y

p
c

p
d



F
J
q
m
q
m
q
f
J
F
J
q
f
x
m
m
m
f
f
f
F
T
n
n
n
T
n
n
T
z
y
x
z
y
x


















1
let

and


dynamics

Aristotole
,
work
virtual
of

principle

From
)
(
Kinematics

Forward
effector

end
at

applied

Force
1
1
6
1
6
6
1
1
6


Fall 2012

45

Jacobian Transpose

q
1

q
2

L
1

L
2

x

y














































2
1
2
2
1
2
1
1
2
1
2
2
1
2
1
1
2
1
2
1
2
1
2
1
1
2
1
2
1
1
cos
cos
cos
sin
sin
sin
)
sin(
sin
)
cos(
cos

q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
q
L
L
L
L
L
L
y
y
x
x
J
q
J
x
L
L
y
L
L
x
Kinematics
Forward


Fall 2012

46

Fall 2012

47

Fall 2012

48

Fall 2012

49

Fall 2012

50

Differences

x
J
q
q
J
x
q
f
x











1
3
3
2
1
2
)
(
inverse
Jacobian
x
J
q
q
J
x
q
f
x
T
D

D
D

D




1
3
3
2
1
2
)
(
ranspose
Jacobian t
Fall 2012

51

[Constraint Dynamics]

q
1

q
2

L
1

L
2

P
1

P
2



a
T
T
c
c
a
c
a
JWf
JWJ
J
f
f
f
JW
q
q
J
q
C
q
C
f
f
Q
WQ
q















work
virtual
of

Principle
0

Combining
0
)
(
)
(

0
)
(

Constraint


:
motion

of
Eqn



























































1
2
1
2
2
1
2
1
1
1
2
2
2
1
2
2
1
2
2
1
2
1
2
1
2
2
2
2
0
0
2
2

0
0
2
2
2
2
1
2
1
2
2
1
2
1
1
1
1
1
y
y
x
x
y
y
x
x
y
x
J
L
y
y
x
x
L
y
x
y
C
x
C
y
C
x
C
y
C
x
C
y
C
x
C
Fall 2012

52

Fall 2012

53

q
1

q
2

L
1

L
2

g

All joints rotate w.r.t. local z

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

F
B

F
1

F
2

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

)
0
,
0
,
(
)
,
(
)
,
(



1
2
1
2
1
1
1
2
1
2
1
1
L
trans
z
Rot
T
z
Rot
T
g
T
T
g
g
T
g
F
F
F
F
F
F
F
F
F
F
F
F
F
F
B
B
B
B
B







q
q
frameB

frameA to

taking

:
T
T
x
T
x
A
B
B
A
B
A

Fall 2012