EME406-Lecture Note - 06

EME 406
Tharindu Weerakoon, PhD

SLT Campus
29/05/2019
Week #08- Lecture #06
1. An Overview of Robotics
2. Robotic Mechanics
3. Robot Electronics Design
4. Robotic Sensors and Vision System
5. Robot Path and Trajectory Planning
6. Robot Control and Programming
7. Robot Intelligence
8. Robotics Standards, Application Engineering and
Future Trend
Dr. Tharindu Weerakoon SLT Campus 2
֍ Robot-arm kinematics
 Forward and Inverse
֍ Jacobian
 Velocity Propagation, Force Propagation
֍ Dynamics
 Inertia matrix, Euler-Newton, Lagrange Method
֍ Obstacle avoidance

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 effector
𝐿3 𝐿4
Geometric Solution - Concept:
֍ Decompose spatial geometry
𝐿2
into several plane geometry
𝐿1
Examples - RRR (3R) manipulators -

Geometric Solution
Algebraic Solution - Concept

֍ Existence of Solutions
֍ Multiple Solutions
֍ Method of solutions
⁂ Close form solution
 Algebraic solution
 Geometric solution
⁂ Numerical solutions

0
֍ 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

DW  RW

֍ Example 1 - 𝐿1 = 𝐿2
Reachable
Workspace
Dexterous
Workspace

֍ Example 2 - 𝐿1 ≠ 𝐿2
Reachable
Workspace
NO Dexterous
Workspace

End Effector Rotation
֍ Example 3 - 𝐿1 = 𝐿2
Reachable
Workspace
&
Dexterous
Workspace

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
֍ 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

֍ 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 end
effector from the goal
֍ 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.
Inverse Kinematics - Generalized Algebraic
(Analytical) Solutions – Case 1-7

֍ Equation
֍ Solution (Unique)

֍ Equation
֍ Solution
⁂ Two solutions
⁂ Singularity at the Boundary

֍ Equation
֍ Solution
⁂ Two solutions 180° apart
⁂ Singularity

֍ Equation

֍ Solution
⁂ Two solutions
⁂ For a solution to exist
⁂ No solution (outside of the workspace)
⁂ One solution (singularity)

֍ Equation
֍ Solution

֍ Equation
֍ Solution
⁂ For an exiting solution (the determinant must be positive)

֍ 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

֍ Transcendental equations
֍ Substitute 𝑐𝜃, 𝑠𝜃 with the following equations
֍ Yields

֍ 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 𝑎 + 𝑐 = 0 then 𝜃 = 180°

1. Geometric Approach

𝜃1
Given:
𝜃2
֍ Manipulator Geometry
𝑌෠𝑇
𝑍መ𝑆
֍ Goal Point Definition: The position 𝜃3
𝑌෠𝑆 {𝑇}
𝑥𝑑 , 𝑦𝑑 , 𝑧𝑑 of the wrist in space {𝑆}
𝑍መ 𝑇 𝑋෠𝑇
𝑋෠𝑆
Problem: 𝐿3 𝐿4
֍ What are the joint angles ( 𝜃1 , 𝜃2 , 𝜃3 )
as a function of the goal (wrist 𝐿2
position and orientation)
𝐿1

𝜃1
 Mark the coordinate systems at

𝜃2
each joint.
𝑌෠𝑇
𝑍መ𝑆
𝜃3
𝑌෠𝑆 {𝑇}
{𝑆}
𝐿3 𝐿4 𝑍መ 𝑇 𝑋෠𝑇
𝑋෠𝑆
𝐿2
𝐿1

 The planar geometry - top view of 𝜃1
the robot
𝜃2
𝑌෠𝑇
𝑍መ𝑆
𝜃3
𝑌෠𝑆 {𝑇}
{𝑆}
𝑋෠𝑆

𝜃1
 The planar geometry - side view

𝜃2
of the robot
𝑌෠𝑇
𝑍መ𝑆
𝜃3
𝑌෠𝑆 {𝑇}
{𝑆}
𝑋෠𝑆

