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

f ( ) BTT  N0T
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

  f 1 ( BTT )  f 1 ( N0T )

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 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

 r11 r12 r13 px 
r r r23 py 
0
T  0
T N 1
T   21 22 
 r31 r32 r33 pz 
N 1 N

 
0 0 0 1

Direct Kinematics Goal (Numeric values)

Examples - PUMA 560 - Algebraic Solution

Instructor: Jacob Rosen

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

• Existence of Solutions

• Multiple Solutions

• Method of solutions

– Close form solution

• Algebraic solution
• Geometric solution

– Numerical solutions

Instructor: Jacob Rosen

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

• For a solution to exist, 0

NT 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

DW  RW

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Solvability - Existence of Solution - Workspace - 2R
Example 1 - L1  L2

Reachable
Workspace

Dexterous
Workspace

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Solvability - Existence of Solution - Workspace - 2R
Example 2 - L1  L2

Reachable
Workspace

NO Dexterous
Workspace

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Solvability - Existence of Solution - Workspace - 3R
Example 3 - L1  L2

End Effector
Rotation

Reachable
Workspace
&
Dexterous
Workspace

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 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
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 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 ai  i d i  i

• Example: The PUMA 560 can reach

certain goals with 8 different
(solutions) arm configurations
– Four solutions are depicted  4   4  180o
– Four solutions are related to a
“flipped” wrist  5   5
 6   6  180o

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 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

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 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.

Instructor: Jacob Rosen

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

Robot - 6 DOF
Single Series Chain
Revolute & Prismatic Joints

Analytic Solution Numeric Solution Non

Real-Time
Real-Time

Close Form Solution

Sufficient Condition Iterations
Industrial Three adjacent axes
Robots (rotary or prismatic)
must intersect

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 1-7

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 1

• Equation sin   a a  [-1,1]

cos  b b  [-1,1]

• Solution (Unique)   A tan 2(a, b)

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 2

• Equation sin   a a  [-1,1] cos   b b  [-1,1]

cos   1  a 2 sin    1  b 2

• Solution
  A tan 2(a, 1  a 2 )   A tan 2( 1  b 2 , b)
– Two Solutions
1   1  
 2  180    2  

– Singularity at the Boundary

When   90o , a 1 When   0o ,1800 , b 1

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 3

• Equation a(cos )  b(sin  )  0

sin  a

cos b
• Solution
– Two Solutions 1800 apart
  A tan 2(a,b)
  A tan 2(a, b)

– Singularity b0

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 4

• Equation
a(cos )  b(sin  )  c
a, b, c  0

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 4

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 4

• Solution
– Two Solutions

  A tan 2( a 2  b 2  c 2 , c)  A tan 2(b, a)

– For a solution to exist a 2  b2  c 2  0

– No solution (outside of the workspace) a 2  b2  c 2  0

– One solution (singularity) a 2  b2  c 2  0

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 5

• Equation sin  sin   a

cos sin   b

• Solution
  A tan 2(a, b) if sin is positive
  A tan 2(a,b) if sin is negative

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 6

• Equation acos  c sin   d

ecos  f sin   g

• Solution   A tan 2(ag  de, df  cg )

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

af  ce  0

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 7

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

c , s
f (c , s )  k

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

veritable u , Using this substitutions, transcendental equation are converted
into polynomial equation


u  tan
2
1 u2
cos  
1 u2
2u
sin  
1 u2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 7

• Transcendental equation

ac  bs  c

• Substitute c , s with the following equations

1 u2
cos  
1 u2
2u
sin  
1 u2
• yields
a(1  u 2 )  2bu  c(1  u 2 )
(a  c)u 2  2bu  (c  a)  0

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 7

• Which is solved by the quadratic formula to be

b  b2  a 2  c2
u
ac

 b  b2  a 2  c2 
  2 tan  1 
 ac 
 

• Note

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

equation

