Rick Parent - CIS682 Inverse Kinematics 1.Set goal configuration of end effector 2.calculate interior joint angles Analytic approach – when linkage is

Rick Parent - CIS682

Inverse Kinematics

1. Set goal configuration of end effector2. calculate interior joint angles

Analytic approach – when linkage is simple enough, directly calculate joint angles in configuration that satifies goal

Numeric approach – complex linkagesAt each time slice, determine joint movements that take you in direction of goal position (and orientation)


Forward Kinematics - review

Pose – linkage is a specific configuration

Pose Vector – vector of joint angles for linkage

Degrees of Freedom (DoF) – of joint or of whole figure

Articulated linkage – hierarchy of joint-link pairs

Types of joints: revolute, prismatic

Tree structure – arcs & nodesRecursive traversal – concatenate arc matricesPush current matrix leaving node downwardPop current matrix traversing back up to node


Goal

End Effector

1

2

3L1

L2L3

Inverse Kinematics


Underconstrained – if fewer constraints than DoFsMany solutions

Overconstrained – too many constraintsNo solution

Dextrous workspace – volume end effector can reach in any orientation

Reachable workspace – volume the end effector can reach

Inverse Kinematics


Given arm configuration (L1, L2, …)

Given desired goal position (and orientation) of end effector: [x,y] or [x,y,z, 1,2, 3]

Analytically compute goal configuration (1,2)

Interpolate pose vector from initial to goal

Inverse Kinematics - Analytic


Analytic Inverse Kinematics

(X,Y)L1

L2

1

2

Goal



(X,Y)Goal

Multiple solutions



(X,Y)L1L2

1

T

180- 2



(X,Y)L1L2

1

T

180- 2

X

Y

x 2 y 2


Law of Cosines

A

B

C

AB

CBA

2)cos(

222



(X,Y)L1L2

1 T

180- 2

X

Y

x 2 y 2

22)cos(

YX

XT

22

1cosYX

XT

221

22

2221

12

)cos(YXL

LYXLT

TYXL

LYXL

)

2(cos

221

22

22211

1

21

2222

21

2 2)180cos(

LL

YXLL

)