֍ By Apply the law of cosines we get
r22  L23  L24  2 L3 L4 cos(180   3 )  L23  L24  2 L3 L4 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 𝑐3 is defined above in terms of known parameters
𝐿3 , 𝐿4 , 𝑥𝑑 , 𝑦𝑑 and 𝑧𝑑
2. Analytical Approach

Algebraic Solution - Concept
Direct Kinematics Goal (Numerical

Values)
Example – PUMA 560 - Geometric Solution

0
Given : PUMA 560 - 6 DOF, 6T
Solve: 1  6
 r11 r12 r13 px 
r r r23 py 
0
T  0 1 2 3 4 5
T T T T T T   21 22 
 r31 r32 pz 
6 1 2 3 4 5 6
r33
 
0 0 0 1
Total Number of Equations: 12
Independent Equations: 3 - Rotation Matrix

3 - Position Vector
Type of Equations: Non-linear

• 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 
N 1
NT  1T  N T 
0 0  21 22 
 r31 r32 r33 pz 
 
0 0 0 1
Examples - PUMA 560 - Algebraic Solution

֍ Given:
⁂ Direct Kinematics: The homogenous transformation from the base to the
𝐵
wrist 𝑊 𝑇
⁂ Goal Point Definition: The position and orientation of the wrist in space

֍ Problem:
What are the joint angles ( 𝜃1 … 𝜃6 ) as a function of the wrist
𝐵
position and orientation ( or when 𝑊 𝑇 is given as numeric values)
 r11 r12 r13 px 

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

֍ 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 01𝑇(𝜃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 )

