Inverse Manipulator Kinematics - UCLAbionics.seas.ucla.edu/education/MAE_263D/MAE_263D_C04_V01.pdf · • Solution (Inverse Kinematics)- A “solution” is the set of joint variables

Inverse Manipulator Kinematics

Review

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA

Direct Versus Inverse Kinematics

Direct (Forward) Kinematics

Given: Joint angles and links geometry

Compute: Position and orientation of the end

effector relative to the base frame

Inverse Kinematics

Given: Position and orientation of the end

effector relative to the base frame

Compute: All possible sets of joint angles and

links geometry which could be

used to attain the given position and

orientation of the end effetor

TTf N

B

T

0)(

)()( 011 TfTf N

B

T



Central Topic - Inverse Manipulator Kinematics -

Examples

• Geometric Solution - Concept

Decompose spatial geometry into several plane

geometry

Examples - RRR (3R) manipulators -

Geometric Solution

• Algebraic Solution - Concept

Examples - PUMA 560 - Algebraic Solution

1000

333231

232221

131211

10

1

0

z

y

x

N

NNprrr

prrr

prrr

TTT

Direct Kinematics Goal (Numeric values)



Solvability

• Existence of Solutions

• Multiple Solutions

• Method of solutions

– Close form solution

• Algebraic solution

• Geometric solution

– Numerical solutions



Solvability - Existence of Solution

• For a solution to exist, must be in the workspace of the manipulator

• Workspace - Definitions

– Dexterous Workspace (DW): The subset of space in which the robot end

effector can reach all orientation.

– Reachable Workspace (RW): The subset of space in which the robot end

effector can reach in at least 1 orientation

• The Dexterous Workspace is a subset of the Reachable Workspace

TN

0

RWDW



Solvability - Existence of Solution - Workspace - 2R

Example 1 - 21 LL

Reachable

Workspace

Dexterous

Workspace




Example 2 - 21 LL

Reachable

Workspace

NO Dexterous

Workspace




Example 3 - 21 LL

Reachable

Workspace

&

Dexterous

Workspace

End Effector

Rotation



Solvability - Multiple Solutions

• Problem: The fact that a manipulator

has multiple multiple solutions may

cause problems because the system

has to be able to choose one

• Solution: Decision criteria

– The closest (geometrically) -

minimizing the amount that each

joint is required to move

• Note 1: input argument -

present position of the

manipulator

• Note 2: Joint Weight -

Moving small joints (wrist)

instead of moving large

joints (Shoulder & Elbow)

– Obstacles exist in the workspace

- avoiding collision



Solvability - Multiple Solutions

• Multiple solutions are a common

problem that can occur when solving

inverse kinematics because the

system has to be able to chose one

• The number of solutions depends on

the number of joints in the

manipulator but is also a function of

the links parameters

• Example: The PUMA 560 can reach

certain goals with 8 different

(solutions) arm configurations

– Four solutions are depicted

– Four solutions are related to a

“flipped” wrist

iai id

i

o

o

180

180

66

55

44



Solvability - Multiple Solutions - Number of Solutions

• Task Definition - Position the end

effector in a specific point in the

plane (2D)

• No. of DOF = No. of DOF of the task

Number of solution: 2

(elbow up/down)

• No. of DOF > No. of DOF of the task

Number of solution:

Self Motion - The robot can be

moved without moving the the end

effector from the goal



Solvability - Methods of Solutions

• Solution (Inverse Kinematics)- A “solution” is the set of joint variables

associated with an end effector’s desired position and orientation.

• No general algorithms that lead to the solution of inverse kinematic equations.

• Solution Strategies

– Closed form Solutions - An analytic expression includes all solution sets.

• Algebraic Solution - Trigonometric (Nonlinear) equations

• Geometric Solution - Reduces the larger problem to a series of plane

geometry problems.

– Numerical Solutions - Iterative solutions will not be considered in this

course.



Solvability

Robot - 6 DOF

Single Series Chain Revolute & Prismatic Joints

Analytic Solution

Numeric Solution

Close Form Solution Sufficient Condition

Three adjacent axes

(rotary or prismatic)

must intersect

Industrial

Robots

Real-Time Non

Real-Time

Iterations



Inverse Kinematics -

Generalized Algebraic (Analytical) Solutions – Case 1-7




Generalized Algebraic (Analytical) Solutions – Case 1

• Equation

• Solution (Unique)

[-1,1]bco

[-1,1]a sin

bs

a

),(2tan baA





• Equation

• Solution

– Two Solutions

– Singularity at the Boundary

21co

[-1,1]a sin

as

a

)1,(2tan 2aaA

1802

1

1a , 90 oWhen

21sin

[-1,1]b cos

b

b

),1(2tan 2 bbA

2

1

1b , ,1800 0 oWhen





• Equation

• Solution

– Two Solutions apart

– Singularity

b

a

s

bs

co

sin

0)(sin)a(co

),(2tan

),(2tan

baA

baA

1800

0b





• Equation

0,,

)(sin)a(co

cba

cbs









•

• Solution

– Two Solutions

– For a solution to exist

– No solution (outside of the workspace)

– One solution (singularity)

),(2tan ),(2tan 222 abAccbaA

0222 cba

0222 cba

0222 cba





• Equation

• Solution

bs

a

sinco

sinsin

negative is sin if ),(2tan

positive is sin if ),(2tan

baA

baA





• Equation

• Solution

– For an exiting solution (the determinant must be positive)

gfse

dcsa

sinco

sinco

),(2tan cgdfdeagA

0ceaf





• Transcendental equations are difficult to solve because they are a function of

• Making the following substitutions yields an expression in terms of a single

veritable , Using this substitutions, transcendental equation are converted

into polynomial equation

sc ,kscf ),(

u

2

2

2

1

2sin

1

1cos

2

tan

u

u

u

u

u





• Transcendental equation

• Substitute with the following equations

• yields

cbsac

2

2

2

1

2sin

1

1cos

u

u

u

u

sc ,

0)(2)(

)1(2)1(

2

22

acbuuca

ucbuua





• Which is solved by the quadratic formula to be

• Note

– If u is complex there is no real solution to the original transcendental

equation

– If then

ca

cabbu

222

ca

cabb 2221tan2

0 ca o180




Geometric Approach



Inverse Kinematics - 3D RRR (3R) -

Geometric Solution - 1/

• Given:

– Manipulator Geometry

– Goal Point Definition: The position of the wrist in space

• Problem:

What are the joint angles ( ) as a function of the goal (wrist position and

orientation)

ddd zyx ,,

321 ,,









• The planar geometry - top view of the robot

x0

y0

xd

yd

z2

z3

L3

L4

1

),(2tan1 dd xyA

22

1 dd yxr

x2

x3









• The planar geometry - side view of the robot:

• where

z0

L1+L2 zd

L3

L4

r2

r1

221

2222222

12 )(ˆˆ LLzyxzyxzrr ddddd

z

3

)(ˆ21 LLzz d





• By Apply the law of cosines we get

• Rearranging gives

• and

• Solving for we get

• Where is defined above in terms of known parameters

)cos(2)180cos(2 343

2

4

2

3343

2

4

2

3

2

2 LLLLLLLLr

43

2

4

2

3

2

23

2

)(

LL

LLrc

2

33 1 cs

),1tan( 3

2

33 ccA

3

3cddd zyxLL and,,,4,3





• Finally we need to solve for

• where

L3

L4

r2

r1

α

β

2

3

2

2

),ˆ(2tan 1rzA

22

1 dd yxr )(ˆ21 LLzz d

z





• Based on the law of cosines we can solve for

)cos(2 32

2

3

2

2

2

4 LrLrL

32

2

4

2

3

2

2

2 Lr

LLrc

),1(2tan 2

ccA

),1(2tan)),((2tan 222

212 ccAyxLLzA ddd





• Summary

)

))((2

)(,

))((2

)(1(2tan

)),((2tan

2

321

22

2

4

2

3

2

21

222

2

321

22

2

4

2

3

2

21

22

22

212

LLLzyx

LLLLzyx

LLLzyx

LLLLzyxA

yxLLzA

ddd

ddd

ddd

ddd

ddd

),(2tan1 dd xyA

)

2

)()(,

2

)()(1tan(

43

2

4

2

3

2

21

222

43

2

4

2

3

2

21

22

3LL

LLLLzyx

LL

LLLLzyxA dddddd




Analytical Approach




Examples



1000

333231

232221

131211

10

1

0

z

y

x

N

NNprrr

prrr

prrr

TTT




Solvability - PUMA 560

Given : PUMA 560 - 6 DOF,

Solve:

Total Number of Equations: 12

Independent Equations: 3 - Rotation Matrix

3 - Position Vector

Type of Equations: Non-linear

61

1000

333231

232221

131211

5

6

4

5

3

4

2

3

1

2

0

1

0

6

z

y

x

prrr

prrr

prrr

TTTTTTT

T0

6




Examples

• Geometric Solution - Concept

Decompose spatial geometry into several plane

geometry

Examples - Planar RRR (3R) manipulators -

Geometric Solution



1000

333231

232221

131211

10

1

0

z

y

x

N

NNprrr

prrr

prrr

TTT




Inverse Kinematics - PUMA 560 -

Algebraic Solution - 1/

• Given:

– Direct Kinematics: The homogenous transformation from the base to the

wrist

– Goal Point Definition: The position and orientation of the wrist in space

TB

W





• Problem:

What are the joint angles ( ) as a function of the wrist position and

orientation ( or when is given as numeric values)

61 T0

6

1000

)()()()()()(333231

232221

131211

6

5

65

4

54

3

43

2

32

1

21

0

1

0

6

z

y

x

prrr

prrr

prrr

TTTTTTT

Goal Direct Kinematics





• Solution (General Technique): Multiplying each side of the direct kinematics

equation by a an inverse transformation matrix for separating out variables in

search of solvable equation

• Put the dependence on on the left hand side of the equation by multiplying

the direct kinematics eq. with gives

)()()()()()()]([)]([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

1

0

1

0

6

1

1

0

1 TTTTTTTTT

1

1

0

1 )]([ T1

I

)()()()()()()],,,([)],,,([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

4321

0

4

0

6

1

4321

0

4 TTTTTTTTT

)()()()()()()]([)],,([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

1

0

3

0

6

1

321

0

3 TTTTTTTTT

)()()()()()()],,,,([)],,,,([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

54321

0

5

0

6

1

54321

0

5 TTTTTTTTT





• Put the dependence on on the left hand side of the equation by multiplying

the direct kinematics eq. with gives

)()()()()()()]([)]([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

1

0

1

0

6

1

1

0

1 TTTTTTTTT

1

1

0

1 )]([ T1

I

1000

0100

00

00

11

11

0

1

cs

sc

T

1000

][ 1 BORG

ATA

B

TA

BB

A

A

B

PRRTT

1000

0100

00

00

][11

11

10

1

1

0

cs

sc

TT





Tprrr

prrr

prrr

cs

sc

z

y

x

1

6

333231

232221

131211

11

11

10001000

0100

00

00





1000

00

0100

00

22

22

1

2

cs

sc

T

1000

100

00

0

3

33

233

2

3d

cs

asc

T

1000

00

100

0

44

4

344

3

4

cs

d

asc

T

1000

00

0100

00

55

55

4

5

cs

sc

T

1000

00

0100

00

66

66

5

6

cs

sc

T

1000

'

33

'

32

'

31

'

'

23

'

22

'

21

'

'

13

'

12

'

11

'

1

6

z

y

x

prrr

prrr

prrr

T





• Solution for

• Equating the (2,4) elements from both sides of the equation we have

• To solve the equation of this form we make the trigonometric substitution

Tprrr

prrr

prrr

cs

sc

z

y

x

1

6

333231

232221

131211

11

11

10001000

0100

00

00

311 dpcps yx

sin

cos

y

x

p

p

1





• Substituting with we obtain

• Using the difference of angles formula

),(2tan

22

yx

yx

ppA

pp

yx pp , ,

311

dcssc

3

1)sin(d





• Based on

• and so

• The solution for may be written

• Note: we have found two possible solutions for corresponding to the +/- sign

1)(cos)(sin 1

2

1

2

2

2

31 1)cos(

d

2

2

331 1,2tan

ddA

2

2

331 1,2tan),(2tan

ddAppA xy

1

1





• Solution for

• Equating the (1,4) element and (3,4) element

• We obtain

Tprrr

prrr

prrr

cs

sc

z

y

x

1

6

333231

232221

131211

11

11

10001000

0100

00

00

2223423311 casdcapspc yx

22234233 casdcapz

3





• If we square the following equations and add the resulting equations

311 dpcps yx

2223423311 casdcapspc yx

22234233 sacdsapz




Algebraic Solution - 11/ (Continue)









• we obtain

• where

• Note that the dependence on has be removed. Moreover the eq. for is of

the same form as the eq. for and so may be solved by the same kind of

trigonometric substitution to yield a solution for

Ksdca 3433

2

2

4

2

3

2

3

2

2

222

2a

adaapppK

zyx

1 3

13





• Note that the +/- sign leads to two different solution for

3

22

4

2

3433 ,2tan),(2tan KdaKAdaA





• Solution for

• Equating the (1,4) element and (2,4) element we obtain

• These equations may be solved simultaneously for and resulting in

)()()()()()()]([)],,([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

1

0

3

0

6

1

321

0

3 TTTTTTTTT

1000

0

10001000

0 546465464654

456565

3546465464654

333231

232221

131211

311

3223231231

3223231231

ssccscsscccs

dcsscs

asccssccssccc

prrr

prrr

prrr

dcs

sacsssc

cascscc

z

y

x

33223231231 acapspcspcc zyx

43223231231 dsapcpsspsc zyx

23s23c

I2









• Since the denominator are equal and positive, we solve for the sum of and

as

• The equation computes four values of according to the four possible

combination of solutions for and

2

11

2

43211223

23)(

))(()(

yxz

yxz

pspcp

dsapspcpcaas

2

11

2

11323432

23)(

))(()(

yxz

yxz

pspcp

pspccaapdsac

2 3

)])(()(

),)(()[(2tan

11323432

4321122323

yxz

yxz

pspccaapdsa

dsapspcpcaaA

23

31





• Then, four possible solutions for are computed as

• Solution for

• Equating the (1,3) and the (3,3) elements

• we get

3232

2

1000

0

10001000

0 546465464654

456565

3546465464654

333231

232221

131211

311

3223231231

3223231231

ssccscsscccs

dcsscs

asccssccssccc

prrr

prrr

prrr

dcs

sacsssc

cascscc

z

y

x

5433232312323113 scrscsrccr

5

1231134

s

crsrs

4

5

332323123231134

s

rscsrccrc

54123113 sscrsr





• As long as we can solve for

• When the manipulator is in a singular configuration in which joint axes

4 and 6 line up and cause the same motion of the last link of the robot. In this

case all that can be solved for is the sum or difference of and . This

situation is detected by checking whether both arguments of Atan2 defining

are near zero. If so is chosen arbitrary (usually chosen to be equal to the

present value of joint 4), and is computed later.

05 s 4

),(2tan 332323123231131231134 rscsrccrcrsrA

05

4

5

6

4 6

4

6

4





• Solution for

• Equating the (1,3) and the (3,3) elements we get

• We can solve for

)()()()()()()],,,([)],,,([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

4321

0

4

0

6

1

4321

0

4 TTTTTTTTT

1000

0

00

0

10001000

56565

66

56565

333231

232221

131211

43223231231

4343432423414231414231

4343432423414231414231

csscs

cs

ssccc

prrr

prrr

prrr

dsacsssc

sacdscassccscscsscc

casdccacsscccsssccc

z

y

x

5423334142312341423113 )()()( scsrscccsrsscccr

523332312323113 )()()( ccrssrscr

5

),(2tan 555 csA

5





• Solution for

• Equating the (3,1) and the (1,1) elements we get

• We can solve for

)()()()()()()],,,,([)],,,,([ 6

5

65

4

54

3

43

2

32

1

21

0

1

1

54321

0

5

0

6

1

54321

0

5 TTTTTTTTT

6523152313123215231523111 )()()( scsssscrcrssscscr

654154231541542313142321541542315415423111 )()()( ccsccccssssscccrcsrsscsccscssccccr

1000

00

0100

00

10001000

66

66

333231

232221

131211

432523152312352315231

434343254154231541542314235415423154154231

434343254154231541542314235415423154154231

cs

sc

prrr

prrr

prrr

dsacssssccssscsc

sacdscaccccscsscsssccsssccsscsccscscc

casdccacsccccsssssccccssscsccscsscccc

z

y

x

6 ),(2tan 666 csA

6





• Summary - Number of Solutions

• Four solution

• For each of the four solutions the wrist can be

flipped

22

4

2

3433 ,2tan),(2tan KdaKAdaA

2

2

331 1,2tan),(2tan

ddAppA xy

o

o

180

180

6

'

6

5

'

5

4

'

4





• After all eight solutions have been computed, some or all of them may have to

be discarded because of joint limit violations.

• Of the remaining valid solutions, usually the one closest to the present

manipulator configuration is chosen.



Pieper’s Solution - Three consecutive Axes Intersect

• High Level Introduction to the Solution




• High Level Introduction to the Solution




• When the last three axes of a 6 DOF robot intersect, the origins of link frame {4},

{5}, and {6} are all located at the point of intersection. This point is given in the

base coordinate system as

• From the general forward kinematics method for determining homogeneous

transforms using DH parameters, we know:

orgorg PTTTP 4

32

3

1

2

0

14

0

1000

0

1111

1111

1

1

iiiiiii

iiiiiii

iii

i

idccscss

dsscccs

asc

T

Ri

i

1

iorg

i P1




• For i=4

• Using the fourth column and substituting for , we find

1000

0

4333434

4333434

344

3

4dccscss

dsscccs

asc

T

1

43

43

3

2

3

1

2

0

14

32

3

1

2

0

14

0

dc

ds

a

TTTPTTTP orgorg

orgP4

3

orgP4

3R3

4

1

)(

)(

)(

33

32

31

1

2

0

14

32

3

1

2

0

14

0

f

f

f

TTPTTTP orgorg




• where

11

)(

)(

)(

43

43

3

2

3

33

32

31

dc

ds

a

Tf

f

f

11000

0

1

)(

)(

)(

43

43

3

3222323

3222323

233

33

32

31

dc

ds

a

dccscss

dsscccs

asc

f

f

f

23324323432333

23324323432332

23343331

cdccdcssdssaf

sdcsdccsdscaf

assdcaf




• Repeating the same process again

1

)(

)(

)(

33

32

31

1

2

0

14

32

3

1

2

0

14

0

f

f

f

TTPTTTP orgorg

1

)(

)(

)(

1

)(

)(

)(

33

32

31

1

2

23

22

21

f

f

f

Tg

g

g

1

)(

)(

)(

1000

0

1

)(

)(

)(

33

32

31

2111212

2112212

122

23

22

21

f

f

f

dccscss

dsscccs

asc

g

g

g




• Repeating the same process for the last time

123121211223

123121211222

1221221

cdfcfscfssg

sdfsfccfssg

afsfcg

1

)(

)(

)(

22

22

21

0

14

32

3

1

2

0

14

0

g

g

g

TPTTTP orgorg

1

)(

)(

)(

1000

0

23

22

21

1000101

1000101

011

4

0

g

g

g

dccscss

dsscccs

asc

P org




• Frame {0} - The frame attached to the base of the robot or link 0 called frame

{0} This frame does not move and for the problem of arm kinematics can be

considered as the reference frame.

• Assign {0} to match {1} when the first joint veritable is zero

1

)(

)(

)(

1000

0

23

22

21

1000101

1000101

011

4

0

g

g

g

dccscss

dsscccs

asc

P org

0 0 0101 ad

1

3

2111

2111

4

0

g

gcgs

gsgc

P org




• Through algebraic manipulation of these equations, we can solve for the desired

joint angles ( ).

• The first step is to square the magnitude of the distance from the frame {0}

origin to frame {4} origin.

• Using the previously define function for we have

321 ,,

2

3

2

2

2

1

2

4

02

4

02

4

02 gggPPPr orgzorgyorgx

ig

2212132

2

2

2

1

2

3

2

2

2

1

2 2 fsfcafddafffr




• Applying a substitution of temporary variables, we can write the magnitude

squared term along with the z-component of the {0} frame origin to the {4} frame

origin distance.

• These equations are useful because dependence on has been eliminated,

and dependence on takes a simple form

123121211234

0 cdfcfscfssgPZ orgz

2212132

2

2

2

1

2

3

2

2

2

1

2 2 fsfcafddafffr

412221

312221

2 2

ksckskZ

kaskckr

12134

32

2

2

2

1

2

3

2

2

2

13

22

11

2

cdcfk

fddafffk

fk

fk

1

2




• Consider 3 cases while solving for :

• Case 1 -

• Solution Methodology - Reduction to Ploynomial => Quadratic Equation

3

01 a

3

2 kr

32

2

2

2

1

2

3

2

2

2

13 2 fddafffk

23324323432333

23324323432332

23343331

cdccdcssdssaf

sdcsdccsdscaf

assdcaf

22

2

1

2sin

1

1cos

2tan

u

u

u

uu

412221

312221

2 2

ksckskZ

kaskckr




• Case 2 -

• Solution Methodology - Reduction to Ploynomial => Quadratic Equation

01 s

4kZ

12134 cdcfk

23324323432333 cdccdcssdssaf

22

2

1

2sin

1

1cos

2tan

u

u

u

uu

412221

312221

2 2

ksckskZ

kaskckr




• Case 3 (General case) : We can find through the following algebraic

manipulation:

• squaring both sides, we find

3

2221

1

4

2221

1

3

2

2

cksks

kZ

skcka

kr

2221

2

2

2

2

2

2

2

1

2

2221

2

1

4

2221

2

2

2

2

2

2

2

1

2

2221

2

1

3

2

2

22

sckkckskcksks

kZ

sckkskckskcka

kr

412221

312221

2 2

ksckskZ

kaskckr




• Adding these two equations together and simplifying using the trigonometry

identity (Reduction to Ploynomial), we find a fourth order equation for

3

2

2

2

1

2

1

4

2

1

3

2

2kk

s

kZ

a

kr

23324323432333

23324323432332

23343331

cdccdcssdssaf

sdcsdccsdscaf

assdcaf

12134

32

2

2

2

1

2

3

2

2

2

13

22

11

2

cdcfk

fddafffk

fk

fk




• With solved, substitute into to find

412221

312221

2 2

ksckskZ

kaskckr

3 2Zr ,2

23324323432333

23324323432332

23343331

cdccdcssdssaf

sdcsdccsdscaf

assdcaf

12134

32

2

2

2

1

2

3

2

2

2

13

22

11

2

cdcfk

fddafffk

fk

fk




• With solved, substitute into to find

• Solve for using the reduction to polynomial method

32,

1

3

2111

2111

4

0

g

gcgs

gsgc

P org

orgP4

0

21114

0

21114

0

gcgsP

gsgcP

orgy

orgx

23324323432333

23324323432332

23343331

cdccdcssdssaf

sdcsdccsdscaf

assdcaf

1

123121211223

123121211222

1221221

cdfcfscfssg

sdfsfccfssg

afsfcg




• To complete our solution we need to solve for

• Since the last three axes intersect these joint angle affect the orientation of only

the last link. We can compute them based only upon the rotation portion of the

specified goal

• - The orientation of link frame {4} relative to the base frame {0} when

• are the Euler angles applied to

654 ,,

R0

6

RRR 0

60

10

40

4

6 44

0

0

4 4R

04

654 ,, 0

4

6 4R

RRR 4

60

0

4

0

6 4




Examples

• Algebraic Solution (closed form) - Piepers Method (Continue) - Last three

consecutive axes intersect at one point

• Consider a 3 DOF (wrist) non-planar robot whose axes all intersect at a point.



Mapping - Rotated Frames - Z-Y-Z Euler Angles

Start with frame {4}.

• Rotate frame {4} about by an angle



Note - Each rotation is preformed about an axis of the moving reference frame

{4}, rather then a fixed reference.

BY4Z

Euler Angles

4Z







100

0

0

0

010

0

100

0

0

)()()(),,('''

cs

sc

cs

sc

cs

sc

RRRR ZYZZYZ

csscs

ssccscsscccs

sccssccssccc

R ZYZ ),,('''

0

4

6''' 4),,( RR ZYX

A

B



Three consecutive Axes Intersect - wrist

Goal

Direct Kinematics

csscs

ssccscsscccs

sccssccssccc

rrr

rrr

rrr

333231

232221

131211




• Solve for using element

• Using the Atan2 function, we find

2222

32

2

31 scsrr

cr 33

2

32

2

31 rrs

33

2

32

2

31 ,Atan2 rrr

333231 ,, rrr

cr

ssr

csr

33

32

31




• Solve for using elements

ssr 23

scr 13

srsr /,/Atan2 1323

1323, rr




• Solve for using elements

ssr 32

csr 31

3132, rr

srsr /,/Atan2 3132




• Note: Two answers exist for angle which will result in two answers each for

angles and .

• If the solution degenerates

33

2

32

2

31 ,Atan2 rrr

srsr /,/Atan2 1323

srsr /,/Atan2 3132

0180,0 soo




• We are left with for every case. This means we can’t solve for either,

just their sum.

csscs

ssccscsscccs

sccssccssccc

rrr

rrr

rrr

333231

232221

131211

o0

100

0)()(

0)()(

100

0

0

333231

232221

131211

cs

sc

ccsssccs

csscsscc

rrr

rrr

rrr




• One possible convention is to choose

• The solution can be calculated to be

o0

0

0

csrr ,Atan2,Atan2 1112

180

0

csrr ,Atan2,Atan2 1112




• For this example, the singular case results in the capability for self-rotation.

That is, the middle link can rotate while the end effector’s orientation never

changes.



Gimbal Lock

Normal situation

The three gimbals are independent

Gimbal lock:

Two out of the three gimbals are in the

same plane, one degree of freedom is lost

http://youtu.be/zc8b2Jo7mno



http://youtu.be/zc8b2Jo7mno

Gimbal Lock – Robotics

• In robotics, gimbal lock is commonly referred to as "wrist flip", due to the use of

a "triple-roll wrist" in robotic arms, where three axes of the wrist, controlling yaw,

pitch, and roll, all pass through a common point.

• An example of a wrist flip, also called a wrist singularity, is when the path

through which the robot is traveling causes the first and third axes of the robot's

wrist to line up. The second wrist axis then attempts to spin 180° in zero time to

maintain the orientation of the end effector. The result of a singularity can be

quite dramatic and can have adverse effects on the robot arm, the end effector,

and the process.

• The importance of non-singularities in robotics has led the American National

Standard for Industrial Robots and Robot Systems — Safety Requirements to

define it as "a condition caused by the collinear alignment of two or more robot

axes resulting in unpredictable robot motion and velocities".



Documents

Inverse Manipulator Kinematics - UCLAbionics.seas.ucla.edu/education/MAE_263D/MAE_263D_C04_V01.pdf · • Solution (Inverse Kinematics)- A “solution” is the set of joint variables