L92_InverseKinematics

8/12/2019 L92_InverseKinematics

1/66

Rick Parent - CIS682

Inverse Kinematics

1. Set goal configurationof end effector

2. calculate interiorjoint angles

Analytic approachwhen linkage is simple enough, directlycalculate joint angles in configuration that satifies goal

Numeric approachcomplex linkages

At each time slice, determine joint movements that take

you in direction of goal position (and orientation)


2/66


Forward Kinematics - review

Poselinkage is a specific configuration

Pose Vectorvector of joint angles for linkage

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

Articulated linkagehierarchy of joint-link pairs

Types of joints: revolute, prismatic

Tree structurearcs & nodes

Recursive traversalconcatenate arc matrices

Push current matrix leaving node downward

Pop current matrix traversing back up to node


3/66


Goal

End Effector

q1

q2 q3L1

L2L3

Inverse Kinematics


4/66


Underconstrainedif fewer constraints than DoFs

Many solutions

Overconstrainedtoo many constraints

No solution

Dextrous workspacevolume end effector can reach inany orientation

Reachable workspacevolume the end effector can reach

Inverse Kinematics


5/66


Givenarm configuration (L1, L2, )

Givendesired goal position (and orientation) of

end effector: [x,y] or [x,y,z, y1,y2, y3]

Analytically compute goal configuration (q1,q2)

Interpolate pose vector from initial to goal

Inverse Kinematics - Analytic


6/66


Analytic Inverse Kinematics

(X,Y)

L1L2

q1

q2

Goal


7/66Rick Parent - CIS682


(X,Y)

Goal

Multiple solutions




(X,Y)L1

L2

q1qT

180- q2




(X,Y)L1

L2

q1qT

180- q2

X

Yx2 y2



Law of Cosines

A

B

C

a

AB

CBA

2)cos(

222 a




(X,Y)L1L2

q1qT

180- q2

X

Yx2 y2

22)cos( YX

XT

q

22

1cosYX

XTq

22

1

2

2

222

11

2)cos(

YXL

LYXLT

qq

T

YXL

LYXL qq

)2

(cos22

1

2

2

222

11

1

21

222

2

2

12

2)180cos(

LL

YXLL q

)

2(cos180

21

222

2

2

11

2LL

YXLL q



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

q2

a2 d2=EF-J2

a2x 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



Inverse Jacobian

MethodInstantaneous linear change in

end effector for ith joint

= (EF - Ji) xa

i



Inverse Jacobian

MethodWhat is the change inorientation of end effectorinduced by joint i that has

axis of rotation aiand position Ji?

ii

a 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




InverseJacobianMethod- 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 equations

yi: state variable

xi : system parameter

fi : relate system parameters to state variable



Inverse JacobianMethod- 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)

Matrix Form YF(X)



Inverse Jacobian Method- Mathematics

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

f

dxx

f

dxx

f

dxx

f

dxx

f

dxx

f

dy

iiiiii

i



YF(X) dXX

FdY

6

6

5

5

4

4

3

3

2

2

1

1

dxx

fdx

x

fdx

x

fdx

x

fdx

x

fdx

x

fy iiiiiii

Matrix Form

Inverse Jacobian Method- Mathematics




dXX

FdY

Change in position (and

orientation) of end effectorChange in joint angles

Linear approximation that relates change injoint angle to change in end effector position

(and orientation)


23/66



dXX

FdY

q

JV


24/66


InverseJacobian

Method

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

q

q

q

q

q

q

q

q

q

q

q

q

q

2

1

1

1

1

1

21

21


25/66



n

z

y

x

z

n

yyy

n

xxx

z

y

x

z

y

x

a

a

a

p

ppp

ppp

v

v

v

q

q

q

q

q

q

q

q

q

q

q

q

q

2

1

1

1

1

1

21

21

= (S - J1) x a1

= 1


26/66


The Matrices

n

zzz

n

yyy

n

xxx

T

n

T

zyxzyx

aaa

ppp

ppp

J

vvvV

q

q

q

q

q

q

q

q

q

qqqqq

...

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

...

...

...

21

21

21

321

qJV


27/66


The Matrices

Vdesired linear and angular velocities

JJacobian

Matrix of partials

qchange to joint angles (unknowns)

3x1, 6x1

3xN, 6xN

N DoFs

N x 1

qJV


28/66


Pseudo Inverse of the Jacobian

qq

q

q

VJJJJJVJJJ

JJVJ

JV

TTTT

TT