2(cos180

21

2222

211

2 LL

YXLL




Iterative Inverse Kinematics

When linkage is too complex for analytic methods

At each time step, determine changes to joint angles that take the end effector toward goal position and orientation

Need to recompute at each time step


End Effector

2

a2 d2=EF-J2

a2 x d2

- Compute instantaneous effect of each joint

- Linear approximation to curvilinear motion

- Find linear combination to take end effector towards goal position

Inverse Jacobian Method



Instantaneous linear change in end effector for ith joint

= (EF - Ji) x ai


Inverse Jacobian Method What is the change in

orientation of end effector induced by joint i that has

axis of rotation ai

and position Ji? iia Angular velocity


Solution only valid for an instantaneous step

Angular affect is really curved, not straight line

Once a step is taken, needto recompute solution



Inverse Jacobian Method - Mathematics

y1 f1(x1,x2,x3,x4,x5,x6)

y2 f2(x1,x2,x3,x4,x5,x6)

y3 f3(x1,x2,x3,x4,x5,x6)

y4 f4 (x1,x2,x3,x4,x5,x6)

y5 f5(x1,x2,x3,x4,x5,x6)

y6 f6(x1,x2,x3,x4,x5,x6)

Set up equationsyi: state variablexi : system parameterfi : relate system parameters to state variable



y1 f1(x1,x2,x3,x4,x5,x6)

y2 f2(x1,x2,x3,x4,x5,x6)

y3 f3(x1,x2,x3,x4,x5,x6)

y4 f4 (x1,x2,x3,x4,x5,x6)

y5 f5(x1,x2,x3,x4,x5,x6)

y6 f6(x1,x2,x3,x4,x5,x6)

Matrix Form

Y F(X)



Use chain rule to differentiate equations to relate changes in

system parameters to changes in state variables

),,,,,( 6543211 xxxxxxfyi

66

55

44

33

22

11

dxx

fdx

x

fdx

x

fdx

x

fdx

x

fdx

x

fdy iiiiii

i


Y F(X) dXX

FdY

66

55

44

33

22

11

dxx

fdx

x

fdx

x

fdx

x

fdx

x

fdx

x

fy iiiiiii

Matrix Form




dXX

FdY

Change in position (and orientation) of end effector Change in joint angles

Linear approximation that relates change in joint angle to change in end effector position (and orientation)



dXX

FdY

JV



dXX

FdY

n

z

y

x

z

n

yyy

n

xxx

z

y

x

z

y

x

a

a

a

p

ppp

ppp

v

v

v

2

1

1

1

1

1

21

21



n

z

y

x

z

n

yyy

n

xxx

z

y

x

z

y

x

a

a

a

p

ppp

ppp

v

v

v

2

1

1

1

1

1

21

21

= (S - J1) x a1

= 1


The Matrices

n

zzz

n

yyy

n

xxx

T

n

Tzyxzyx

aaa

ppp

ppp

J

vvvV

...

............

...

...

...

21

21

21

321

JV


The Matrices

V – desired linear and angular velocities

J – JacobianMatrix of partials

– change to joint angles (unknowns)

3x1, 6x1

3xN, 6xN

N DoFs

N x 1

JV


Pseudo Inverse of the Jacobian

VJ

JJJJVJJJ

JJVJ

JV

TTTT

TT

11 )()(

J (JT J) 1JT JT (J JT ) 1


LU decomposition

Solving using the Pseudo Inverse

T

T

T

TT

J

VJJ

VJJ

VJJJ

VJ

)(

)(

)(1

1


Adding a Control Term

A solution of this form

…doesn’t affect the desired configuration

But it can be used to biasThe solution vector

0

0

)(

)(

)(

)(

V

zV

zJJV

zJJJJV

zIJJJV

JV

zIJJ

When put into this formula

Like this

After some manipulation, you can show that it…


Form of the Control TermDesired angles and corresponding gains are input

Bias to desired angles(not the same as hard joint limits)

‘z’ is H differentiated

H i

i1

n

(i ci)

zH dH

d i

i1

n

(i ci) 1

Where the deviation is large, you bump up the solution vector in such a way that you don’t disturb the desired effect


Some Algebraic Manipulation

Include this in equation HIJJJJV )(

HIJJVJ )(Isolate vector of unknown

Rearrange to isolate the inverse

HHJVJJJ

HHJVJJJ

HHJVJ

HHJJVJ

HIJJVJ

TT

TT

)]()[(

)()(

)(

)(

1

1


LU decomp.

Solving the Equations

)(

)()( 1

T

T

T

JJHJV

HJ

HJVJJ


Use to bias to desired mid-angle

Does not enforce joint angles

Does not address “human-like” or “natural” motion

Control Term

Only kinematic control – no forces involved


Jacobian transpose

Alternate Jacobian – use goal position

HAL – human arm linkage

Other ways to numerically IK

CCD

Damped Least Squares


Jacobian Transpose

Use projection of effect vector onto desired movement


Jacobian Transpose

)())(( SGaJS ii

VJ T S


Jacobian Transpose

…

z

y

x

T

z

y

xnnxx

SG

SG

SG

aJS

aJS

aJSaJSaJS

)(

)(

)(

...))((

.........))((

))(())(())((

11

11

2211

)())(( SGaJS nn

)())(( 11 SGaJS

)())(( 22 SGaJS

VJ T


Alternate Jacobian

…

AltJ)())(( SGaJG nn

)())(( 11 SGaJG

)())(( 22 SGaJG

G

Use the goal postion instead of the end-effector!!??

!?


Damped Least Squares

VJIJJ TT )( 2

G

VJIJJ TT 12 )( VIJJJ TT 12 )(

VIJJf T 12 )( VfIJJ T )( 2

fJ T

substitution

Solve


Hueristic Human-Like Linkage (HAL)

3 DoF

3 DoF

1 DoF

G

Decompose into simpler subproblems

Fix wrist position – use as Goal

7 DoF linkageRA (, , RB ( ), RC(,

Set hand position and rotation based on relative position of Goal to shoulder



Set based on distance between shoulder and wrist

s

ew

Assume axis of elbow is perpendicular to plane defined by s, e, w use law of cosines

t

t LL

LLL

swL

ewL

seL

180

2)cos(

4

21

222

21

2

1

L1 L2

L



Determine elbow position based on heuristics

s

w

e

For example: project forearm straight from hand orientation

Clamp to inside of limits

if arm intersects torso or a shoulder angle exceeds joint limit (or exceeds comfort zone) –

Elbow lies on circle defined by w, s &



From e and w and hand orientation, determine RB

s

w

e

From e and s, determine RA


Traverse linkage from distal joint inwardsOptimally set one joint at a timeUpdate end effector with each joint change

Cyclic-Coordinate Descent

Use weighted average of position and orientation.

At each joint, minimize difference between end effector and goal

Easy if only trying to match position; heuristic if orientation too



)()()( qEqEqE op

2)()( cdp PPqE

23

1

)1)(()(

jcj

jdo uuqE .



icaxisic PRPi

)()(

idicp PPg )()(

)()(3

1

jcj

jdo uug

)()()( oopp gwgwg

Rotational joint:

.



)()()( oopp gwgwg

1ow)1( pw

Wk /

),max(

),min(

icid

icid

PP

PP

Rotational joint:

.



)sin()cos())cos(1()( 321 kkkg

)()())((3

11 ijc

jijdoiiciidp axisuaxisuwaxisPaxisPwk

)()(3

12 jc

jjdoicidp uuwPPwk

)]()([3

13 jc

jjdoicidpi uuwPPwaxisk

Rotational joint:

.



)sin()cos())cos(1()( 321 kkkg

0)cos()sin()( 321 kkk

))(

(tan12

31

kk

k

c c

iii wqq

Rotational joint:

.



iicid axisPP )(

Translational joint:

..


IK w/ constraintsChris Welman, “Inverse Kinematics and Geometric Constraints for Articulated Figure Manipulation,” M.S. Thesis, Simon Fraser University, 2001.

Basic idea:

Constraints are geometric, e.g., point-to-point, point-to-plan, specific orientation, etc.

Assume starting out in satisfied configuration

Forces are applied to system

Detect, and cancel out, force components that would violate constraints.


IK w/ constraints

Point-on-a-plane constraint Fa Fc

Ft

Given: geometric constraints & applied forces

Determine: what constraints will be violated & what (minimal) forces are needed to counteract the components of the applied forces responsible for the violations.


Constraints

0)( qC

0)(

qq

CqC

j

ic q

cJ

.

To maintain constraints, need:

Notation:


Constraints

0)(

qq

CqC

KgFJKq T

KgJC c

.Generalized forceConstraint Jacobian

IK Jacobian


example constraint

0))(( 2 qPR

])(,)(,)[(

))(()),(()),(()(222

321

zzyyxx

zyx

PRPRPR

qPcqPcqPcqC

j

k

k

i

j

ic q

P

P

c

q

cJ

Usually sparse

.

Geometric constraint on point that is function of pose


Computing the constraint force

ca ggg

accc KgJKgJ To counteract ga’s affect on constraints:

.

Applied force Yet to be determined constraint force

g should lie in the nullspace of JcK 0KgJ c


Computing the constraint force

acTcc KgJKJJ

.

Solve linear system to find Lagrange multiplier vector.

The system is usually underconstrainedRestrict gc to move the system in a direction it may not go

cc Jg


Solving for Lagrange Multipliers],...,[ 1 m

acTcc KgJKJJ

Shortest distance from A to B passing through a point P

Constrain P to lie on g

g

A

B

Points on ellipse are set of points for which sum of distances to foci is equal to some constant

Direction of gradients are equal

gradient


Solving for Lagrange Multipliers

TccKJJUse truncated SVD with backsubstitution on

TUDVA

diagonalRange basis Nullspace basis.

acTcc KgJKJJ

bA


Solving for Lagrange Multipliers

bAUDV T

ii

i

T

TT

TT

T

Vw

bU

bUVD

bUDV

bUDV

bUDV

1

1


Feedback term

Tcca kCJggKq )(

Spring that penalizes deviation from constraints.


Implementation

Handles on skeletons

Point handle

orientation handle

Center-of-mass handle

Each handle must know how to compute the Jacobian.

Each handle must know how to its value from q


Constraints on handles

Constraining a point handle to a location

Constraining a point handle to a plane

Constraining a point handle to a line

Constraining an orientation handle to an orientation

.


Dataflow approach

..

x q

x

f

q

x

x

f

q

f

)(xf

q

f

Constraint function block

Knows how to computeIts function in term of x

Knows its Jacobian wrt x


Example network

..

c1

JcC

h1 h2 h3h4

q

c2

Documents

Rick Parent - CIS682 Inverse Kinematics 1.Set goal configuration of end effector 2.calculate interior joint angles Analytic approach – when linkage is