֍ Put the dependence on 𝜃1 on the left hand side of the equation by
multiplying the direct kinematics eq. with 01𝑇(𝜃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
T [ 0
T ] 1
  1 1 
 0 0
0 1
0 1
 
 0 0 0 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

 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
3T 
 3 
 0 0 1 d3   ' r11 '
r12 '
r13 '
px 
 
 0 0 0 1 ' ' ' ' 
 c 4  s 4  r21 r22 r23 py 
 0
0 a3  1
T  '
d4  6  r31
4T 
3 
0 1

'
r32 '
r33 '
pz 
 s 4  c 4 0 0  

 0 0 0 1

 0 0 0 1 
 c 5  s 5 0 0
 0 0 1 0
5T 
4  
 s 5 c 5 0 0
 
 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
֍ 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 p x  c1 p y  d 3
֍ To solve the equation of this form we make the trigonometric

substitution
px   cos
p y   sin 

֍ 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 p x  c1 p y  d 3
֍ To solve the equation of this form we make the trigonometric

substitution
px   cos
p y   sin 

  p x2  p y2
  A tan 2( px , p y )
֍ Substituting 𝑝𝑥 , 𝑝𝑦 with 𝜌, ∅ we obtain

d3
c1s  s1c 

֍ Using the difference of angles formula

d3
sin(  1 ) 


֍ Based on sin 2 (  1 )  cos2 (  1 )  1
d 32
cos(  1 )   1 
2
֍ and so d d 2 
  1  A tan 2 3 , 1  32 
   

֍ The solution for 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 corresponding to
the +/- sign
֍ 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 p x  s1 p y  a3c23  d 4 s23  a2 c2
 pz  a3c23  d 4 s23  a2c2

֍ If we square the following equations and add the resulting
equations
 s1 p x  c1 p y  d 3
c1 p x  s1 p y  a3c23  d 4 s23  a2 c2
 pz  a3s23  d 4c23  a2 s2
֍ Do the calculation as a homework

֍ We obtain
a3c3  d 4 s3  K
p x2  p y2  p z2  a22  a32  d 32  a42

where K
2a2
֍ Note that the dependence on 𝜃1 has be removed. Moreover the eq. for
𝜃3 is of 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


 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

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 py   s5c6  s5s6  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 p x  s1c23 p y  s23 pz  a2 c3  a3

 c1s23 p x  s1s23 p y  c23 pz  a2 s3  d 4
֍ These equations may be solved simultaneously for 𝑠23 and 𝑐23

( 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 𝜃1 and 𝜃3

֍ Then, four possible solutions for 𝜃2 are computed as
 2   23   3
֍ 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  s5s6  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
 r13c1c23  r23 s1c23  s23r33

r13c1c23  r23s1c23  s23r33  c4 s5  c4 
֍ we get s5
 r13s1  r23c1  s4 s5 r s r c
 s4  13 1 23 1
s5
֍ As long as 𝑠5 ≠ 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 6
up and cause the same motion of the last link of
5
the robot. In this case all that can be solved for
is the sum or difference of 𝜃4 and 𝜃6 . This
situation is detected by checking whether both
arguments of Atan2 defining 𝜃4 are near zero. 4
If so 𝜃4 is chosen arbitrary (usually chosen
to be equal to the present value of joint 4), and
𝜃6 is computed later.
[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  c5 s6  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  s5 s6 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 )

[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  s23 s4  c1c23 s4 s5  s1c4 s5  s1c23 s4 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  s1s23 s5  c23  c1s23 s5  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 )

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

EME406-Lecture Note - 06

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

EME406-Lecture Note - 06

Uploaded by

Copyright:

Available Formats

EME 406

Tharindu Weerakoon, PhD

Dr. Tharindu Weerakoon SLT Campus 3

Examples - RRR (3R) manipulators -

Algebraic Solution - Concept

Dr. Tharindu Weerakoon SLT Campus 5

Dr. Tharindu Weerakoon SLT Campus 6

֍ The Dexterous Workspace is a subset of the Reachable Workspace

Dr. Tharindu Weerakoon SLT Campus 7

Dr. Tharindu Weerakoon SLT Campus 8

Dr. Tharindu Weerakoon SLT Campus 9

Dr. Tharindu Weerakoon SLT Campus 10

Dr. Tharindu Weerakoon SLT Campus 12

֍ No. of DOF > No. of DOF of the

Dr. Tharindu Weerakoon SLT Campus 16

Dr. Tharindu Weerakoon SLT Campus 17

⁂ Singularity at the Boundary

Dr. Tharindu Weerakoon SLT Campus 18

⁂ Two solutions 180° apart

Dr. Tharindu Weerakoon SLT Campus 19

Dr. Tharindu Weerakoon SLT Campus 20

⁂ For a solution to exist

⁂ No solution (outside of the workspace)

⁂ One solution (singularity)

Dr. Tharindu Weerakoon SLT Campus 21

Dr. Tharindu Weerakoon SLT Campus 22

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

Dr. Tharindu Weerakoon SLT Campus 23

֍ Making the following substitutions

Dr. Tharindu Weerakoon SLT Campus 24

֍ Substitute 𝑐𝜃, 𝑠𝜃 with the following equations

Dr. Tharindu Weerakoon SLT Campus 25

Dr. Tharindu Weerakoon SLT Campus 26

Dr. Tharindu Weerakoon SLT Campus 27

Dr. Tharindu Weerakoon SLT Campus 28

 Mark the coordinate systems at

Dr. Tharindu Weerakoon SLT Campus 29

Dr. Tharindu Weerakoon SLT Campus 30

 The planar geometry - side view

Dr. Tharindu Weerakoon SLT Campus 31

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

֍ Solving for 𝜃3 we get

Dr. Tharindu Weerakoon SLT Campus 36

Direct Kinematics Goal (Numerical

Example – PUMA 560 - Geometric Solution

Total Number of Equations: 12

Independent Equations: 3 - Rotation Matrix

Dr. Tharindu Weerakoon SLT Campus 38

Examples - Planar RRR (3R) manipulators -

• Algebraic Solution - Concept

 r11 r12 r13 px 

Examples - PUMA 560 - Algebraic Solution

Dr. Tharindu Weerakoon SLT Campus 39

Dr. Tharindu Weerakoon SLT Campus 40

 r11 r12 r13 px 

Direct Kinematics Goal

Dr. Tharindu Weerakoon SLT Campus 41

Dr. Tharindu Weerakoon SLT Campus 42

Dr. Tharindu Weerakoon SLT Campus 43

Dr. Tharindu Weerakoon SLT Campus 44

֍ To solve the equation of this form we make the trigonometric

Dr. Tharindu Weerakoon SLT Campus 46

֍ To solve the equation of this form we make the trigonometric

Dr. Tharindu Weerakoon SLT Campus 47