11

)()(

J (JTJ)1JT J

T(J JT)1


29/66


LU decomposition

Solving using the Pseudo Inverse

q

q

q

T

T

T

TT

J

VJJ

VJJ

VJJJ

VJ

)(

)(

)(

1

1


30/66


Adding a Control Term

A solution of this form

doesnt affect thedesired configuration

But it can be used to bias

The solution vector

0

0

)(

)(

)(

)(

V

zV

zJJV

zJJJJV

zIJJJV

JV

zIJJ

q

q

When put into this formula

Like this

After some manipulation,

you can show that it


31/66


Form of the Control TermDesired angles and

correspondinggains are input

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

z is H differentiated

H aii1

n

(qiqci)

y

z qH dHdq

y aii1

n

(qiqci)y1

Where the deviation is large, you bump up the solution vector

in such a way that you dont disturb the desired effect


32/66


Some Algebraic Manipulation

Include this in equation HIJJJJV qq

)(

HIJJVJ qq )(Isolate vector of unknown

Rearrange to

isolate the inverse

HHJVJJJ

HHJVJJJ

HHJVJ

HHJJVJ

HIJJVJ

TT

TT

qq

qq

qq

qq

q

q

q

q

q

q

)]()[(

)()(

)(

)(

1

1


33/66


LU decomp.

Solving the Equations

q

q

q

q

)(

)()( 1

T

T

T

JJHJV

HJ

HJVJJ


34/66


Use to bias to desired mid-angle

Does not enforce joint angles

Does not address human-like or natural motion

Control Term

Only kinematic controlno forces involved


35/66


Jacobian transpose

Alternate Jacobianuse goal position

HALhuman arm linkage

Other ways to numerically IK

CCD

Damped Least Squares


36/66


Jacobian Transpose

Use projection of effect vector onto desired movement


37/66


Jacobian Transpose

)())(( SGaJS ii

VJT q

S


38/66


Jacobian Transpose

z

y

x

T

z

y

xnnxx

SG

SG

SG

aJS

aJS

aJSaJSaJS

)(

)(

)(

.. .))((

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

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

11

11

2211

)())(( SGaJS nn

)())(( 11 SGaJS )())(( 22 SGaJS

q VJT


39/66


Alternate Jacobian

AltJ)())(( SGaJG nn

)())(( 11 SGaJG

)())(( 22 SGaJG

G

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

!?


40/66


Damped Least Squares

VJIJJ TT q )( 2

G

VJIJJ

TT 12

)(

q VIJJJ TT 12 )( q

VIJJf T 12 )(

VfIJJT )( 2

qfJT

substitution

Solve


41/66


Hueristic Human-Like Linkage (HAL)

3 DoF

3 DoF

1 DoF

G

Decompose into simpler subproblems

Fix wrist positionuse as Goal

7 DoF linkage

RA(q1,q2 ,q3, RB( q4), RC(q5,q6 ,q7

Set hand position and rotation

based on relative position of

Goal to shoulder


42/66



Set q4based on distance

between shoulder and wrist

s

e

w

Assume axis of elbow is

perpendicular to plane defined

by s, e, w use law of cosines

t

tLL

LLL

swL

ewL

seL

qq

q

180

2)cos(

4

21

22

2

2

1

2

1

L1 L2

L


43/66



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 & q4

q4


44/66



From e and w and hand orientation, determine RB

s

w

e

From e and s, determine RA

q4


45/66


Traverse linkage from distal joint inwards

Optimally set one joint at a time

Update 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


46/66



)()()( qEqEqE op

2

)()( cdp PPqE 2

3

1

)1)(()(

jc

j

jdo uuqE .


47/66



icaxisic PRP i )()(

idicp PPg )()(

)()(3

1

jcj

jdo uug

)()()( oopp gwgwg

Rotational joint:

.


48/66



)()()( oopp gwgwg

1ow)1( a pw

Wk/a

),max(

),min(

icid

icid

PP

PP

Rotational joint:

.


49/66



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

kkkg

)()())((3

1

1 ijc

j

ijdoiiciidp axisuaxisuwaxisPaxisPwk

)()(

3

12 jc

jjdoicidp uuwPPwk

)]()([3

1

3 jc

j

jdoicidpi uuwPPwaxisk

Rotational joint:

.


50/66



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

0)cos()sin()( 321 kkk

))(

(tan12

31

kk

k

c c

iii wqq

Rotational joint:

.


51/66



iicid axisPP )(

Translational joint:

..


52/66


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 wouldviolate constraints.


53/66


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.


54/66


Constraints

0)( qC

0)(

q

q

CqC

j

i

c q

c

J

.

To maintain constraints, need:

Notation:


55/66


Constraints

0)( qqCqC

KgFJKq T

KgJC c

.Generalized forceConstraint Jacobian

IK Jacobian


56/66


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


57/66


Computing the constraint force

ca ggg

accc KgJKgJ To counteract gas

affect on constraints:

.

Applied force Yet to be determined constraint force

g should lie in the nullspace of JcK 0KgJc


58/66


Computing the constraint force

ac

T

cc

KgJKJJ

.

Solve linear system to find Lagrange multiplier vector.

The system is usually underconstrained

Restrict gcto move the system in a direction it may not go

cc Jg


59/66


Solving for Lagrange Multipliers

],...,[ 1 m

ac

T

cc 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


60/66



T

ccKJJUse truncated SVD with backsubstitution onTUDVA

diagonalRange basis Nullspace basis.

acTcc KgJKJJ

bA


61/66



bAUDVT

i

i

i

T

TT

TT

T

Vw

bUbUVD

bUDVbUDV

bUDV

1

1


62/66


Feedback term

T

cca kCJggKq )(

Spring that penalizes deviation from constraints.


63/66


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


64/66


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

.


65/66


Dataflow approach

..x q

x

f

q

x

x

f

q

f

)(xf

q

f

Constraint

function block

Knows how to compute

Its function in term of x

Knows its Jacobian wrt x


66/66

Example network

..

c1

JcC

h1 h2 h3 h4

q

c2

L92_InverseKinematics

Documents

Transcript of L92_InverseKinematics