– If a  c  0 then   180o

Instructor: Jacob Rosen

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

Geometric Approach

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 1/

• Given:
– Manipulator Geometry
– Goal Point Definition: The position xd , yd , zd of the wrist in space

• Problem:
What are the joint angles ( 1, 2 ,3 ) as a function of the goal (wrist position and
orientation)

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 2/

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 3/

• The planar geometry - top view of the robot

yd x3
L4

y0 L3
1
x2 z3

x0 xd

z2

1  A tan 2( yd , xd )

r1  xd2  yd2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 4/

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 5/

• The planar geometry - side view of the robot:

 3
L4
L3
r2
ẑ
r1

L1+L2
zd

z0

r2  r12  zˆ 2  xd2  yd2  zˆ 2  xd2  yd2  zd  ( L1  L2 )

2

• where
zˆ  zd  ( L1  L2 )

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 6/

• By Apply the law of cosines we get

r22  L23  L24  2 L3L4 cos(180  3 )  L23  L24  2 L3L4 cos(3 )

• Rearranging gives
r22  ( L23  L24 )
c3 
2 L3 L4
• and
s3  1  c32
• Solving for 3 we get

3  A tan( 1  c32 , c3 )

• Where c3 is defined above in terms of known parameters L3, L4, xd , yd , and zd

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 8/

L4
 3
β
L3
r2
α ẑ
2
r1

• Finally we need to solve for  2

2    
  A tan 2( zˆ, r1 )

• where r1  xd2  yd2 zˆ  zd  ( L1  L2 )

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 9/

• Based on the law of cosines we can solve for 

L24  r22  L23  2r2 L3 cos(  )

r22  L23  L24

c 
2r2 L3

  A tan 2(  1  c2 , c )

 2  A tan 2( zd  ( L1  L2 ), xd2  yd2 )  A tan 2(  1  c2 , c )

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 10/

• Summary

1  A tan 2( yd , xd )

 2  A tan 2( zd  ( L1  L2 ), xd2  yd2 ) 

2
 x 2  y 2  z  ( L  L ) 2  L2  L2  x 2  y 2  z  ( L  L ) 2  L2  L2
A tan 2( 1   d d d 1 2 3 4 
, d d d 1 2 3 4
)
 2 x  y  (z  (L  L )L ) 
2 2 2
2 xd  yd  ( zd  ( L1  L2 ) L3 )
2 2 2
 d d d 1 2 3 

 xd2  yd2  zd  ( L1  L2 ) 2  ( L23  L24 )  xd2  yd2  zd  ( L1  L2 ) 2  ( L23  L24 )
2

3  A tan( 1    , )
 2 L L
3 4  2 L L
3 4

Instructor: Jacob Rosen

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

Analytical Approach

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Central Topic - Inverse Manipulator Kinematics -
Examples

• Algebraic Solution - Concept

 r11 r12 r13 px 

r r r py 
0
T  0
T N 1
T   21 22 23 
 r31 r32 r33 pz 
N 1 N

 
0 0 0 1

Direct Kinematics Goal (Numeric values)

Examples - PUMA 560 - Algebraic Solution

Instructor: Jacob Rosen

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

0
Given : PUMA 560 - 6 DOF, 6T

Solve: 1 6

 r11 r12 r13 px 

r r r23 py 
6T  1T 2T 3T 4T 5T 6T 
0 0 1 2 3 4 5  21 22 
 r31 r32 r33 pz 
 
0 0 0 1

Total Number of Equations: 12

Independent Equations: 3 - Rotation Matrix

3 - Position Vector
Type of Equations: Non-linear

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Central Topic - Inverse Manipulator Kinematics -
Examples

• Geometric Solution - Concept

Decompose spatial geometry into several plane
geometry

Examples - Planar RRR (3R) manipulators -

Geometric Solution

• Algebraic Solution - Concept

 r11 r12 r13 px 
