Inverse Kinematics

Amirkabir University of TechnologyComputer Engineering & Information Technology Department

Course site:http://ce.aut.ac.ir/~shiry/lectures.html

Inverse KinematicsGiven a desired position (P) & orientation (R) of the end-effector

Find the joint variables which can bring the robot to the desired configuration.

),,( 21 nq

x

y

z ),,,,,( TAOzyxY

A Simple Example

1

X

Y

S

Revolute and Prismatic Joints Combined

(x , y)

Finding :

)x

yarctan(θ

More Specifically:

)x

y(2arctanθ arctan2() specifies that it’s in the

first quadrant

Finding S:

)y(xS 22

SolvabilityGiven the numerical value of we attempt tofind values of

TN0

.,,, 21 n

The PUMA 560:

Given as 16 numerical values, solve for6 joint angles,

12 equations and 6 unknowns

6 equations and 6 unknowns (nonlinear, transcendental equations)

T06

.,,, 621

Inverse Kinematics

More difficult. The equations to solve are

nonlinear thus systematic closed-form solution is not always available.

Solution not unique. Redundant robot. Elbow-up/elbow-down

configuration. Robot dependent.

2 solutions!

The Workspace

Workspace: volume of space which can be reached by the end effectorDextrous workspace: volume of space where the end effector can be arbitrarily orientedReachable workspace: volume of space which the robot can reach in at least one orientation

Two-link manipulatorIf l1 = l2 reachable work space is a disc of radius 2l1. The dextrous Workspace is a point: origin

If l1 l2 there is no dextrous workspace. The reachable work space is a ring of outer radius l1 + l2 and inner radius l1 - l2

Existence of Solutions

A solution to the IKP exists if the target belongs to the workspace.Workspace computation may be hard. In practice it is made easy by special design of the robot.

Multiple SolutionsThe IKP may have more than one solution. We need to be able to calculate all the possible solutions.

The system has to be able to choose one.

2 solutions!

The closest solution: the solution which minimizes the amount that each joint is required to move.

The Closest Solution in Joint Space

Weights might be applied.

Selection favors moving smaller joints rather than moving larger joints when a choice exist.

The presence of obstacles.

Two Possible Solutions

In the absence of obstacles the upper configuration is selected

Number of Solutions

.180

,

,180

66

55

44

Another 4 solution

4 Solutions of the PUMA 560

Depends upon the number and range of joints and also is a function of link parameters ,a,d

8 solutions exits

Number of Solutions vs. Nonzero ai

0

0

0

0

3

53

531

iaAll

a

aa

aaa

16

16

8

4

The more the link length parameters are nonzero, the bigger the maximum number of solutions!

Solvability

All systems with revolute and prismatic joints having total of 6 D.O.F in a single series chain are solvable.But this general solution is a numerical one. Robots with analytic solution: several intersecting joint axes and/or many i = 0, 90o.

Methods of Solutions

A manipulator is solvable if the joint variables can be determined by an algorithm. The algorithm should find all possible solutions.

closed form solutions

numerical solutions• Solutions

Numerical Solutions

Results in a numerical, iterative solution to system of equations, for example Newton/Raphson techniques.Unknown number of operations to solve.Only returns a single solution.Accuracy is dictated by user.Because of these reasons, this is much less desirable than a closed-form solution.Can be applied to all robots.

Closed-form solutions

Analytical solution to system of equations

Can be solved in a fixed number of operations (therefore, computationally fast/known speed)

Results in all possible solutions to the manipulator kinematics

Often difficult or impossible to find

Most desirable for real-time control

Most desirable overall

Given a 6 axis robot. It can be proven that there exists a closed

form solution for inverse kinematics: If three adjacent revolute joint axes

intersect at a point (PUMA, Stanford). If three adjacent revolute joint axes are

parallel to one another (MINIMOVER).

In any case algebraic or geometric intuition is required to obtain closed form solutions.

Closed-form solutions

Methods of Solutions

In general the IK problem can be solved by various methods such as:Inverse Transform ( Paul, 1981)Screw Algebra (Kohli 1975)Dual Matrices (Denavit 1956)Iterative (Uicker 1964)Geometric approach (Lee 1984)Decoupling of position and orientation (Pieper 1968)

Closed-form Solutions

We are interested in closed-form solutions:

1. Algebraic methods

2. Geometric methods

Algebraic solution

TTTTTBW

23

12

01

03

Consider a 3-link manipulator

We can derive kinematic equations:

i i-1 i-1 di i

1 0 0 0 1

2 0 L1 0 2

3 0 L2 0 3

1

X

3

Y

1

Y2

X2

Y

3

X

0

X

0

Y

Algebraic solutionD-H transformation

Algebraic Solution

1000

0100

00

0

1000

0100

00

0

1000

0100

00

00

1000

0

33

233

23

22

122

12

11

11

1000101

1000101

011

01

cs

Lsc

Tcs

Lsc

T

cs

sc

dccscss

dsscccs

asc

T

Algebraic SolutionThe kinematics of the example seen before are:

1000

0100

0

0

12211123123

12211123123

03

slslcs

clclsc

TTBW

Assume goal point is the specification of wrist frame, specified by 3 numbers:

1000

0100

0

0

ycs

xsc

TBW

Algebraic SolutionBy comparison, we get

the four equations:

12211

12211

123123

slsly

clclx

sscc

Summing the square of the last 2 equations: 221

22

21

22 2 cllllyx

From here we get an expression for c2

21

22

21

22

2 2 ll

llyxc

).,(2tan1 222222 csAcs And finally:

Algebraic Solution

The arc cosine function does not behave well as its accuracy in determining the angle is dependant on the angle ( cos(q)=cos(-q)).When sin(q) approaches zero, division by sin(q) give inaccurate solutions.Therefore an arc tangent function which is more consistent is used.

),(2tan csA

Using c12=c1c2-s1s2 and s12= c1s2-c2s1:

1211

1211

cksky

skckx

where k1=l1+l2c2 and k2=l2s2.

To solve these eqs, set

r=+ k12+k2

2 and =Atan2(k2,k1).

Algebraic Solution

Algebraic Solutionk1

k22

l1

l2

Then: k1=r cos , k2=r sin , and we can write:

x/r= cos cos 1 - sin sin 1

y/r= cos sin 1 + sin cos 1

or: cos(+1) = x/r, sin(+1) =y/r

Algebraic Solution

Therefore:

+1 = atan2(y/r,x/r) = atan2(y,x)

And so:

1 = atan2(y,x) - atan2(k2,k1)

Finally, 3 can be solved from:

1+ 2+ 3 =

Geometric Solution

x

y

a1

a2

Two-link Planar Manipulator

Geometric SolutionApplying the “law of cosines”:

We should try to avoid using the arccos function because of inaccuracy.

Geometric Solution

The second joint angle 2is then found accurately as:

Because of the square root, two solutions result:

Geometric Solution

1 is determined uniquely given 2

1=-

Note the two different solutions for 1 corresponding to the elbow-down versus elbow-up configurations

Examples: IK Solution for PUMA 560

Algebraic Solution for UNIMATION PUMA 560

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

1000

6565

454

343

232

121

01

333231

232221

131211

06 TTTTTT

prrr

prrr

prrr

Tz

y

x

We wish to Solve

For i when 06T is given as numeric values

(4.54)

Examples: IK Solution for PUMA 560

Paul (1981) suggest pre multiplying the above matrix (4.54) by its unworn inverse transform successively and determine the unknown angle from the elements of the resultant matrix equation.

Examples: IK solution for PUMA 560

)()()()()()( 6565

454

343

232

12

06

1

101 TTTTTTT

Tprrr

prrr

prrr

cs

sc

z

y

x

16

333231

232221

131211

11

11

10001000

0100

00

00

By multiplying both sides of (4.54)

Inverting 01T we get:

(4.56)

Examples: IK Solution for PUMA 560

16T from (3.13) is:

1000

][][

][][

2342223323554236523465462365234654623

3456465464654

2342332223554236523465462365234654623

16 cdsasaccscssssscccsscscsscccs

dssccscsscscs

sdcacascsccsssscccscssssscccc

T

Examples: IK solution for PUMA 560

By adapting 16T from (3.13) and

equating elements we have 311 dpcps yx

sin

cos

y

x

p

p

),(2tan

22

xy

yx

ppA

pp

3

11

dcssc

Using trigonometric substitution:

We obtain

Using difference of angles: 3

1)sin(d

Examples: IK Solution for PUMA 560

And so:2

23

1 1)cos(

d

2

233

1 1,2tan

ddA

23

2231 ,2tan,2tan dppdAppA yxxy

Solution for 1:

Note: two possible solution for 1

Examples: IK Solution for PUMA 560

22234233

2223423311

sacdsap

casdcapspc

z

yx

By equating elements (1,4) and also (3,4) from Equ. (4.56):

By squaring and addition of resulting equations and Equ. (4.57):

2

24

23

23

22

222

3433

2a

ddaapppK

Ksdca

zyx

Where

Examples: IK Solution for PUMA 560

The above equation is only dependant on 3 so with similar procedure we get:

224

23433 ,2tan),(2tan KdaKAdaA

again two different solution for 3.

Tprrr

prrr

prrr

dcs

sacsssc

cascscc

saytoisthat

TTTTT

z

y

x

36

333231

232221

131211

311

3223231231

3223231231

6565

454

34

06

1

203

10001000

0

)()()()(

We may rewrite (4.54) as

(4.70)

Examples: IK Solution for PUMA 560

211

2

1132343223

211

2

43211323

43223231231

33223231231

)(

))(()(

)(

))(()(23

yxz

yxz

yxz

yxz

zyx

zyx

pspcp

pspccaapdsac

pspcp

dsapspcpcaas

dsapcpsspsc

acapspcspcc

)])(()(

),)(()[(2tan

11323432

3241132323

yxz

yxz

pspccaapdsa

sadpspcpcaaA

3232

By adapting 36T from (3.11) and equating

(1,4) and (2,4) we have

We solve for 23 as

Four possible solutions for 2

Examples: IK Solution for PUMA 560

By equating elements (1,3) and also (3,3) from (4.70) :

54123113

5423332312323113

sscrsr

scsrcsrccr

),(2tan 233323123231131231134 srcsrccrcrsrA

05 sif

Examples: IK Solution for PUMA 560

),(2tan

)()()(

555

6565

45

06

104

csA

TTTT

),(2tan

)()(

666

656

06

105

csA

TTT

We continue similar procedure for rest of the Parameters…

Examples: IK Solution for PUMA 560

After all eight solutions have been computed, some of them may have to be discarded because of joint limitations. Usually the one closest to present manipulator configuration is chosen.

180

180

66

5'5

4'4

Other possible solutions :

Pieper’s Solution When Three Axes Intersect (E.G, Spherical Wrists)

A completely general robot with six degrees of freedom does not have a closed form solution, however special cases can be solved.The technique involves decoupling the position and orientation problems. The position problem positions the wrist center, while the orientation problem completes the desired orientation.

Pieper’s Solution

This method applies to all six joints revolute, with the last 3 intersection

It can be applied to majority of industrial robots

{0}{0}

ZZ44

ZZ55

ZZ66PPcc

dd

dd66

Pieper’s Solution: Basic ConceptFirst, the location of the wrist center, pc is

found from the given tool position (d) and the tool pointing direction (here z6). Since

the wrist center location depends on the first three joint variables, this results in three equations and three unknowns which are solved for 1 – 3. Then, the relative

wrist orientation R63 which is a function of

the last three joint variables, 4 – 6 is found

from the arm orientation R30 and the given

tool orientation R60. The relative wrist

orientation is set equal to the kinematic description of R6

3 and the last 3 joint

variables are solved.

Pieper’s Solution Step-by-step Procedure

Start with the given tool pose, as .Solve portions of the forward kinematics to find find .Find the location of the wrist center, as (last column of ) - (tool offset length) x (third column of ).Set = last column of and solve for for .Solve as (put into ). Set equal to and solve for . Along the way, keep track of the number of solutions for each joint variable.

T06

),,(),,,( 65436321

03 RT

cP 6dT0

6 6d 6Z

cP ),,( 32103 T

321 ,, R3

6 RR 06

103

321 ,, ),,( 321

03 R

),,( 65436 RR3

6 654 ,,

T06

Standard Frames

{B}

{W}

{T}

{G}{S}

Repeatability and accuracy

Repeatability: how precisely a manipulator can return to a taught point?Accuracy: the precision with which a computed point can be attained.