r r r23 py 
0
T  0
T N 1
T   21 22 
 r31 r32 r33 pz 
N 1 N

 
0 0 0 1

Direct Kinematics Goal (Numeric values)

Examples - PUMA 560 - Algebraic Solution

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 1/

• Given:
– Direct Kinematics: The homogenous transformation from the base to the
wrist WBT
– Goal Point Definition: The position and orientation of the wrist in space

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 2/

• Problem:
What are the joint angles ( 1  6 ) as a function of the wrist position and
orientation ( or when 0T is given as numeric values)
6

 r11 r12 r13 px 

r r r23 py 
6T  1T (1 ) 2T ( 2 ) 3T ( 3 ) 4T ( 4 ) 5T ( 5 ) 6T ( 6 ) 
0 0 1 2 3 4 5  21 22 
 r31 r32 r33 pz 
 
0 0 0 1
 
Direct Kinematics Goal

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 3/

• 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 1 on the left hand side of the equation by multiplying
the direct kinematics eq. with [01T (1 )]1 gives

I
[01T (1 )]1 06T  [01T (1 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

[03T (1,2 ,3 )]1 06T  [03T (1 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

[04T (1,2 ,3 ,4 )]1 06T  [04T (1,2 ,3 ,4 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

[05T (1,2 ,3 ,4 ,5 )]1 06T  [05T (1,2 ,3 ,4 ,5 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 4/

• Put the dependence on 1 on the left hand side of the equation by multiplying
the direct kinematics eq. with [01T (1 )]1 gives


[01T (1 )]1 06T  [01T (1 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

I
c1  s1 0 0  
 s   BART A PBORG 
c1 0
A
0 RT
1T 
0  1  [ ABT ]1  BAT   B 
 0 0 1 0  
   
 0 0 0 1 0 0 0 1 
 c1 s1 0 0
 s c 0 0
1 0 1
0T [ 1T ] 
 1 1 
 0 0 1 0
 
 0 0 0 1
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 5/

 c1 s1 0 0  r11 r12 r13 px 

 s c 0 0 r21 r22 r23 py  1
 1 1    6T
 0 0 1 0  r31 r32 r33 pz 
  
 0 0 0 1  0 0 0 1

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 6/
 c 2  s 2 0 0
 0 0 1 0
2T 
1  
 s 2  c 2 0 0
 
 0 0 0 1
c 3  s 3 0 a2 
 s c 3 0 0 
2
T   3 
 0 1 d3 
3
0
 
 0 0 0 1
 c 4  s 4 0 a3 
 ' r11 '
r12 '
r13 '
px 
 0 0 1 d4  ' ' ' ' 
4T 
3  
 r21 r22 r23 py 
6T  '
1
 s 4  c 4 0 0 

 0 0 0 1
  r31 '
r32 '
r33 '
pz 
c 5  s 5 0 0
 
 0 0  1 0
 0 0 0 1 
4
T   
 s 5 c 5 0 0
5

 
 0 0 0 1
 c 6  s 6 0 0
 0 0 1 0
6T 
5 
 s 6  c 6 0 0
 
 0 0 0 1

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 7/

• Solution for 1

 c1 s1 0 0  r11 r12 r13 px 

 s c 0 0 r21 r22 r23 py  1
 1 1    6T
 0 0 1 0  r31 r32 r33 pz 
  
 0 0 0 1  0 0 0 1

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

 s1 px  c1 p y  d 3

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

px   cos 
p y   sin 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 8/

  p x2  p y2
  A tan 2( p x , p y )

• Substituting px , p y with  , we obtain

d3
c1s  s1c 


• Using the difference of angles formula

d3
sin(  1 ) 


Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 9/

• Based on sin 2 (  1 )  cos 2 (  1 )  1

d 32
cos(  1 )   1 
• and so 2

d d 2 
  1  A tan 2 , 1  2 
3 3
   

• The solution for 1 may be written

d d 2 
1  A tan 2( p y , px )  A tan 2 3 , 1  32 
   
 
• Note: we have found two possible solutions for  1 corresponding to the +/- sign

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 10/

• Solution for  3
• Equating the (1,4) element and (3,4) element

 c1 s1 0 0  r11 r12 r13 px 

 s c 0 0 r21 r22 r23 py  1
 1 1    6T
 0 0 1 0  r31 r32 r33 pz 
  
 0 0 0 1  0 0 0 1

• We obtain c1 px  s1 p y  a3c23  d 4 s23  a2c2

 pz  a3c23  d 4 s23  a2c2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 11/

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

 s1 px  c1 p y  d 3
c1 px  s1 p y  a3c23  d 4 s23  a2c2
 pz  a3 s23  d 4c23  a2 s2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 11/ (Continue)

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 11/ (Continue)

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 12/

• we obtain

a3c3  d 4 s3  K

• where
px2  p y2  pz2  a22  a32  d 32  a42
K
2a2

• Note that the dependence on  has be removed. Moreover the eq. for  3 is of
1
the same form as the eq. for  1 and so may be solved by the same kind of
trigonometric substitution to yield a solution for  3

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 13/


 3  A tan 2(a3 , d 4 )  A tan 2 K , a32  d 42  K 2 
• Note that the +/- sign leads to two different solution for 3

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 14/

• Solution for 2 I
[03T (1,2 ,3 )]1 06T  [03T (1 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

 c1c23 s1c23  s23  a2c3   r11 r12 r13 px   c4c5c6  s4 s6  c4c5 s6  s4c6  c4 s5 a3 
 c s  s1s23  c23 a2 s3  r21 r22 r23 p y   s5c6  s5 s6  c5 d4 
 1 23   
  s1 c1 0  d 3   r31 r32 r33 pz   s4c5c6  c4 s6 s4c5s6  c4c6 s4 s5 0
    
 0 0 0 1  0 0 0 1   0 0 0 1
• Equating the (1,4) element and (2,4) element we obtain

c1c23 px  s1c23 p y  s23 pz  a2c3  a3

 c1s23 px  s1s23 p y  c23 pz  a2 s3  d 4

• These equations may be solved simultaneously for s23 and c23 resulting in

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 14/ (Continue)

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 15/

( a3  a2c2 ) pz  (c1 px  s1 p y )(a2 s3  d 4 )

s23 
pz2  (c1 px  s1 p y )2
(a2 s3  d 4 ) pz  ( a3  a2c3 )(c1 px  s1 p y )
c23 
pz2  (c1 px  s1 p y )2

• Since the denominator are equal and positive, we solve for the sum of  2 and  3
as

 23  A tan 2[( a3  a2c2 ) pz  (c1 px  s1 p y )(a2 s3  d 4 ),

(a2 s3  d 4 ) pz  ( a3  a2c3 )(c1 px  s1 p y )]

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

combination of solutions for  and 
1 3

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 16/

• Then, four possible solutions for 2 are computed as

2   23  3
• Solution for  4
• Equating the (1,3) and the (3,3) elements

 c1c23 s1c23  s23  a2c3   r11 r12 r13 px   c4c5c6  s4 s6  c4c5 s6  s4c6  c4 s5 a3 
 c s  s1s23  c23 a2 s3  r21 r22 r23 py   s5c6  s5 s6  c5 d4 
 1 23   
  s1 c 1 0  d 3   r31 r32 r33 pz   s4c5c6  c4 s6 s4c5s6  c4c6 s4 s5 0
    
 0 0 0 1  0 0 0 1  0 0 0 1
• we get
 r13c1c23  r23s1c23  s23r33
r13c1c23  r23s1c23  s23r33  c4 s5  c4 
s5
 r13s1  r23c1  s4 s5 r s r c
 s4  13 1 23 1
s5
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 17/

• As long as s5  0 we can solve for  4

4  A tan 2(r13s1  r23c1,r13c1c23  r23s1c23  s23r33)
• When 5  0 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  4 and  . This
situation is detected by checking whether both arguments of Atan2 defining  4
6

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

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

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 18/

• Solution for 5
[04T (1,2 ,3 ,4 )]1 06T  [04T (1,2 ,3 ,4 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

 c1c23c4  s1s4 s1c23c4  c1s4  s23c4  a2c3c4  d 3s4  a3c4   r11 r12 r13 px  c5c6  c5s6  s5 0
 c c s  s c  s1c23s4  c1c4 s23s4 a2c3s4  d 3c4  a3s4  r21 r22 r23 p y   s6 c6 0 0
 1 23 4 1 4   
  c1s23  s1s23 c23 a2 s3  d 4   r31 r32 r33 pz   s5c6  s5s6 c5 0
    
 0 0 0 1  0 0 0 1  0 0 0 1

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

r13 (c1c23c4  s1s4 )  r23 (s1c23c4  c1s4 )  r33 (s23c4 )  s5

r13(c1s23)  r23( s1s23)  r33(c23)  c5
• We can solve for 5
5  A tan 2( s5 , c5 )
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 19/

• Solution for6
[05T (1,2 ,3 ,4 ,5 )]1 06T  [05T (1,2 ,3 ,4 ,5 )]1 01T (1 ) 12T (2 ) 23T (3 ) 34T (4 ) 45T (5 ) 56T (6 )

 c1c23c4 c5  s1s4 c5  s1c23c4 s5  c1s4 s5 s23c4 c1c23c4 s5  s1s4 s5  s1c23c4 c5  c1s4 c5  a2 c3c4  d 3 s4  a3c4   r11 r12 r13 px 
 c c s c  s c c  s c s s  c c s  s23s4  c1c23s4 s5  s1c4 s5  s1c23s4 c5  c1c4 c5 a2 c3 s4  d 3c4  a3 s4  r21 r22 r23 p y 
 1 23 4 5 1 4 5 1 23 4 5 1 4 5 
  c1s23c5  s1s23s5  c23  c1s23s5  s1s23c5 a2 s3  d 4  r31 r32 r33 pz 
  
 0 0 0 1  0 0 0 1
 c6  s6 0 0
 0 0 1 0

 s6  c6 0 0
 
 0 0 0 1

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

r11(c1s23c5  s1s23s5 )  r21(c23 )  r31(c1s23s5  s1s23c5 )  s6

r11(c1c23c4c5  s1s4c5  s1c23c4 s5  c1s4 s5 )  r21(s23c4 )  r31(c1c23c4 s5  s1s4 s5  s1c23c4c5  c1s4c5 )  c6

• We can solve for 6
6  A tan 2( s6 , c6 )

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 21/

• Summary - Number of Solutions

• Four solution
d d 2 
1  A tan 2( p y , px )  A tan 2 3 , 1  32 
   


 3  A tan 2(a3 , d 4 )  A tan 2 K , a32  d 42  K 2 
• For each of the four solutions the wrist can be
flipped

 4'   4  180o
 5'   5
 6'   6  180o

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Inverse Kinematics - PUMA 560 -
Algebraic Solution - 22/

• 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.

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• High Level Introduction to the Solution

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• High Level Introduction to the Solution

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• 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

0
P4org 01T 12T 23T 3P4org

• From the general forward kinematics method for determining homogeneous

transforms using DH parameters, we know:

i 1 i 1
i R Piorg
 c i  s i 0 ai 1 
 s c c i ci 1  si 1  si 1d i 
i 1
iT 
 i i 1 
 s i si 1 c i si 1 ci 1 ci 1d i 
 
 0 0 0 1 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• For i=4 3 3
4 R P4org
 c 4  s 4 0 a3 
 s c c c  s3  s3d 4 
4T 
3  4 3 4 3 
 s 4 s3 c 4 s 3 c 3 c3d 4 
 
 0 0 0 1 

• Using the fourth column and substituting for 3 P4,org we find

 a3   f1 ( 3 ) 
   
0 1 2   s 3 d 4  0 1  f 2 ( 3 )
0
P4org  1T 2T 3T P4org  1T 2T 3T
0 1 2 3 0
P4org  1T 2T 3T P4org  1T 2T
0 1 2 3

 c 3d 4   f 3 ( 3 ) 
   
 1   1 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• where  f1 ( 3 )   a3 
 f ( )  s d 
 2 3   3T 
2 3 4

 f 3 ( 3 )   c 3d 4 
   
 1   1 
 f1 ( 3 )   c 3  s 3 0 a2   a3 
 f ( )  s c c 3 c 2  s 2  s 2d 3   s 3d 4 
 2 3  3 2  
 f 3 ( 3 )   s 3 s 2 c 3 s 2 c 2 c 2d 3   c 3d 4 
    
 1   0 0 0 1  1 

f1  3   a3c3  d 4 s3s3  a2
f 2  3   a3c 2 s3  d 4 s3c 2c3  d 4 s 2c3  d 3s 2
f 3  3   a3s 2 s3  d 4 s3s 2c3  d 4c 2c3  d 3c 2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• Repeating the same process again

 f1 ( 3 ) 
 f ( )
0
P4org  01T 12T 23T 3P4org  01T 12T  2 3 
 f 3 ( 3 ) 
 
 1 
 g1 ( 2 )   f1 ( 3 ) 
 g ( )  f ( )
 2 2   2T  2 3 
1

 g3 ( 2 )   f 3 ( 3 ) 
   
 1   1 
 g1 ( 2 )   c 2  s 2 0 a1   f1 ( 3 ) 
 g ( )  s c c c  s1  s1d 2   f 2 ( 3 )
 2 2  2 1 2 2  
 g3 ( 2 )   s 2 s1 c 2 s1 c1 c1d 2   f 3 ( 3 ) 
    
 1   0 0 0 1  1 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

g1  2   c2 f1  s2 f 2  a1
g 2  2   s2 s1 f1  c2 c1 f 2  s1 f 3  d 2 s1
g 3  2   s2 s1 f1  c2 s1 f 2  c1 f 3  d 2 c1
• Repeating the same process for the last time

 g1 ( 2 ) 
 
0  g 2 ( 2 )
0
P4org  1T 2T 3T P4org  1T
0 1 2 3

 g 2 ( 2 )
 
 1 
 c1  s1 0 a0   g1 ( 2 ) 
 s c c1 c 0  s 0  s 0d1   g 2 ( 2 )
0
P4org   1 0  
 s1 s0 c1 s0 c 0 c 0d1   g3 ( 2 ) 
  
 0 0 0 1  1 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• 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  0 0  d1  a0  0
 c1  s1 0 a0   g1 ( 2 ) 
 s c c c  s 0  s 0d1   g 2 ( 2 )
0
P4org   1 0 1 0  
 s1 s0 c1 s0 c 0 c 0d1   g3 ( 2 ) 
  
 0 0 0 1  1 
c1 g1  s1 g 2 
s g  c g 
0
P4org   1 1 1 2 
 g3 
 
 1 
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• Through algebraic manipulation of these equations, we can solve for the desired
joint angles ( 1 ,  2 ,  3 ).

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

  
2
 
2
2
r 2  0 P4orgx  0 P4orgy  0 P4orgz  g12  g 22  g32

• Using the previously define function for gi we have

r 2  f12  f 22  f 32  a12  d 22  2d 2 f 3  a1 c2 f1  s2 f 2 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

r 2  f12  f 22  f 32  a12  d 22  2d 2 f 3  a1 c2 f1  s2 f 2 

Z 0P4orgz  g3  s2 s1 f1  c2 s1 f 2  c1 f 3  d 2c1

• 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.
k1  f1
r 2  k1c2  k2 s2 2a1  k3 k2   f 2
Z  k1s2  k2c2 s1  k4 k3  f12  f 22  f 32  a12  d 22  2d 2 f 3
k4  f 3c1  d 2c1
• These equations are useful because dependence on 1 has been eliminated,
and dependence on  2 takes a simple form

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• Consider 3 cases while solving for 3 :

• Case 1 - a1  0

r 2  k1c2  k2 s2 2a1  k3 r 2  k3
Z  k1s2  k2c2 s1  k4
k3  f12  f 22  f 32  a12  d 22  2d 2 f 3

f1  3   a3c3  d 4 s3s3  a2
f 2  3   a3c 2 s3  d 4 s3c 2c3  d 4 s 2c3  d 3s 2
f 3  3   a3s 2 s3  d 4 s3s 2c3  d 4c 2c3  d 3c 2

• Solution Methodology - Reduction to Ploynomial => Quadratic Equation

 1 u2 2u
u  tan cos   sin  
2 1 u2 1 u2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• Case 2 - s1  0
r 2  k1c2  k2 s2 2a1  k3 Z  k4
Z  k1s2  k2c2 s1  k4
k4  f 3c1  d 2c1

f 3  3   a3 s 2 s3  d 4 s3 s 2c3  d 4c 2c3  d3c 2

• Solution Methodology - Reduction to Ploynomial => Quadratic Equation

 1 u2 2u
u  tan cos   sin  
2 1 u2 1 u2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

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

manipulation:

r 2  k3
r 2  k1c2  k2 s2 2a1  k3  k1c2  k2 s2 
2a1
Z  k1s2  k2c2 s1  k4 Z  k4
 k1s2  k2c2 
s1

• squaring both sides, we find

2
 r  k3 
2
   k1c2  k2 s2 2  k12c22  k22 s22  2k1k2c2 s2
 2a1 
2
 Z  k4 
   k1s2  k2c2 2  k12 s22  k22c22  2k1k2c2 s2
 s1 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• Adding these two equations together and simplifying using the trigonometry
identity (Reduction to Ploynomial), we find a fourth order equation for  3

2 2
 r  k3   Z  k 4 
2
      k12  k22
 2a1   s1 

k1  f1
k2   f 2
k3  f12  f 22  f 32  a12  d 22  2d 2 f 3
k4  f 3c1  d 2c1
f1  3   a3c3  d 4 s3s3  a2
f 2  3   a3c 2 s3  d 4 s3c 2c3  d 4 s 2c3  d 3s 2
f 3  3   a3s 2 s3  d 4 s3s 2c3  d 4c 2c3  d 3c 2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• With 3 solved, substitute into r 2 , Z to find  2

r 2  k1c2  k2 s2 2a1  k3
Z  k1s2  k2c2 s1  k4

k1  f1
k2   f 2
k3  f12  f 22  f 32  a12  d 22  2d 2 f 3
k4  f 3c1  d 2c1

f1  3   a3c3  d 4 s3s3  a2
f 2  3   a3c 2 s3  d 4 s3c 2c3  d 4 s 2c3  d 3s 2
f 3  3   a3s 2 s3  d 4 s3s 2c3  d 4c 2c3  d 3c 2

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• With  2 , 3 solved, substitute into 0 P4org to find

c1 g1  s1 g 2 
s g  c g 
0
P4org   1 1 1 2
 g3 
 
 1 
0
P4orgx  c1 g1  s1 g 2
0
P4orgy  s1 g1  c1 g 2
g1  2   c2 f1  s2 f 2  a1 f1  3   a3c3  d 4 s3s3  a2
g 2  2   s2 s1 f1  c2 c1 f 2  s1 f 3  d 2 s1 f 2  3   a3c 2 s3  d 4 s3c 2c3  d 4 s 2c3  d 3s 2
g 3  2   s2 s1 f1  c2 s1 f 2  c1 f 3  d 2 c1 f 3  3   a3s 2 s3  d 4 s3s 2c3  d 4c 2c3  d 3c 2
• Solve for 1 using the reduction to polynomial method

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Pieper’s Solution - Three consecutive Axes Intersect

• To complete our solution we need to solve for  4 ,  5 , 6

• 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 0 R
6
0
6 R 40R  4 0 4
6 R

4
6 R  4 0  40R 1  4 0 0
6R

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

•  4 ,  5 , 6 are the Euler angles applied to 4

6 R  4 0

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Central Topic - Inverse Manipulator Kinematics -
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.

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Mapping - Rotated Frames - Z-Y-Z Euler Angles

Start with frame {4}.

• Rotate frame {4} about Ẑ 4 by an angle 
• Rotate frame {4} about YˆB by an angle 
• Rotate frame {4} about Ẑ 4 by an angle 

Euler Angles

Note - Each rotation is preformed about an axis of the moving reference frame
{4}, rather then a fixed reference.

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Mapping - Rotated Frames - Z-Y-Z Euler Angles

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Mapping - Rotated Frames - Z-Y-Z Euler Angles

c  s 0  c 0 s  c  s 0
RZ 'Y 'Z ' ( ,  ,  )  RZ ( ) RY (  ) RZ ( )   s c 0  0 1 0   s c 0
 0 0 1  s 0 c   0 0 1

ccc  ss  ccs  sc cs 

RZ 'Y 'Z ' ( ,  ,  )   scc  cs  scs  cc ss 
  sc ss c 

A
B RX 'Y 'Z ' ( ,  ,  )64R4 0

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

 r11 r12 r13  ccc  ss  ccs  sc cs 

r r23    scc  cs  scs  cc ss 
 21 r22
r31 r32 r33    sc ss c 
 
Goal Direct Kinematics

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

• Solve for  using element r31, r32 , r33

r31   sc
r32  ss
r33  c


r312  r322  s 2 c 2  s 2 
r33  c

s   r312  r322

• Using the Atan2 function, we find


  Atan2  r312  r322 , r33 
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

• Solve for  using elements r23 , r13

r13  cs
r23  ss

  Atan2r23 / s , r13 / s 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

• Solve for  using elements r32 , r31

r32  ss
r31  sc

  Atan2r32 / s ,r31 / s 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

• Note: Two answers exist for angle  which will result in two answers each for
angles  and  .


  Atan2  r312  r322 , r33 
  Atan2r23 / s , r13 / s 

  Atan2r32 / s ,r31 / s 

• If   0o ,   180o  s  0 the solution degenerates

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

 r11 r12 r13  ccc  ss  ccs  sc cs 

r r23    scc  cs  scs  cc ss 
 21 r22
r31 r32 r33    sc ss c 

  0o

 r11 r12 r13  cc  ss  cs  sc 0 c(   )  s(   ) 0

r r23    sc  cs  ss  cc 0   s(   ) c(   ) 0
 21 r22
r31 r32 r33   0 0 1  0 0 1

• We are left with     for every case. This means we can’t solve for either,
just their sum.

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

• One possible convention is to choose   0o

• The solution can be calculated to be

 0   180
 0  0
  Atan2 r12 , r11   Atan2s , c    Atan2r12 ,r11   Atan2s , c 

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 Three consecutive Axes Intersect - wrist

• 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.

Instructor: Jacob Rosen

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

http://youtu.be/zc8b2Jo7mno

Normal situation Gimbal lock:

The three gimbals are independent Two out of the three gimbals are in the
same plane, one degree of freedom is lost

Instructor: Jacob Rosen

Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
 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".

Instructor: Jacob Rosen

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