Are you sure?
This action might not be possible to undo. Are you sure you want to continue?
Robot Mechanics
Dr. Robert L. Williams II
Mechanical Engineering
Ohio University
NotesBook™ Supplement for:
EE/ME 429/529
Mechanics and Control of Robotic Manipulators
© 2011 Dr. Bob Productions
williar4@ohio.edu
oak.cats.ohiou.edu/~williar4
These notes supplement the
EE/ME 429/529 NotesBook
TM
by Dr. Bob
This document presents supplemental notes to accompany the EE/ME 429/529 NotesBook™.
The outline given in the Table of Contents on the next page dovetails with and augments the EE/ME
429/529 NotesBook™ outline and hence may appear to be incomplete here.
2
EE/ME 429/529 Supplement Table of Contents
CHAPTER 1. INTRODUCTION ............................................................................................................................................ 3
1.3 MOBILITY .......................................................................................................................................................................... 3
1.5 MATRICES ......................................................................................................................................................................... 8
CHAPTER 2. SERIAL MANIPULATOR KINEMATICS ........................................................................................................... 16
2.4 DENAVITHARTENBERG (DH) PARAMETERS – ADDITIONAL EXAMPLES ...................................................................... 16
2.5 FORWARD POSE KINEMATICS ....................................................................................................................................... 25
2.5.1 3D MATLAB Graphics for Animation ................................................................................................................. 25
2.5.2 Spatial 6dof 6R PUMA Robot FPK Example .................................................................................................. 27
2.6 INVERSE POSE KINEMATICS .......................................................................................................................................... 31
2.8 VELOCITY KINEMATICS .................................................................................................................................................. 38
2.8.1 Velocity Introduction ............................................................................................................................................ 38
2.8.2 Velocity Equations Derivations........................................................................................................................... 40
2.8.3 Jacobian Matrices ............................................................................................................................................... 42
2.8.8 Velocity Kinematics Examples ........................................................................................................................... 46
2.8.9 Jacobian Matrix Expressed in Another Frame ................................................................................................. 47
2.8.10 Cartesian Transformation of Velocities and Wrenches ................................................................................ 48
2.9 ACCELERATION KINEMATICS ........................................................................................................................................ 52
2.10 ROBOT WORKSPACE ................................................................................................................................................... 57
CHAPTER 3. KINEMATICALLY‐REDUNDANT ROBOTS ...................................................................................................... 64
3.2 INVERSE VELOCITY (RESOLVEDRATE) SOLUTION ...................................................................................................... 64
3.2.1 Pseudoinversebased .......................................................................................................................................... 64
3.2.3 Klein and Huang’s Algorithm .............................................................................................................................. 64
3.2.4 Singular Value Decomposition ........................................................................................................................... 65
3.2.5 Generalized Inverses ........................................................................................................................................... 67
CHAPTER 4. MANIPULATOR DYNAMICS ........................................................................................................................ 68
4.1 MASS DISTRIBUTION: INERTIA TENSOR ....................................................................................................................... 69
4.2 NEWTONEULER RECURSIVE ALGORITHM .................................................................................................................... 70
4.3 LAGRANGEEULER ENERGY METHOD .......................................................................................................................... 72
4.4 SIMPLE DYNAMICS EXAMPLE ........................................................................................................................................ 73
4.5 STRUCTURE OF MANIPULATOR DYNAMICS EQUATIONS ............................................................................................... 78
4.6 ROBOT DYNAMICS EXAMPLE ......................................................................................................................................... 79
CHAPTER 5. ROBOT CONTROL ARCHITECTURES ............................................................................................................. 83
5.1 INVERSE POSE CONTROL ARCHITECTURE .................................................................................................................... 83
5.2 INVERSE VELOCITY (RESOLVEDRATE) CONTROL ARCHITECTURE ............................................................................. 83
5.3 INVERSE DYNAMICS CONTROL ARCHITECTURE ............................................................................................................ 84
5.4 NASA LANGLEY TELEROBOTICS RESOLVEDRATE CONTROL ARCHITECTURE ......................................................... 85
5.5 NATURALLYTRANSITIONING RATETOFORCE CONTROLLER ARCHITECTURE .......................................................... 86
5.6 SINGLE JOINT CONTROL ................................................................................................................................................ 87
CHAPTER 6. PARALLEL ROBOTS .................................................................................................................................... 97
6.1 INTRODUCTION ............................................................................................................................................................... 97
6.2 PLANAR 3RPR MANIPULATOR POSE KINEMATICS ..................................................................................................... 99
6.3 NEWTONRAPHSON METHOD ...................................................................................................................................... 101
6.4 PARALLEL MANIPULATOR WORKSPACE ..................................................................................................................... 105
6.5 PLANAR 3RPR MANIPULATOR VELOCITY KINEMATICS ............................................................................................ 106
3
Chapter 1. Introduction
1.3 Mobility
Mobility is the number of degreesoffreedom (dof) which a robot or mechanism possesses. Structures
have zero (or negative) dof, mechanisms have one dof, and robots have greater than one dof.
Degreesoffreedom (dof) – the minimum number of independent parameters required to fully specify
the location of a device.
For planar or spatial serial robots, to find the dof, one may simply count the number of single degreeof
freedom joints. This is also equal to the number of independent motors to control the robot. More
formally, we can also use Kutzbach’s mobility equations for planar or spatial robots.
Kutzbach's Planar Mobility Equation
( )
1 2
3 1 2 1 M N J J = ÷ ÷ ÷
Where:
M is the mobility
N is the total number of links, including ground
J
1
is the number of onedof joints
J
2
is the number of twodof joints
J
1
– onedof joints: revolute, prismatic
J
2
– twodof joints: cam joint, gear joint, slottedpin joint
revolute joint R – pin joint, turning pair prismatic joint P – sliding pair
In general J
1
R and P joints are used much more extensively in practical robots than J
2
joints.
In Kutzbach's planar mobility equation, links give degrees of freedom and joints constrain or
remove degrees of freedom. Each moving link has 3dof in the plane – but the ground link is counted
and hence must be subtracted in the equation. Onedof joints thus remove 2dof in planar motion and
twodof joints thus remove 1dof in planar motion.
4
Planar robot mobility examples:
2R serial robot 3R serial robot 4R serial robot
RRPR serial robot fivebar parallel robot 3RRR parallel robot
( ) 3 5 1 2(5) 1(0) 2dof M = ÷ ÷ ÷ =
( ) 3 8 1 2(9) 1(0) 3dof M = ÷ ÷ ÷ =
A general planar nlink serial robot has n+1 links (including the fixed ground link), connected by
n active 1dof joints:
( ) ( ) 3 1 1 2 1 0 3 2 M n n n n n = + ÷ ÷ ÷ = ÷ = dof
This agrees with the previous statement, that we can just count the number of active joints to find the
mobility. For parallel robots this does not apply, but we can still count the required motors.
5
Kutzbach's Spatial Mobility Equation
( )
1 2 3 4 5
6 1 5 4 3 2 1 M N J J J J J = ÷ ÷ ÷ ÷ ÷ ÷
Where:
M is the mobility
N is the total number of links, including ground
J
1
is the number of onedof joints
J
2
is the number of twodof joints
J
3
is the number of threedof joints
J
4
is the number of fourdof joints
J
5
is the number of fivedof joints
J
1
– onedof joints: revolute, prismatic, screw
J
2
– twodof joints: universal, cylindrical, gear joint
J
3
– threedof joints: spherical
J
4
– fourdof joints: spherical in a slot
J
5
– fivedof joints: spatial cam
universal joint U spherical joint S
In general J
1
R and P, J
2
U, and J
3
S joints are used much more extensively in practical robots
than the other named joints.
In Kutzbach's spatial mobility equation, links give degrees of freedom and joints constrain or
remove degrees of freedom. Each moving link has 6dof in 3D – but the ground link is counted and
hence must be subtracted in the equation. Onedof joints remove 5dof, twodof joints remove 4dof,
threedof joints remove 3dof, and so on, in spatial motion.
6
Spatial robot mobility examples:
5R serial robot 6R PUMA serial robot
7R serial FTS robot RRPR Adept SCARA robot
8R serial ARMII robot Stewart platform
7
Human arm
A general spatial nlink serial robot has n+1 links (including the fixed ground link), connected by
n active 1dof joints:
( ) ( ) ( ) ( ) ( ) 6 1 1 5 4 0 3 0 2 0 1 0 6 5 M n n n n n = + ÷ ÷ ÷ ÷ ÷ ÷ = ÷ = dof
This agrees with the previous statement, that we can just count the number of active joints to find the
mobility. For parallel robots this does not apply, but we can still count the required motors.
For some more unsolved mobility examples, please see Dr. Bob’s atlas of structures, mechanisms, and
robots:
oak.cats.ohiou.edu/~williar4/PDF/MechanismAtlas.pdf
MATLAB function to calculate mobility
% Function for calculating planar mobility
% Dr. Bob, EE/ME 429/529
function M = dof(N,J1,J2)
M = 3*(N1)  2*J1  1*J2;
Usage:
mob = dof(4,4,0); % for 4bar and slidercrank mechanisms
Result:
mob = 1
8
1.5 Matrices
Matrix: an m x n array of numbers, where m is the number of rows and n in the number of columns.
 
11 12 1
21 22 2
1 2
n
n
m m mn
a a a
a a a
A
a a a
=
Matrices can be used to simplify and standardize the solution of n linear equations in n unknowns
(where m = n). In robotics matrices are used a lot, in pose (position and orientation) description,
velocity, acceleration, and dynamics.
Special Matrices
square matrix (m = n = 3)  
11 12 13
21 22 23
31 32 33
a a a
A a a a
a a a
=
diagonal matrix  
11
22
33
0 0
0 0
0 0
a
A a
a
=
identity matrix  
3
1 0 0
0 1 0
0 0 1
I
=
transpose matrix  
11 21 31
12 22 32
13 23 33
T
a a a
A a a a
a a a
=
(switch rows and columns)
symmetric matrix    
11 12 13
12 22 23
13 23 33
T
a a a
A A a a a
a a a
= =
column vector (3x1 matrix) { }
1
2
3
x
X x
x
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
row vector (1x3 matrix) { } { }
1 2 3
T
X x x x =
9
Matrix Addition add like terms and keep the results in place
a b e f a e b f
c d g h c g d h
+ +
+ =
+ +
Matrix Multiplication with Scalar multiply each term and keep the results in place
a b ka kb
k
c d kc kd
=
Matrix Multiplication
    
C A B =
In general,
     
A B B A =
The row and column indices must line up as follows:
    
( x ) ( x )( x )
C A B
m n m p p n
=
÷
That is, in a matrix multiplication product, the number of columns p in the lefthand matrix must equal
the number of rows p in the righthand matrix. If this condition is not met, the matrix multiplication is
undefined and cannot be done.
The size of the resulting matrix [C] is from the number of rows m of the lefthand matrix and the
number of columns n of the righthand matrix: mxn.
Multiplication proceeds by multiplying like terms and adding them, along the rows of the left
hand matrix and down the columns of the righthand matrix (use your index fingers from the left and
right hands).
Example:
 
(2x1) (2x3)(3x1)
g
a b c ag bh ci
C h
d e f dg eh fi
i
+ +
= =
+ +
÷
Note the inner indices (p = 3) must match, as stated above, and the dimension of the result is dictated by
the outer indices, i.e. mxn = 2x1.
10
Matrix Multiplication Examples
 
1 2 3
4 5 6
A
=
 
7 8
9 8
7 6
B
=
    
7 8
1 2 3
9 8
4 5 6
7 6
7 18 21 8 16 18 46 42
28 45 42 32 40 36 115 108
C A B =
=
+ + + +
= =
+ + + +
(2x2) (2x3)(3x2) ÷
    
7 8
1 2 3
9 8
4 5 6
7 6
7 32 14 40 21 48 39 54 69
9 32 18 40 27 48 41 58 75
7 24 14 30 21 36 31 44 57
D B A =
=
+ + +
= + + + =
+ + +
(3x3) (3x2)(2x3) ÷
11
Matrix Inversion
Since we cannot divide by a matrix, we multiply by the matrix inverse instead. Given
    
C A B = , solve for [B]:
    
C A B = ¬
        
  
 
1 1
A C A A B
I B
B
÷ ÷
=
=
=
     
1
B A C
÷
¬ =
Matrix [A] must be square (m = n) to invert.
        
1 1
A A A A I
÷ ÷
= =
where [I] is the identity matrix, the matrix 1 (ones on the diagonal and zeros everywhere else). To
calculate the matrix inverse:
 
  1
adjoint( ) A
A
A
÷
=
where: A determinant of [A]
   
adjoint( ) cofactor( )
T
A A =
cofactor(A) ( 1)
i j
ij ij
a M
+
= ÷
minor minor M
ij
is the determinant of the submatrix with row i and
column j removed.
For another example, given
    
C A B = , solve for [A]
    
C A B = ¬
      
  
 
1 1
C B A B B
A I
A
÷ ÷
=
=
=
    
1
A C B
÷
¬ =
In general the order of matrix multiplication and inversion is crucial and cannot be changed.
12
System of Linear Equations
We can solve n linear equations in n unknowns with the help of a matrix. For n = 3:
11 1 12 2 13 3 1
21 1 22 2 23 3 2
31 1 32 2 33 3 3
a x a x a x b
a x a x a x b
a x a x a x b
+ + =
+ + =
+ + =
Where a
ij
are the nine known numerical equation coefficients, x
i
are the three unknowns, and b
i
are the
three known righthandside terms. Using matrix multiplication backwards, this is written as
 { } { } A x b = :
11 12 13 1 1
21 22 23 2 2
31 32 33 3 3
a a a x b
a a a x b
a a a x b
¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
=
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
where:
 
11 12 13
21 22 23
31 32 33
a a a
A a a a
a a a
=
is the matrix of known numerical coefficients;
{ }
1
2
3
x
x x
x
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
is the vector of unknowns to be solved; and
{ }
1
2
3
b
b b
b
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
is the vector of known numerical righthandside terms.
There is a unique solution { }   { }
1
x A b
÷
= only if [A] has full rank. If not, 0 A = (the determinant of
coefficient matrix [A] is zero) and the inverse of matrix [A] is undefined (since it would require dividing
by zero; in this case the rank is not full, it is less than 3, which means not all rows/columns of [A] are
linearly independent). Actually, Gaussian Elimination is more robust and more computationally
efficient than matrix inversion to solve the problem
 { } { } A x b = for {x}.
13
Matrix Example – solve linear equations
Solution of 2x2 coupled linear equations.
1 2
1 2
2 5
6 4 14
x x
x x
+ =
+ =
¬
1
2
1 2 5
6 4 14
x
x
¦ ¹ ¦ ¹
=
´ ` ´ `
¹ ) ¹ )
 
1 2
6 4
A
=
{ }
1
2
x
x
x
¦ ¹
=
´ `
¹ )
{ }
5
14
b
¦ ¹
=
´ `
¹ )
{ }   { }
1
x A b
÷
=
( ) ( ) 1 4 2 6 8 A = ÷ = ÷
The determinant of [A] is nonzero so there is a unique solution.
 
1
4 2 1/ 2 1/ 4
1
6 1 3/ 4 1/ 8
A
A
÷
÷ ÷
= =
÷ ÷
check:         
1 1
2
1 0
0 1
A A A A I
÷ ÷
= = =
1
2
1/ 2 1/ 4 5 1
3/ 4 1/ 8 14 2
x
x
÷ ¦ ¹ ¦ ¹ ¦ ¹
= =
´ ` ´ ` ´ `
÷
¹ ) ¹ ) ¹ )
answer.
Check this solution: substitute the answer {x} into the original equations
 { } { } A x b = and make sure
the required original {b} results:
1 2 1 1(1) 2(2) 5
6 4 2 6(1) 4(2) 14
+ ¦ ¹ ¦ ¹ ¦ ¹
= =
´ ` ´ ` ´ `
+
¹ ) ¹ ) ¹ )
checks out
14
Same Matrix Examples in MATLAB
%
% Matrices.m  matrix examples
% Dr. Bob, ME 301
%
clear; clc;
A1 = diag([1 2 3]) % 3x3 diagonal matrix
A2 = eye(3) % 3x3 identity matrix
A3 = [1 2;3 4]; % matrix addition
A4 = [5 6;7 8];
Add = A3 + A4
k = 10; % matrixscalar multiplication
MultSca = k*A3
Trans = A4' % matrix transpose (swap rows,cols)
A5 = [1 2 3;4 5 6]; % define two matrices
A6 = [7 8;9 8;7 6];
A7 = A5*A6 % matrixmatrix multiplication
A8 = A6*A5
A9 = [1 2;6 4]; % matrix for linear equations solution
b = [5;14]; % define RHS vector
dA9 = det(A9) % calculate determinant of A
invA9 = inv(A9) % calculate the inverse of A
x = invA9*b % solve linear equations
x1 = x(1); % extract answers
x2 = x(2);
Check = A9*x % check answer – should be b
xG = A9\b % Gaussian elimination is more efficient
who % display the usercreated variables
whos % usercreated variables with dimensions
The first solution of the linear equations above uses the matrix inverse – actually, to solve linear
equations, Gaussian Elimination is more efficient (more on this in the dynamics notes later) and more
robust numerically; Gaussian elimination implementation is given in the third to the last line of the
above mfile (with the backslash).
15
Output of Matrices.m
A1 =
1 0 0
0 2 0
0 0 3
A2 =
1 0 0
0 1 0
0 0 1
Add =
6 8
10 12
MultSca =
10 20
30 40
Trans =
5 7
6 8
A7 =
46 42
115 108
A8 =
39 54 69
41 58 75
31 44 57
dA9 = 8
invA9 =
0.5000 0.2500
0.7500 0.1250
x = 1
2
Check = 5
14
xG = 1
2
16
Chapter 2. Serial Manipulator Kinematics
2.4 DenavitHartenberg (DH) Parameters – additional examples
1. 3axis Spatial 3P Cartesian Robot
Workspace shown in hatched lines.
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1 0 0 d
1
0
2 90 ÷
0 d
2
90 ÷
3 90 ÷
0 d
3
0
17
2. 3axis Spatial PRP Cylindrical Robot (incomplete)
Workspace shown in hatched lines.
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1
2
3
18
3. 3axis Spatial RRP Spherical Robot (incomplete)
Workspace shown in hatched lines.
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1
2
3
19
4. 4axis Spatial 4R Articulated Robot (incomplete)
Workspace shown in hatched lines.
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1
2
3
4
20
5. 4axis Spatial RRPR SCARA Robot (incomplete)
Workspace shown in hatched lines.
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1
2
3
4
21
6. 6axis Spatial 6R Fanuc S10 Robot (incomplete)
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1
2
3
4
5
6
6. 6R Fanuc S10 Robot (continued) – coordinate frames
23
7. 7axis Spatial 7R NASA Flight Telerobotic Servicer (FTS) Robot (incomplete)
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1
2
3
4
5
6
7
24
8. 8axis Spatial 8R NASA AAI ARMII Robot (incomplete)
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1
2
3
4
5
6
7
8
25
2.5 Forward Pose Kinematics
2.5.1 3D MATLAB Graphics for Animation
This section presents some basic MATLAB code to draw a 3D robot to the screen, for either
snapshots or motion animation following Forward Pose Kinematics, or other robotics calculations to
follow later in the 429/529 NotesBook. Specifically we will show how to draw a 3D stickfigure
representation for the laboratory SCARA robot below.
Adept 550 SCARA 4dof 3D Robot
MATLAB Code:
xx1 = [0 0]; % link from {B} to {0}
yy1 = [0 0];
zz1 = [0 L0];
xx2 = [0 L1*c1]; % link from {0} to {2}
yy2 = [0 L1*s1];
zz2 = [L0 L0];
xx3 = [L1*c1 L1*c1+L2*c12]; % link from {2} to end of L2
yy3 = [L1*s1 L1*s1+L2*s12];
zz3 = [L0 L0];
xx4 = [L1*c1+L2*c12 L1*c1+L2*c12]; % link from end of L2 to {4}
yy4 = [L1*s1+L2*s12 L1*s1+L2*s12];
zz4 = [L0 L0d3];
figure;
plot3(xx1,yy1,zz1,'b',xx2,yy2,zz2,'b',xx3,yy3,zz3,'b',xx4,yy4,zz4,'b');
This will generate the basic stickfigure on the screen; here are some addons:
 Be sure to use axis(‘equal’) or axis(‘square’) with appropriate axis limits.
 Use grid to clarify the resulting image.
 Use 'linewidth',4 to make thicker lines.
 Draw thinner unit vectors to represent the origins and directions of {B}, {0}, and {4}.
 Use subplot for 4 views (3 planar projections and orthographic view) – see view(AZ,EL).
 Use X Y Z axis labels.
26
Here is what the simplified 3D 4dof Adept 550 SCARA robot could look like in MATLAB:
This was generated using snapshot FPK for the inputs
 
1 2 3 4
15 25 0.300 35 d u u u = ÷
.
The thicker robot lines are drawn in blue ('b' in the code) and the {B}, {0}, and {4} axes use the
convention r g b for the X Y Z unit vectors.
If you want to use this SCARA graphics in a motion simulation, you must use pause(dt)
within your motion loop. Otherwise the animation will flash by so fast you cannot see it. dt can be
designed to approximately obtain realtime animations, though normal MATLAB and certainly MS
Windows are not realtime.
Use of onscreen robot graphics is very helpful in validating that your simulated snapshots and
motion trajectories are working correctly.
27
2.5.2 Spatial 6dof 6R PUMA Robot FPK Example
Spatial 6R PUMA Robot Forward Pose Kinematics Symbolic Derivations
Given ( )
1 2 3 4 5 6
, , , , , u u u u u u , calculate
0
6
T
and
B
H
T
.
i
1 i
o
÷
1 i
a
÷
i
d
i
u
1 0 0 0 u
1
2 90 ÷
0 L
0
2
90 u ÷
3 0 L
1
0
3
90 u +
4 90
0 L
2
u
4
5 90 ÷
0 0 u
5
6 90
0 0
6
90 u +
28
Substitute each row of the DH parameters table into the equation for
1 i
i
T
÷
. Evaluate the angle offsets
also, i.e. use the trigonometric sumofangle formulas (see the 429/529 NotesBook) to simplify.
1
1 1 1 1 1
1 1 1 1
0
0 0 0 1
i i i
i i i i i i i i
i
i i i i i i i
c s a
s c c c s d s
T
s s c s c d c
u u
u o u o o o
u o u o o o
÷
÷ ÷ ÷ ÷ ÷
÷ ÷ ÷ ÷
÷
÷ ÷
=
1 1
1 1 0
1
0 0
0 0
0 0 1 0
0 0 0 1
c s
s c
T
÷
=
2 2
0 1
2
2 2
0 0
0 0 1
0 0
0 0 0 1
s c
L
T
c s
=
÷
3 3 1
3 3 2
3
0
0 0
0 0 1 0
0 0 0 1
s c L
c s
T
÷ ÷
÷
=
4 4
2 3
4
4 4
0 0
0 0 1
0 0
0 0 0 1
c s
L
T
s c
÷
÷ ÷
=
5 5
4
5
5 5
0 0
0 0 1 0
0 0
0 0 0 1
c s
T
s c
÷
=
÷ ÷
6 6
5
6
6 6
0 0
0 0 1 0
0 0
0 0 0 1
s c
T
c s
÷ ÷
÷
=
÷
( ) ( ) ( ) ( ) ( ) ( )
( ) ( )
0 0 1 2 3 4 5
6 1 1 2 2 3 3 4 4 5 5 6 6
0 0 3
6 3 1 2 3 6 4 5 6
, , , ,
T T T T T T T
T T T
u u u u u u
u u u u u u
=
=
( ) ( ) ( )
0 0 1
3 1 2 3 1 1 3 2 3
, , , T T T u u u u u u =
Take advantage of parallel Z axes in consecutive frames {2} and {3} – use trigonometric sumofangles
formulas.
( )
( )
1 1 23 23 1 2
1 1 0 0
3 1 2 3
23 23 1 2
1 23 1 23 1 0 1 1 1 2
1 23 1 23 1 0 1 1 1 2 0
3 1 2 3
23 23 1 2
0 0 0
0 0 0 0 1
, ,
0 0 1 0 0
0 0 0 1 0 0 0 1
, ,
0
0 0 0 1
c s c s L s
s c L
T
s c L c
c c c s s L s L c s
s c s s c L c L s s
T
s c L c
u u u
u u u
÷ ÷
=
÷ ÷
÷ ÷ ÷ +
÷ +
=
÷ ÷
where we used the abbreviations:
( )
( )
23 2 3
23 2 3
cos
sin
c
s
u u
u u
= +
= +
29
( ) ( ) ( ) ( )
3 3 4 5
6 4 5 6 4 4 5 5 6 6
, , T T T T u u u u u u =
There are no consecutive parallel Z axes in frames {4}, {5}, and {6}. Therefore, no sumofangles
formula simplification is possible.
( )
4 6 4 5 6 4 6 4 5 6 4 5
5 6 5 6 5 2 3
6 4 5 6
4 6 4 5 6 4 6 4 5 6 4 5
0
, ,
0
0 0 0 1
s c c c s s s c c c c s
s s s c c L
T
c c s c s c s s c c s s
u u u
÷ ÷ ÷
÷ ÷ ÷ ÷
=
÷ ÷ ÷
It is left to the reader to perform the symbolic multiplication
0 0 3
6 3 6
T T T =
; the result is complicated
and can easily be found by using symbolic math on the computer (e.g. the MATLAB Symbolic
Toolbox). Alternatively, these two matrices can be evaluated and multiplied numerically, again using
0 0 3
6 3 6
T T T =
.
Here we will present one aspect of this multiplication; due to the spherical wrist (wrist frames
{4}, {5}, and {6} share a common origin), the position vector
{ }
0
6
P is a function only of the first three
joint angles:
( ) { } { } { }
0 1 1 1 2 2 1 23
0 0 0 3
6 1 2 3 4 3 4 0 1 1 1 2 2 1 23
1 2 2 23
, ,
L s Lc s L c s
P P T P L c L s s L s s
Lc L c
u u u
÷ + + ¦ ¹
¦ ¦
= = = + +
´ `
¦ ¦
+
¹ )
Where constant vector
{ } { }
3
4 2
0 0
T
P L = ÷ . In this way the PUMA FPK symbolic expressions are
partially decoupled: the position vector
{ }
0
6
P is only a function of ( )
1 2 3
, , u u u , but the rotation matrix
0
6
R
is a complicated function of all six joint angles.
Two additional, fixed transforms
The basic Forward Pose Kinematics result is
0
6
T
. For this robot we need to control the {H}
frame. There is also a separate base frame {B}, different from {0}. The overall Forward Pose
Kinematics Homogeneous Transformation Matrix is given conceptually below; the symbolic terms
would be worse in complexity than
0
6
T
. The below overall transform can be evaluated numerically.
L
B
and L
H
are constants. The orientation of {B} is identical to {0} and the orientation of {H} is identical
to {6} for all motion.
( ) ( ) ( )
0 6
0 6 1 2 3 4 5 6
, , , , ,
B B
H B H H
T T L T T L u u u u u u =
( )
0
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
B
B
B
T L
L
=
( )
6
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
H H
H
T L
L
=
30
Note that ( )
0
B
B
T L
and ( )
6
H H
T L
are not evaluated by any row in the DH parameter table, since there
is no variable associated with these fixed homogeneous transformation matrices. Instead, they are
determined by inspection, using the rotation matrix and position vector components of the basic
homogeneous transformation matrix definition.
PUMA Robot FPK Examples
Given fixed robot lengths
0 1 2
1.0, 0.3, 1.5, 1.2, 0.5
B H
L L L L L = = = = = (m).
1) Given { } ( )
10 , 20 , 30 , 40 , 50 , 60 O =
, calculate
0
6
T
and
B
H
T
.
( ) ( )
( ) ( ) ( )
0 0 3
6 3 1 2 3 6 4 5 6
0 6
0 6 1 2 3 4 5 6
, , , ,
, , , , ,
B B
H B H H
T T T
T T L T T L
u u u u u u
u u u u u u
=
=
0
6
0.023 0.637 0.771 1.358
0.030 0.771 0.636 0.544
0.999 0.008 0.036 2.181
0 0 0 1
T
÷
=
÷
0.023 0.637 0.771 1.744
0.030 0.771 0.636 0.862
0.999 0.008 0.036 3.163
0 0 0 1
B
H
T
÷
=
÷
2) Given { } ( )
60 , 50 , 40 , 30 , 20 , 10 O = ÷ ÷ ÷ ÷ ÷ ÷
, calculate
0
6
T
and
B
H
T
.
0
6
0.638 0.699 0.322 0.915
0.437 0.015 0.899 2.184
0.634 0.715 0.296 0.964
0 0 0 1
T
÷ ÷
=
÷ ÷
0.638 0.699 0.322 1.076
0.437 0.015 0.899 2.634
0.634 0.715 0.296 1.816
0 0 0 1
B
H
T
÷ ÷
=
÷ ÷
31
2.6 Inverse Pose Kinematics
Decoupled Inverse Pose Kinematics Solution  Pieper's Solution
Pieper proved that if a 6dof robot has any three consecutive joint axes intersecting, there exists a
closedform (analytical) solution to the inverse position kinematics. The majority of industrial robots
are in this category.
In particular, many robots have a spherical wrist, i.e. three wrist actuators that rotate about a axes
intersecting in a common point. In this case, the position and orientation subproblems may be
decoupled. We solve for the first three joints first using the position vector input. Then we solve for the
second three joints next using the given orientation, based on the orientation caused by the first three
joints. The given position vector must point to the wrist point (the shared origin point of three
consecutive wrist frames). We can always transform any endeffector or other tool vector vector to this
origin point, since there are no more active joints beyond the last wrist joint.
PUMA Example (without details):
Given
0
6
T
, calculate
( )
1 2 3 4 5 6
, , , , , u u u u u u .
( )
0
6 1 2 3 4 5 6
, , , , , T f u u u u u u =
( ) ( ) { }
0 0
6 1 2 3 4 5 6 6 1 2 3 0
6
, , , , , , ,
0 0 0 1
R P
T
u u u u u u u u u
=
Joints 4, 5, and 6 cannot affect the translation of the wrist origin point.
1) Translational equations: Given
{ }
0
6
P , calculate
1 2 3
, , u u u values (4 sets).
3 independent equations, 3 unknowns.
2) Rotational equations: Given
0
6
R
, and now knowing
1 2 3
, , u u u , calculate
4 5 6
, , u u u values (2 sets).
3 independent equations, 6 dependent equations, 3 unknowns.
( ) ( ) ( )
3 0 0
6 4 5 6 3 1 2 3 6 1 2 3 4 5 6
, , , , , , , , ,
T
R R R u u u u u u u u u u u u =
4 sets of
1 2 3
, , u u u ; 2 sets of
4 5 6
, , u u u for each. Therefore, there are 8 overall solutions to the
inverse position problem for the PUMA. Some may lie outside joint ranges. Generally one would
choose the closest solution to the previous position.
32
Peeloff homogeneous transformations matrices with unknowns to separate variables.
( ) ( ) ( ) ( ) ( ) ( )
0 0 1 2 3 4 5
6 1 1 2 2 3 3 4 4 5 5 6 6
T T T T T T T u u u u u u =
LHS
0
6
T
is numbers
( ) ( ) ( ) ( ) ( ) ( )
1
0 0 1 2 3 4 5
1 1 6 2 2 3 3 4 4 5 5 6 6
T T T T T T T u u u u u u
÷
=
can solve for u
1
and u
3
( ) ( ) ( ) ( )
1
0 0 3 4 5
3 2 6 4 4 5 5 6 6
T T T T T u u u u
÷
=
can solve for u
2
with u
1
and u
3
known
( ) ( ) ( )
1
0 0 4 5
4 4 6 5 5 6 6
T T T T u u u
÷
=
isolate and solve u
4
, u
5
, and u
6
33
3. 8axis Spatial 8R NASA AAI ARMII Robot
Inverse Pose Kinematics General Statement
Given: Calculate:
Since m = 6 (Cartesian dof) and n = 8 (joint dof) we have the kinematicallyredundant underconstrained
case. There are infinite solutions (multiple as well). There are some great ways to make use of this
redundancy for performance optimization in addition to following commanded Cartesian translational
and rotational velocity trajectories. For inverse pose purposes we will here simplify instead and lock out
the redundancy so that m = n = 6; let us choose
3 5
0 u u = = for all motion to accomplish this. Then we
have a determined Inverse Pose Kinematics problem, still with multiple solutions (but finite).
34
The Forward Pose Kinematics relationship is:
So, the first step should be to simplify the equations as much as possible by calculating the required
0
8
T
to achieve the commanded
B
H
T
:
The problem can be decoupled between the arm joints 14 and the wrist joints 58 since the ARMII has a
spherical wrist (all 4 wrist joint Cartesian coordinate frames share the same origin). See the previous
section that explained the Pieper results for the 6axis PUMA robot.
Now, we will further simplify by ignoring the wrist joints 68 (5 is already locked to zero) and solve the
Inverse Pose Kinematics problem only for the arm joints 1,2, and 4. The full inverse solution is given in
Williams0F
1
.
Inverse Pose Kinematics Symbolic Solution for Arm Joints Only, with
3
0 u =
Reduced problem statement:
Given
{ } { }
5 5 5 5
T
B
P x y z = , calculate
( )
1 2 4
, ,
i
u u u , for all possible solution sets i.
That is, with only three active joints, we can only specify three Cartesian objectives, in this case the XYZ
location of the origin of {5} with respect to origin of {B} (and expressed in the basis of {B}). Note that
{ } { }
5 8
B B
P P = due to the spherical wrist.
The equations to solve for this problem come from the Forward Pose Kinematics relationships
for the ARMII robot, the translational portion only (further, with
3
0 u = ):
This equation yields (the derivation is left to student, use symbolic Forward Pose Kinematics):
{ }
( )
( )
5 1 3 2 5 24
5 5 1 3 2 5 24
5 3 2 5 24
B
B
x c d s d s
P y s d s d s
z d d c d c
÷ + ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= = ÷ +
´ ` ´ `
¦ ¦ ¦ ¦
+ +
¹ ) ¹ )
where:
( )
( )
24 2 4
24 2 4
cos
sin
c
s
u u
u u
= +
= +
(Since
3
0 u = always, the Z axes of 2 and 4 are always parallel and we used the sumofangles trig
formulas.)
1
R.L. Williams II, Kinematic Equations for Control of the Redundant EightDegreeofFreedom Advanced Research Manipulator II,
NASA Technical Memorandum 4377, NASA Langley Research Center, Hampton, VA, July 1992.
35
Solution Process:
1. A ratio of the Y to X equations yields:
1
180 u +
is also a valid solution
2. Since u
1
is now known (two values), we can modify the Y and Z equations:
where:
5
1
5 B
y
Y
s
Z z d
÷
=
= ÷
Isolate the
( )
2 4
u u + terms:
Square and add to eliminate u
4
:
The result is one equation in one unknown u
2
:
2 2
cos sin 0 E F G u u + + =
where:
3
3
2 2 2 2
3 5
2
2
E Zd
F Yd
G Y Z d d
=
=
= ÷ ÷ ÷ +
We can solve this equation for u
2
by using the Tangent HalfAngle Substitution. We presented this
back in the Inverse Pose Solution of the planar 3R robot; we solve for u
2
(in that section, it was for u
1
).
Solve for u
2
by inverting the original Tangent HalfAngle definition:
36
Two u
2
solutions result from the ± in the quadratic formula; both are correct (there are multiple
solutions – elbow up and elbow down). To find u
4
, return to original (arranged) translational equations:
5 24 3 2
5 24 3 2
d s Y d s
d c Z d c
= ÷
= ÷
Find the unique u
4
for each u
2
value (use the quadrantspecific atan2 function in MATLAB):
Solutions Summary
The solution is now complete for the ARMII robot reduced inverse pose problem (translational
joints only, plus
3
0 u = ).
There are multiple solutions: two values for u
1
; for each u
1
, there are two values for u
2
; for each
valid
( )
1 2
, u u , there is a unique u
4
. So there are a total of four
( )
1 2 4
, , u u u solution sets for this reduced
problem. We can show this with the PUMA model (it’s not the same robot, but it has similar joints
when
3
0 u = ).
These four solution sets occur in a very special arrangement pattern, summarized in the table
below.
i u
1
u
2
u
3
u
4
1 u
1
1
2
u
0 u
4
2 u
1
2
2
u
0 –u
4
3
1
180 u +
2
2
u ÷
0 u
4
4
1
180 u +
1
2
u ÷
0 –u
4
In all numerical examples, you can check the results; plug all solution sets
( )
1 2 4
, , u u u one at a
time into the Forward Pose solution and verify that all sets yield the same, commanded
{ }
5
B
P . You can
also calculate the associated
4
B
R
– all should be different (why?).
37
8R ARMII Robot Translational Inverse Pose Kinematics Example
Given
{ }
5
B
P , calculate
( )
1 2 4
, , , 1, 2, 3, 4
i
i u u u = .
{ }
5
5 5
5
0.6572
0.1159
1.6952
B
x
P y
z
÷ ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= = ÷
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
Answers (deg):
i u
1
u
2
u
3
u
4
1 10 20 0 30
2 10 46.6 0 30
3 190 46.6 0 30
4 190 20 0 30
Check all solution sets via Forward Pose Kinematics to ensure all yield the correct, commanded
{ }
5
B
P .
38
2.8 Velocity Kinematics
2.8.1 Velocity Introduction
2.8.1.1 Translational Velocity Example
This example demonstrates the various indices in
( )
k
i
j
V . A is a fixed reference frame.
Given: the absolute velocity of B is 2 @90
in
s
the absolute velocity of C is 4 @45
in
s
From the given information:
( )
0
2
0
A
A
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
;
( )
2.83
2.83
0
A
A
C
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
From the relative velocity equation:
( ) ( ) ( )
A A A
A A B
C B C
V V V = +
( ) ( ) ( )
2.83
0.83
0
A A A
B A A
C C B
V V V
¦ ¹
¦ ¦
= ÷ =
´ `
¦ ¦
¹ )
; 2.95 @16
in
s
39
Since velocity is a free vector, these same velocity vectors ( , ,
A A B
B C C
V V V ) can be expressed in
frames {B} and {C} by using the rotation matrix:
( ) ( )
k A
i k i
j A j
V R V = (or just look at the components)
( )
2
0
0
B
A
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
( )
2.83
2.83
0
B
A
C
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )
( )
0.83
2.83
0
B
B
C
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )
( )
1.41
1.41
0
C
A
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
( )
4
0
0
C
A
C
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
( )
2.58
1.43
0
C
B
C
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )
There are 27 permutations for A, B, C. So far, we have given 9 combinations. There are two
more types:
1)
( ) ( )
k k
j i
i j
V V = ÷ e.g.
( )
0
2
0
A
B
A
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )
(9 of these)
2)
( ) { } 0
k
i
i
V = e.g.
( )
0
0
0
A
B
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
(9 of these)
40
2.8.2 Velocity Equations Derivations
2.8.2.2 Spatial 3P Cartesian manipulator
{ } { }
0 0
X J L =
{ } { }
1 2 3
T
L d d d =
Derive
0
J : Partial derivatives definition
 
i
j
f
J
L
c
c
=
( )
3
0
0
3 2
1
x
y
z
p d
f P p d
p d
¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= = =
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
1
0
x
p
d
c
c
=
2
0
x
p
d
c
c
=
3
1
x
p
d
c
c
=
1
0
y
p
d
c
c
=
2
1
y
p
d
c
c
=
3
0
y
p
d
c
c
=
1
1
z
p
d
c
c
=
2
0
z
p
d
c
c
=
3
0
z
p
d
c
c
=
( ) { }
0
0
3
0 e =
0
0 0 1
0 1 0
1 0 0
J
=
0
1
2
3
0 0 1
0 1 0
1 0 0
x d
y d
z d
¦ ¹
¦ ¹
¦ ¦ ¦ ¦
=
´ ` ´ `
¦ ¦ ¦ ¦
¹ )
¹ )
41
2.8.2.3 Spatial 3R PUMA robot (translational only)
The PUMA translational velocity equations are derived as follows, where W indicates the origin
of the wrist frame {W} (recall the position of the wrist frame origin is a function of only the first three
joint angles ( )
1 2 3
, , u u u :
( ) { }
0
0 1 1 1 2 2 1 23
0
6 1 2 3 0 1 1 1 2 2 1 23
1 2 2 23
, ,
W
x L s L c s L c s
P y L c L s s L s s
z L c L c
u u u
÷ + + ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= = + +
´ ` ´ `
¦ ¦ ¦ ¦
+
¹ ) ¹ )
( )
( )
( )
0
0 1 1 1 1 2 1 1 1 2 2 2 1 23 1 2 1 23 2 3
0
0 1 1 1 1 2 1 1 1 2 2 2 1 23 1 2 1 23 2 3
0
1 2 2 2 23 2 3
W
W
W
x L c L s s L c c L s s L c c
y L s L c s L s c L c s L s c
z L s L s
u u u u u u
u u u u u u
u u u
= ÷ ÷ + ÷ + +
= ÷ + + + + +
= ÷ ÷ +
{ } { }
0
0 1 1 1 2 2 1 23 1 1 2 2 1 23 2 1 23 1
0 1 1 1 2 2 1 23 1 1 2 2 1 23 2 1 23 2
1 2 2 23 2 23 3
0
0
0
W
W
x L c L s s L s s L c c L c c L c c
y L s L c s L c s L s c L s c L s c
z L s L s L s
X J
u
u
u
¦ ¹ ÷ ÷ ÷ + ¦ ¹
¦ ¦ ¦ ¦
= ÷ + + +
´ ` ´ `
¦ ¦ ¦ ¦
÷ ÷ ÷
¹ )
¹ )
= O
Why is the (3,1) term of the Jacobian matrix zero? (
1
u
cannot affect
0
W
z )
42
2.8.3 Jacobian Matrices
Four ways to derive the Jacobian matrix:
1) Time derivatives and relative angular velocity equation
2) Partial derivatives definition – closely related to the first method
f – vector of m Cartesian functions. This relationship holds only for translational part:
That is, no Cartesian orientation vector exists that we can differentiate to get the angular velocity vector.
43
3) Column as velocity due to joint i
Physical interpretation of the Jacobian matrix: each column i is the Cartesian velocity
vector of the endeffector frame with respect to the base frame, due to joint i only, and with
i
u
factored
out.
( ) ( ) ( )
0 0 0
1 2
  
  
k
k
N N N
N
J X X X
=
( )
( )
( )
0
0
0
k
N k
i
N
k i
N
i
V
X
e
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
a) Revolute Joint Columns
where
ˆ ˆ
k k i
i i i
Z R Z = is the third column of
k
i
R and
( ) ( )
k i
i k i
N i N
P R P = where
( )
i
i
N
P is the translational
part of
i
N
T .
Here is Jacobian matrix column i, for a revolute joint:
{ }
( ) ( )
ˆ
ˆ
ˆ ˆ
k
k i i
k i
i i N k
i N
i
k i k
i i i
R Z P
Z P
J
R Z Z
¦ ¹ ¦ ¹
×
×
¦ ¦ ¦ ¦
= =
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
Here is the Jacobian matrix for an allrevolute manipulator:
( ) ( ) ( )
1
1
1
ˆ ˆ ˆ
ˆ ˆ ˆ
k
k k k
k i k N k
i N N N N k
k k k
i N
Z P Z P Z P
J
Z Z Z
¦ ¹ ¦ ¹ ¦ ¹
× × ×
¦ ¦ ¦ ¦ ¦ ¦
=
´ ` ´ ` ´ `
¦ ¦ ¦ ¦ ¦ ¦
¹ ) ¹ ) ¹ )
44
b) Prismatic Joint Columns
where
ˆ ˆ
k k i
i i i
Z R Z = is the third column of
k
i
R.
Here is Jacobian matrix column i, for a prismatic joint:
{ }
ˆ ˆ
0 0
k k i
k
i i i
i
Z R Z
J
¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= =
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
4) Velocity recursion a'la Craig1F
2
Add translational and rotational velocities serially linkbylink from the base to the endeffector
link.
Revolute:
( )
1 1 1
1 1 1
1 1
1 1
ˆ
i i i i
i i i i i
i i i i i
i i i i i
R Z
v R v P
e e u
e
+ + +
+ + +
+ +
+ +
= +
= + ×
Prismatic:
( )
1 1
1
1 1 1
1 1 1 1
ˆ
i i i
i i i
i i i i i i
i i i i i i i
R
v R v P d Z
e e
e
+ +
+
+ + +
+ + + +
=
= + × +
Start with
0 0
0 0
0 v e = = (if the robot base is fixed).
Use the velocity recursion equations, 0,1, 2, , 1 i N = ÷ . Then, factor out
{ }
O
from ,
N N
N N
v e to get
N
J
.
2
J.J. Craig, 2005, Introduction to Robotics: Mechanics and Control, 3
rd
edition, Pearson Prentice Hall, Upper Saddle River, NJ.
45
Derive
0
J
: Partial derivatives definition
For the planar 3R robot.
0
J =
( )
0
0
3
x
y
p
f P
p
¦ ¹
= = =
´ `
¹ )
3 3
1 1
j
i i i
j
j j
j j
d
p p p
t dt
u
c c c
u
c cu cu
= =
= =
¯ ¯
i = x, y
1
x
p c
cu
=
2
x
p c
cu
=
3
x
p c
cu
=
1
y
p c
cu
=
2
y
p c
cu
=
3
y
p c
cu
=
( ) ( ) ( ) ( )
( ) ( )
0 0 0 0
0 0 1 2
3 1 2 3
0
0
3 1 2 3
ˆ
Z
e e e e
e u u u
= + +
= + +
0
J =
46
2.8.8 Velocity Kinematics Examples
2.8.8.2 Spatial 3P Cartesian Manipulator Velocity Example
a) Forward Velocity Kinematics
{ } { }
0 0
0
1 3
2 2
3 1
0 0 1
0 1 0
1 0 0
X J L
x d d
y d d
z d d
=
¦ ¹ ¦ ¹
¦ ¹
¦ ¦ ¦ ¦ ¦ ¦
= =
´ ` ´ ` ´ `
¦ ¦ ¦ ¦ ¦ ¦
¹ )
¹ ) ¹ )
b) Inverse Velocity Kinematics (analytical result)
{ } { }
1
0 0
0 0
1
2
3
0 0 1
0 1 0
1 0 0
L J X
d x z
d y y
d z x
÷
=
¦ ¹
¦ ¹ ¦ ¹
¦ ¦ ¦ ¦ ¦ ¦
= =
´ ` ´ ` ´ `
¦ ¦ ¦ ¦ ¦ ¦
¹ ) ¹ )
¹ )
c) Singularity Analysis
0 1 2 3
1 J J J J = = = = no possible singularities
d) Static Force Analysis
{ } { }
0 0
0 0
1
2
3
0 0 1
0 1 0
1 0 0
T
x z
y y
z x
f J F
f F F
f F F
f F F
=
¦ ¹ ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦ ¦ ¦
= =
´ ` ´ ` ´ `
¦ ¦ ¦ ¦ ¦ ¦
¹ ) ¹ ) ¹ )
47
2.8.9 Jacobian Matrix Expressed in Another Frame
Here the Jacobian matrix still relates the endeffector frame velocity with respect to the base
frame. But simpler analytical expressions are possible for the Jacobian matrix, by choosing an
intermediate frame to express the coordinates of the velocity vectors (different basis of expression).
 
 
0
0
0
0
0
k k
k
R
v v
R
e e
¦ ¹ ¦ ¹
=
´ ` ´ `
¹ ) ¹ )
{ }
k
k
v
J
e
¦ ¹
= O
´ `
¹ )
{ }
0
0
v
J
e
¦ ¹
= O
´ `
¹ )
{ }
 
 
{ }
0
0
0
0
0
k
k
k
R
J J
R
O = O
O
is not dependent on a frame (relative joint rates)
 
 
0
0
0
0
0
k
k
k
R
J J
R
=
Planar 3R Robot
The Jacobian was derived in frame {0} (here we go to {3} instead of {H}).
1 1 2 12 2 12
0
1 1 2 12 2 12
0
0
1 1 1
L s L s L s
J Lc L c L c
÷ ÷ ÷
= +
2 2 2 2
1
1 2 2 2 2
0
0
1 1 1
L s L s
J L L c L c
÷ ÷
= +
1 1
1
0 1 1
0
0
0 0 1
c s
R s c
= ÷
1 2
2
1 2 2 2
0 0
0
1 1 1
L s
J Lc L L
= +
12 12
2
0 12 12
0
0
0 0 1
c s
R s c
= ÷
1 23 2 3 2 3
3
1 23 2 3 2 3
0
0
1 1 1
L s L s L s
J Lc L c L c
+
= +
123 123
3
0 123 123
0
0
0 0 1
c s
R s c
= ÷
3
J
agrees with that derived in the third method above.
48
2.8.10 Cartesian Transformation of Velocities and Wrenches
Here we show how to move velocity and wrench vectors (both translational and rotational) from
one point to another on a rotating rigid body. We can replace one vector with an equivalent vector
acting at a different point. For example, to calculate the velocity (or wrench) at the wrist to produce a
desired velocity (or wrench) at the hand.
2.8.10.1 Velocity Transformation
Here we find the equivalent velocity at {A} corresponding to a given desired motion of {B}. For
instance, {B} could be the hand frame {H} where we want motion and {A} would then be the last wrist
frame {n} for which the Jacobian matrix is derived.
Basic equations:
A
B A A B
B A
v v P e
e e
= + ×
=
reversing:
A A
A B B B B B B
A B
v v P v P e e
e e
= ÷ × = + ×
=
All vectors must be expressed in same frame, choose {A}.
 
0 0
0 0
0
A A A A B
B B B
A B
A
A B
B
R P R
V V
R
e e
×
¦ ¹ ¦ ¹
=
´ ` ´ `
¹ ) ¹ )
{ } { }
A B
A
B V
X T X =
  0
A A A
B B B
A
B V
A
B
R P R
T
R
×
=
where:
0
0
0
z y
A
B z x
y x
p p
P p p
p p
÷
× = ÷
÷
is the cross product matrix.
49
Velocity transformation example
Find the equivalent velocity at A corresponding to a given motion of B.
Given:
{ }
0
1
1
0
B
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
{ }
0
0
0
2
B
B
e
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
{ }
2.6
1.5
0
A
B
P
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
0.866 0.5 0
0.5 0.866 0
0 0 1
A
B
R
÷
=
0 0 1.5 0.866 0.5 0 0 0 1.5
0 0 2.6 0.5 0.866 0 0 0 2.6
1.5 2.6 0 0 0 1 0 3 0
A A
B B
P R
÷
× = ÷ = ÷
÷
{ } { }
A B
A
B V
X T X =
0
0
0.866 0.5 0 0 0 1.5 1 3.366
0.5 0.866 0 0 0 2.6 1 3.834
0 0 1 0 3 0 0 0
0 0 0 0.866 0.5 0 0 0
0 0 0 0.5 0.866 0 0 0
0 0 0 0 0 1 2 2
A
A
A
V
e
÷ ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
÷ ÷
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦ ¦ ¹
= =
´ ` ´ ` ´ `
÷
¹ ) ¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
¹ ) ¹ )
Check:
A B
e e =
A B
V V r e = ÷ ×
{ }
0
ˆ ˆ ˆ
1 1 0 1
1 0 0 2 1 6 5
0 3 0 0 0 0 0
B
A
i j k
V
¦ ¹ ¦ ¹ ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦ ¦ ¦ ¦ ¦
= ÷ = ÷ = ÷
´ ` ´ ` ´ ` ´ `
¦ ¦ ¦ ¦ ¦ ¦ ¦ ¦
¹ ) ¹ ) ¹ ) ¹ )
50
2.8.10.2 Pseudostatic Wrench Transformation
Here we find the equivalent wrench at {A} corresponding to a given wrench at {B}. For
instance, {B} could be the hand frame {H} where we want the wrench to be exerted and {A} would then
be the last wrist frame {n} for which the Jacobian matrix is derived.
Basic equations:
A B
A
A B B B
F F
M M P F
=
= + ×
more properly,
k
B
F
All vectors must be expressed in the same frame, choose {A}.
  0
A A B
B
A B
A A A
A B
B B B
R
F F
M M
P R R
¦ ¹ ¦ ¹
=
´ ` ´ `
× ¹ ) ¹ )
{ } { }
A B
A
B W
W T W =
  0
A
B
A
B W
A A A
B B B
R
T
P R R
=
×
Note:
A
B W
T
is a blocktranspose of
A
B V
T
; i.e.
A A
B B
P R ×
is switched with 0.
There is a duality here: in the velocity transformation, the rotational term is unchanged, while in
the wrench transformation, the translational term is unchanged.
51
Wrench transformation example
Find the equivalent wrench at A corresponding to a given wrench at B.
Given: { }
1
1
0
B
B
F
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
{ }
0
0
0
B
B
M
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
{ }
, ,
A A A A
B B B B
P R P R ×
are the same as in the velocity example above.
{ } { }
A B
A
B W
W T W =
0.866 0.5 0 0 0 0 1 0.366
0.5 0.866 0 0 0 0 1 1.366
0 0 1 0 0 0 0 0
0 0 1.5 0.866 0.5 0 0 0
0 0 2.6 0.5 0.866 0 0 0
0 3 0 0 0 1 0 3
A
A
A
F
M
÷ ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
¦ ¹
= =
´ ` ´ ` ´ `
÷
¹ )
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
÷
¦ ¦ ¦ ¦
¹ ) ¹ )
Check:
A B
F F =
A B
M r F = ×
Note: the derivations and examples for both velocity and wrench transformations were for the inverse
case, i.e. given the end vector values {B}, calculate the vector values on the same rigid link, but inward
toward {A}.
We could easily adapt the derivations and transformations to perform the forward calculation, i.e.
given the inward vector values {A}, calculate the end vector values on the same rigid link, outward
towards {B}.
52
2.9 Acceleration Kinematics
The acceleration vector is the first time derivative of the velocity vector (and the second time
derivative of the position vector). It is linear in acceleration terms but nonlinear in rate components.
Both Cartesian acceleration and velocity are vectors, with translational and rotational parts.
Acceleration Kinematics Analysis is useful for:
 Resolved Acceleration Control
 Acceleration of any point on manipulator
 Moving objects in workspace – smooth trajectory generation
 Required for Dynamics
Translational Acceleration
( )
k
i
j
A is the translational acceleration of the origin of frame {j} with respect to reference frame
{i}, expressed in the basis (coordinates) of {k}.
Transport Theorem (Craig, 2005):
( )
A B A B A A B
B B Q B B
d
R Q R V R Q
dt
e = + ×
General fivepart acceleration equation:
Position:
A A A B
B B
A A B
B
P P R P
P T P
= +
=
Velocity:
0
A A A B A B
B B B
P
V P R P R P
V V V P e
= + +
= + + ×
Acceleration:
( )
( )
( )
( )
0
2
2
A A A B A A B
B B P B B P
A A A B A A B A A B A A B A A A B
B B P B B P B B P B B P B B B P
A A A B A A B A A B A A A B
B B P B B P B B P B B B P
P
d
A V R V R P
dt
A A R A R V R P R V R P
A A R A R V R P R P
A A A V P P
e
e o e e e
e o e e
e o e e
= + + ×
= + + × + × + × + × ×
= + + × + × + × ×
= + + × + × + × ×
53
Rotational Acceleration
( )
k
i
j
o is the rotational acceleration of frame {j} with respect to reference frame {i}, expressed
in the basis (coordinates) of {k}. No angular orientation exists for which we can take the time derivative
to find the angular velocity. Instead we use relative velocity equations:
Rotational Velocity:
A A A B
C B B C
R e e e = +
Rotational Acceleration:
( ) ( )
A A A B A A B
C B B C B B C
A A A B A A B
C B B C B B C
d d
R R
dt dt
R R
o e e o e
o o o e e
= + = +
= + + ×
For vectors expressed in the local frame:
( ) ( ) ( ) ( ) ( )
A B C B C
A A A A B A A A B
C B B C C B B C C
R R R R o o o e e = + + ×
Combined Translational and Rotational Acceleration
{ }
a
X
o
¦ ¹
=
´ `
¹ )
where:
{ } ( )
{ } ( )
0
0
0
0
N
N
a A
o o
=
=
both a and o are (3x1) vectors
Acceleration Example
Planar 2R robot – acceleration of endeffector frame with respect to {0}, also expressed in {0}.
1) Craig (2005) acceleration recursion
{ }
1
1
2
0
0
L
P
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
{ }
2
2
3
0
0
L
P
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
2 2
2
1 2 2
0
0
0 0 1
c s
R s c
= ÷
 
3
2 3
R I =
0
0
0
0
0
e
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
0
0
0
0
0
o
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
0
0
0
0
0
a
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
1
1
1
0
0 e
u
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
1
1
1
0
0 o
u
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
1
1
0
0
0
a
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
54
2
2
1 2
0
0 e
u u
¦ ¹
¦ ¦
=
´ `
¦ ¦
+
¹ )
2
2
1 2
0
0 o
u u
¦ ¹
¦ ¦
=
´ `
¦ ¦
+
¹ )
( ) ( )
2 2 1 1 1 1 1 1
2 1 1 2 1 1 2 1
2 2
2 2 1 1 1 2 1 1 2 1
2 2
2 2 2 1 1 1 2 1 1 2 1
0
0
0 0 1 0 0
a R P P a
c s L L c L s
a s c L L s L c
o e e
u u u
u u u
= × + × × +
¦ ¹ ¦ ¹ ÷ ÷ +
¦ ¦ ¦ ¦
= ÷ = +
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
3 2
3 2
1 2
0
0 o o
u u
¦ ¹
¦ ¦
= =
´ `
¦ ¦
+
¹ )
( ) ( )
( )
( )
3 3 2 2 2 2 2 2
3 2 2 3 2 2 3 2
2
2
2 1 2 1 2 1 1 2 1
3 2
3 2 1 2 1 2 1 1 2 1
0
a R P P a
L L c L s
a L L s L c
o e e
u u u u
u u u u
= × + × × +
¦ ¹
÷ + ÷ +
¦ ¦
¦ ¦
= + + +
´ `
¦ ¦
¦ ¦
¹ )
( ) ( ) ( )
( )
( ) ( ) ( )
( )
0 0 3
3 3 3
2
2
1 1 1 1 1 2 12 1 2 12 1 2
2
0 2
3 1 1 1 1 1 2 12 1 2 12 1 2
0
a R a
L s c L s c
a L c s L c s
u u u u u u
u u u u u u
=
¦ ¹
÷ + ÷ + + +
¦ ¦
¦ ¦
¦ ¦
= ÷ + + ÷ +
´ `
¦ ¦
¦ ¦
¦ ¦
¹ )
Rewrite for comparison to results in the next section:
( )
( )
( )
1 1 1 2 12 1 2
1
1 1 2 12 2 12 0 1
3
1 1 2 12 2 12 1 2 2
1 1 1 2 12 1 2
L c L c
L s L s L s
a
L c L c L c
L s L s
u u u
u
u
u u u
u u u
÷ ÷ + ¦ ¹
÷ ÷ ÷ ¦ ¹
¦ ¦
= +
´ ` ´ `
+ +
÷ ÷ + ¹ ) ¦ ¦
¹ )
The second term above can be written as:
( ) ( )
( ) ( )
1 1 1 2 12 1 2 2 12 1 2
1
2
1 1 1 2 12 1 2 2 12 1 2
L c L c L c
L s L s L s
u u u u u
u
u
u u u u u
÷ ÷ + ÷ +
¦ ¹
´ `
÷ ÷ + ÷ +
¹ )
55
Acceleration Example (cont.)
Planar 2R robot – acceleration of the endeffector frame with respect to {0}, also expressed in
{0}. Alternative method:
2) Differentiation of rate equation
{ }  { }
X J u =
{ }  { } { }
X J J u u = +
Do in frame {0} – if in frame {2} or {3}, one must account for r e× .
1 1 2 12 2 12
0
1 1 2 12 2 12
1 1
L s L s L s
J Lc L c L c
÷ ÷ ÷
= +
 
ij
dJ d J
J
dt dt
= =
1 2
1 2
ij ij ij ij
N
N
dJ J J J
d d d
dt dt dt dt
c c c
u u u
cu cu cu
= + + +
1
N
ij ij
k
k
k
dJ J
dt
c
u
cu
=
=
¯
( )
( )
1 1 2 12 1 2 12 2 2 12 1 2 12 2
0
1 1 2 12 1 2 12 2 2 12 1 2 12 2
0 0
L c L c L c L c L c
J L s L s L s L s L s
u u u u
u u u u
÷ + ÷ ÷ ÷
= ÷ + ÷ ÷ ÷
( ) ( )
( ) ( )
1 1 1 2 12 1 2 2 12 1 2
0
1 1 1 2 12 1 2 2 12 1 2
0 0
L c L c L c
J L s L s L s
u u u u u
u u u u u
÷ ÷ + ÷ +
= ÷ ÷ + ÷ +
{ } { } { }
0 0 0
X J J u u = +
{ }
( ) ( )
( ) ( )
1 1 1 2 12 1 2 2 12 1 2
1 1 2 12 2 12
0 1 1
1 1 2 12 2 12 1 1 1 2 12 1 2 2 12 1 2
2 2
1 1
0 0
L c L c L c
L s L s L s
X L c L c L c L s L s L s
u u u u u
u u
u u u u u
u u
÷ ÷ + ÷ +
÷ ÷ ÷
¦ ¹ ¦ ¹
= + + ÷ ÷ + ÷ +
´ ` ´ `
¹ ) ¹ )
This yields the same result as before.
56
Uses for general acceleration equation
{ }  { }
{ }  { } { }
{ }   { } { } ( )
1
X J
X J J
J X J
u
u u
u u
÷
=
= +
= ÷
i. Forward Acceleration Kinematics Analysis –
{ }  { } { }
X J J u u = +
predicts the Cartesian
accelerations
{ }
X
given the joint rates and accelerations.
ii. Resolved Acceleration Control – like resolved rate, but acceleration is commanded instead of
velocity. Solve
{ }   { } { } ( )
1
J X J u u
÷
= ÷
and double integrate to get the commanded joint
angles.
iii. Dynamics equations –
{ }
u
is required for the Newton/Euler dynamics recursion in this
429/529 NotesBook Supplement, Chapter 4. If acceleration is calculated via numerical
differentiation, numerical instability can result, so the analytical approach
{ }   { } { } ( )
1
J X J u u
÷
= ÷
is better.
Now, if inverse dynamics control is being used in the resolvedrate control algorithm
framework, assume
{ }
X
is constant and so
{ } { } 0 X =
. In this case:
{ }   { }
1
J J u u
÷
= ÷
How can we find the time rate of change of the Jacobian matrix J
? See the previous page for a
specific example.
57
2.10 Robot Workspace
The workspace (also called work envelope) of a robot manipulator is defined as the volume in
space that a robot’s endeffector can reach. The workspace limits are due to the total reach of the
combined robot links (both extended and folded upon each other). The workspace limits are also due to
the specific joint limits of each joint.
Workspace is defined for both position (translational workspace) and orientation (rotational
workspace). The translational workspace is easier to determine and display, and is the focus of this
section. The figure below shows the translational workspace of a Cybotech P15 articulated robot, in
side (left) and top (right) views.
Generally large translational and rotational workspaces are desired for a more capable robot.
Generally serial robots have a big advantage over parallel robots in terms of larger workspace
(exception: parallel cablesuspended robots).
Cybotech P15 Manipulator Translational Workspace
58
The robot workspace attainable at any orientation is called the reachable workspace. The robot
workspace attainable at all orientations is called the dextrous workspace. The dextrous workspace is
a subset of the reachable workspace. For parallel manipulators the dextrous workspace is often null;
therefore a limited dextrous workspace is generally defined (such as all orientations within 30 ±
rotation
about all three axes).
The reachable workspace of the Adept 550 SCARA robot is shown below. This topview planar
workspace shape is extended 200 mm in the vertical direction by the prismatic joint limits on d
3
(into the
paper), to form a volume.
Adept 550 SCARA Robot Reachable Workspace
Adept 550 TableTop Robot User’s Guide, 1995.
The next five images represent the reachable, or translational, workspaces of five common
manipulator arrangements (Cartesian, cylindrical, spherical, SCARA, and articulated).
59
Cartesian Manipulator Translational Workspace
Cylindrical Manipulator Translational Workspace
60
Spherical Manipulator Translational Workspace
SCARA Manipulator Translational Workspace
61
Articulated Manipulator Translational Workspace
The previous five images came from:
societyofrobots.com/robot_arm_tutorial.shtml
62
The figure below presents the translational workspace for a parallel robot (the threeaxis
Orthoglide). Imagine how much greater the translational workspace would be if there were only one
supporting serial chain to the endeffector. However, the tradeoff is that the parallel robot achieves
much greater accelerations, stiffness, and accuracy, with lower moving mass, than an equivalent serial
robot with a larger workspace.
Orthoglide Robot Reachable Workspace
parallemic.org/Reviews/Review011.html
63
Again, parallel robots such as the 3RRR shown generally have small workspaces.
3RRR Parallel Robot Sample Limited Workspace4F
3
Optimized 3RRR Parallel Robot Workspace
1
A notable exception to the small workspace
rule of parallel robots is the cablesuspended
robots. These devices, such as the NIST
RoboCrane (right, developed at NIST by Dr.
James Albus), can be designed with arbitrarily
large workspaces. Cable suspended robots with
workspaces on the order of several kilometers have
been proposed.
3
R.L. Williams II, 1988, Planar Robotic Mechanisms: Analysis and Configuration Comparison, Ph.D. Dissertation,
Department of Mechanical Engineering, Virginia Polytechnic Institute and State University, Blacksburg, VA, August.
64
Chapter 3. KinematicallyRedundant Robots
3.2 Inverse Velocity (ResolvedRate) Solution
3.2.1 Pseudoinversebased
Particular Solution – satisfies primary task (satisfies Cartesian trajectory)
Minimum Manipulator Kinetic Energy
  ( ) W M = O
; the manipulator inertia tensor
{ }   { }
P
J X

O =
where:           
( )
1
1 1 T T
J M J J M J
÷
 ÷ ÷
=
This MoorePenrose pseudoinverse form is subject to singularities. Singular Value
Decomposition (SVD) would ameliorate this problem.
3.2.3 Klein and Huang’s Algorithm
This methods yields the same results as the pseudoinverse with gradient projection into the null
space, but it is more efficient (less computations). Klein and Huang’s algorithm accomplishes the
particular and homogeneous solutions at the same time.
  
( )
{ } { }   ( ) { }
T
J J w X k J H = ÷ V O
;
solve {w} using Gaussian elimination, then:
{ }   { } ( ) { }
T
J w k H O = + V O
.
So much of the existing kinematicallyredundant robot literature is dedicated to more efficient
redundancy resolution, but I think with today’s processors, this is no longer a problem.
65
3.2.4 Singular Value Decomposition
Singular Value Decomposition (SVD) yields the same results as the pseudoinverse with gradient
projection into the nullspace, but with singularity robustness. If [J] is less than full rank, the solution
cannot track arbitrary Cartesian trajectories, but SVD results are bounded so the motion can drive
through singularities, as opposed to yielding infinite joint rates at singularities.
     
T
J U W V =
J m x n
U m x n column orthogonal
W n x n positive semidefinite diagonal
V n x n orthogonal
     
1 T
j
J V diag U
w

 
= 

\ .
w
j
are the singular values of [J]. For underconstrained systems of equations, there will always be
n – m zero singular values, where n – m is the degree of redundancy. Any additional zero singular
values correspond to degeneracies in J. In the above expression,
1
j
w
is set to zero for 0
j
w = (ain’t
math fun?!?)
Both U and V are columnorthonormal (ignoring the last n – m columns of U). V is also row
orthonormal, i.e.:
        
T T
n
V V V V I = =
    
T
m
U U I = ; however,
     
T
n
U U I = (see the SVD example)
Columns of U corresponding to nonzero
j
w are an orthonormal basis which spans the range of
J. Columns of V corresponding to zero w
j
are an orthonormal basis for the nullspace of J.
66
Singular Value Decomposition (SVD) Example
 
1.366 0.500 0.500
2.366 1.866 0.866
J
÷ ÷ ÷
=
     
T
J U W V =
 
3.467 0 0 0.786 0.514 0.344
0.430 0.903 0
0 0.420 0 0.548 0.836 0
0.903 0.430 0
0 0 0 0.288 0.188 0.939
T
J
÷ ÷
÷
=
÷
     
1 T
j
J V diag U
w

 
= 

\ .
 
*
0.786 0.514 0.344 0.288 0 0 0.430 0.903 1.205 0.323
0.548 0.836 0 0 2.381 0 0.903 0.430 1.732 1.000
0.288 0.188 0.939 0 0 0 0 0 0.441 0.118
J
÷ ÷ ÷ ÷ ÷
= =
÷ ÷ ÷
In the above, the third singular value is zero, so
1
j
w
is set to zero (n – m singular values will always be
zero). This result agrees with
 
*
J from the MoorePenrose pseudoinverse formula in the 429/529
NotesBook.
Note:
    
2
T
U U I =    
3
1 0 0
0 1 0
0 0 0
T
U U I
= =
        
3
T T
V V V V I = =
67
3.2.5 Generalized Inverses
A generalized inverse of a matrix gives an answer to the linear problem even when a true matrix
inverse does not exist (underconstrained, overconstrained, or rowrankdeficient). Mathematically, G is
a generalized inverse of matrix A if:
      A G A G =
The MoorePenrose pseudoinverse is just one possible generalized inverse of a matrix. It is the
one applied most often to redundancy resolution of manipulators. In addition to the above relationship,
the following relationships hold for the MoorePenrose pseudoinverse:
     
   ( )   
   ( )   
*
*
G A G A
G A G A
A G A G
=
=
=
where
( )
*
indicates the complex conjugate transpose.
68
Chapter 4. Manipulator Dynamics
Kinematics is the study of motion without regard to forces.
Dynamics is the study of motion with regard to forces. It is the study of the relationship between
forces/torques and motion. Dynamics is composed of kinematics and kinetics.
a) Forward Dynamics (simulation) – given the actuator forces and torques, compute the
resulting motion (this requires the solution of highly coupled, nonlinear ODEs): Given t, calculate
, , u u u
(all are N x 1 vectors).
b) Inverse Dynamics (control) – given the desired motion, calculate the actuator forces and
torques (this linear algebraic solution is much more straightforward than Forward Dynamics): Given
, , u u u
, calculate t (all N x 1 vectors).
Both problems require the N dynamic equations of motion, one for each link, which are highly coupled
and nonlinear. Methods for deriving the dynamic equations of motion:
 NewtonEuler recursion (force balance, including inertial forces with D'Alembert's principle).
 LagrangeEuler formulation (energy method).
Kinetics:
Translational: Newton's Second Law:
Inertial force at center of mass
Rotational: Euler's Equation:
Inertial moment at center of mass (could be anywhere on body)
The kinematics terms, , ,
Ci i i
a e o , must be moving with respect to an inertiallyfixed frame.
The frame of expression {k} needn't be an inertiallyfixed frame.
Assumptions:
 serial robot
 rigid links
 ignore actuator dynamics
 no friction
 no joint or link flexibility
69
4.1 Mass Distribution: Inertia Tensor
Spatial generalization of planar scalar moment of inertia; units: md
2
(kgm
2
). The inertia
tensor expressed at a given point in the rigid body, relative to frame {A} is:
Mass moments of inertia
( )
2 2
xx
V
I y z dv p = +
ííí
( )
2 2
yy
V
I x z dv p = +
ííí
( )
2 2
zz
V
I x y dv p = +
ííí
Mass products of inertia
xy
V
I xy dv p =
ííí
xz
V
I xz dv p =
ííí
yz
V
I yz dv p =
ííí
Principal moments of inertia
A certain orientation of the reference frame {A}, the principal axes, yields zero products of
inertia. The invariant eigenvalues of a general
A
I are the principal moments of inertia, and the
eigenvectors are the principal axes.
More interesting facts regarding inertia tensors
1) If two axes of the reference frame form a plane of symmetry for the mass distribution, the
products of inertia normal to this plane are zero.
2) Moments of inertia must be positive, products of inertia may be either sign.
3) The sum of the three moments of inertia are invariant under rotation transformations.
Parallelaxis theorem
We can obtain the mass moment of inertia tensor at any point {A} if we know the inertia tensor
at the center of mass {C} (assuming these two frames have the same orientation): { }
T
C C C C
P x y z =
is the vector giving the location of the center of mass {C} from the origin of {A}.
Here are some example inertia tensor components using the parallel axis theorem:
( )
2 2 A C
zz zz C C
A C
xy xy C C
I I m x y
I I mx y
= + +
= ÷
Here is the entire inertia tensor expressed in vectormatrix form using the parallel axis
theorem:
3
A C T T
C C C C
I I m P P I P P = + ÷
70
4.2 NewtonEuler Recursive Algorithm
This is a recursive approach (Craig, 2005) based on freebody diagrams (FBDs) to determine the
dynamics relationships linkbylink. Used numerically it can calculate the inverse dynamics solution
efficiently.
Freebody diagram of link i:
f
i
: internal force exerted on link i by link i1.
n
i
: internal moment exerted on link i by link i1.
Inertial loads Newton and Euler translational and rotational dynamics equations:
Force balance
Moment balance (about CG
i
) (using D'Alembert’s principle, the inertial force is –ma).
71
NewtonEuler Recursive Algorithm Summary
This methods can be used to find the robot dynamics equations of motion. It can also be used to
directly solve the inverse dynamics problem numerically. The summary of equations below, from Craig
(2005), assume an all revolutejoint manipulator (prismatic joint dynamics have different equations).
Outward iteration for kinematics: : 0 1 i N ÷ ÷
(without regard for frames of expression, for clarity)
Velocities and accelerations (kinematics):
( )
( )
1 1 1
1 1 1 1 1
1 1 1
1 1
1 1 1 1 1 1 1
ˆ
ˆ ˆ
i i i i
i i i i i i i
i i
i i i i i i i
i i
Ci i i Ci i i Ci
Z
Z Z
a a P P
a a P P
e e u
o o e u u
o e e
o e e
+ + +
+ + + + +
+ + +
+ +
+ + + + + + +
= +
= + × +
= + × + × ×
= + × + × ×
Inertial loading (kinetics):
1 1 1
1 1 1 1
i i Ci
C C
i i i i
F m a
N I I o e e
+ + +
+ + + +
=
= + ×
Inward iteration for kinetics: : 1 i N ÷
(without regard for frames of expression, for clarity)
Internal forces and moments:
1
1 1 1
i i i
i i
i i Ci i i i i
f f F
n n P F P f N
+
+ + +
= +
= + × + × +
Externally applied joint torques:
i i i
n Z t = ·
Inclusion of gravity forces:
0
0
a g =
This is equivalent to a fictitious upward acceleration of 1g of the robot base, which accounts for
the downward acceleration due to gravity (i.e. this conveniently includes the weight of all links).
72
4.3 LagrangeEuler Energy Method
This is an alternative method to find the robot dynamics equations of motion. It requires only
translational and rotational link velocities, not accelerations. The Lagrangian is formed from the
kinetic energy k and potential energy u of the robot system.
( )
( ) , L k u k u u u u = ÷ = ÷
0 0
1 1
2 2
T T Ci
i i Ci Ci i i i
i REF i Ci
k mV V I
u u m g P
e e = +
= ÷ ·
( )
1
,
N
i
i
k k u u
=
=
¯
( )
1
N
i
i
u u u
=
=
¯
Note:
( )
( )
1
,
2
T
k M u u = O O O
; where ( ) M O is the manipulator mass matrix.
Dynamic equations of motion
These are found for each active joint from the following expression involving the Lagrangian,
joint variable, and actuator torque. Perform this equation N times, once for each joint variable, to yield
N independent dynamics equations of motion.
d L L
dt
c c
t
cu cu
 
÷ =

\ .
This expression may be rewritten using
( )
( ) , L k u u u u = ÷
:
d k k u
dt
c c c
t
cu cu cu
 
÷ + =

\ .
73
4.4 Simple Dynamics Example
Derive the dynamic equation of motion for a planar onelink 1R mechanism by three methods:
1) Sophomore dynamics method – FBD, force and moment dynamics balance.
2) NewtonEuler recursion.
3) LagrangeEuler formulation.
1) Sophomore methods  FBD, force balance FreeBody Diagram:
Y
L
X
e. o
t
g
u
t
mg
F
X
F
Y
÷t
F
Y
F
X
74
1) Sophomore methods – FBD, force balance (cont.)
a)
( )
0 2
0 2
2 2
2
x Cx
x
x
F ma
L L
F m s c
mL
F s c
o u e u
u u u u
=
 
= ÷ ÷

\ .
= ÷ +
¯
b)
( )
0 2
0 2
2 2
2
y Cy
y
y
F ma
L L
F mg m c s
L
F mg m c s
o u e u
u u u u
=
 
÷ = ÷

\ .
= + ÷
¯
c)
0
0
0
2
2
z
mL
M I gc
mL
I gc
u t u
t u u
= = ÷
= +
¯
where:
2
0 2
4
C C
zz
mL
I I md I = + = +
75
Simple Dynamics Example (cont.)
2) NewtonEuler recursion
Outward iteration: i = 0 (the only iteration)
0
1
0
0
0
P
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
1
1
2
0
0
C
L
P
¦ ¹
¦ ¦
¦ ¦
=
´ `
¦ ¦
¦ ¦
¹ )
1
0
0
0
0 0 1
c s
R s c
u u
u u
= ÷
1
1
0 0
0 0
0 0
xx
C
yy
zz
I
I I
I
=
0
0
0
0
0
e
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
0
0
0
0
0
o
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
0
0
0
0
a g
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
to account for gravity
( ) ( )
1 1 0 0 0 0 0 0
1 0 0 1 0 0 1 0
1
1
0 0
0
0 0 1 0 0
a R P P a
c s gs
a s c g gc
o e e
u u u
u u u
= × + × × +
¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= ÷ =
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
( )
2
1 1 1 1 1 1 1
1 1 1 1 1 1 1
2
2
0
C C C
L
gs
L
a P P a gc
u u
o e e u u
¦ ¹
÷
¦ ¦
¦ ¦
¦ ¦
= × + × × + = +
´ `
¦ ¦
¦ ¦
¦ ¦
¹ )
2
1 1
1 1
2
2
0
C
L
gs
L
F m a m gc
u u
u u
¦ ¹
÷
¦ ¦
¦ ¦
¦ ¦
= = +
´ `
¦ ¦
¦ ¦
¦ ¦
¹ )
1 1
1 1 1 1
1 1 1 1 1 1
0
0
C C
zz
N I I
I
o e e
u
¦ ¹
¦ ¦
= + × =
´ `
¦ ¦
¹ )
76
Inward iteration: i = 1 (the only iteration)
2
2
0
0
0
f
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
2
2
0
0
0
n
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
2
1 1 2 1
1 2 2 1
2
2
0
L
gs
L
f R f F m gc
u u
u u
¦ ¹
÷
¦ ¦
¦ ¦
¦ ¦
= + = +
´ `
¦ ¦
¦ ¦
¦ ¦
¹ )
1
1 1 2 1 1 1 1 2 1
1 2 2 1 2 2 2 1
1
1
0 0
0 0
2 2
C
zz
n R n P F P R f N
n
I
mL L
gc
u
u u
= + × + × +
¦ ¹
¦ ¦
¦ ¹
¦ ¦
¦ ¦ ¦ ¦
= +
´ ` ´ `
¦ ¦ ¦ ¦
 
¹ )
¦ ¦
+

¦ ¦ \ . ¹ )
1 1
1 1
2
ˆ
4 2
zz
n Z
mL mL
I gc
t
t u u
= ·
 
= + +

\ .
Check with the sophomore dynamics method above:
2 2
0 0 1 2
1 1 1
2 2 2
0
0
2 2 2
0 0 1
0 0
L L L
gs c s
c s
L L L
f R f s c m gc m s c g
u u u u u u
u u
u u u u u u u u
¦ ¹ ¦ ¹
÷ ÷ ÷
¦ ¦ ¦ ¦
÷
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
= = + = ÷ + +
´ ` ´ `
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
¦ ¦ ¦ ¦
¹ ) ¹ )
77
3) LagrangeEuler formulation
1
1
0
0 e
u
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
1
1
0 0
0 0
0 0
xx
C
yy
zz
I
I I
I
=
Kinetic and potential energy are scalars invariant with frame of expression – write k in {1} and u in {0}.
1
1
0
2
0
C
L
v u
¦ ¹
¦ ¦
¦ ¦
=
´ `
¦ ¦
¦ ¦
¹ )
1
1 1
1 1 1
0 0
0 0
C
zz
I
I
e e
u u
¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
· = ·
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )
2
2 2
1 1
2 4 2
zz
mL
k I u u = +
2
0
0
2 2
0
0
L
c
L L
u m g s mg s
u
u u
¦ ¹
¦ ¦
¦ ¹
¦ ¦
¦ ¦ ¦ ¦
= ÷ ÷ · =
´ ` ´ `
¦ ¦ ¦ ¦
¹ )
¦ ¦
¦ ¦
¹ )
2
2
1
2 4 2
zz
mL L
L k u I mg s u u
 
= ÷ = + ÷

\ .
d L L
dt
c c
t
cu cu
 
÷ =

\ .
2
4
zz
L mL
I
c
u
cu
 
= +

\ .
2
4
zz
d L mL
I
dt
c
u
cu
 
 
= +
 
\ .
\ .
2
L L
mg c
c
u
cu
= ÷
2
4 2
zz
mL mL
I gc t u u
 
= + +

\ .
This result agrees with the sophomore and NewtonEuler recursion dynamics methods above to solve
this same problem.
78
4.5 Structure of Manipulator Dynamics Equations
State Space Equation
( ) ( ) ( ) , M V G t = O O+ O O + O
( ) M O N N × mass matrix; symmetric and positive definite
( )
, V O O
1 N × vector of Coriolis and centrifugal terms
( ) G O 1 N × vector of gravity terms
Configuration Space Equation
( ) ( ) ( ) ( )
2
M B C G t = O O+ O OO + O O + O
( ) M O N N × mass matrix; symmetric and positive definite
( ) B O
( ) 1
2
N N
N
÷
× Coriolis matrix
( ) C O N N × centrifugal matrix
( ) G O 1 N × vector of gravity terms
OO
( ) 1
1
2
N N ÷
× :
{ }
1 2 1 3 1
T
N N
u u u u u u
÷
2
O
1 N × :
{ }
2 2 2
1 2
T
N
u u u
Cartesian State Space Equation
( ) ( ) ( ) ,
x x x
F M X V G = O + O O + O
( )
x
M O N N × Cartesian mass matrix; symmetric and positive definite
( )
,
x
V O O
1 N × vector of Cartesian Coriolis and centrifugal terms
( )
x
G O 1 N × vector of gravity terms in Cartesian space
where: note:
( ) ( )
( ) ( ) ( )
( )
( ) ( )
1
1
, ,
T
x
T
x
T
x
M J M J
V J V M J J
G J G
u
÷ ÷
÷ ÷
÷
O = O
O O = O O ÷ O
O = O
X J
X J J
= O
= O+ O
79
4.6 Robot Dynamics Example
Here we summarize the dynamics equations of motion for the twolink planar 2R robot when
each link is modeled as a homogeneous rectangular solid of dimensions L
i
, w
i
, h
i
of mass m
i
.
NewtonEuler recursion with outward kinematics and inertial calculations, followed by inward
kinetics balances yields:
1
2 2
2 2 1 2 2 2 2 2 2 2 1 2 2 2 2 12
2 2 1 2 2
2 2 2
2 1 1 2 2 2 1 2 2 2 2
1 1 2 2 1 2 1 2 2 1 2 2
2 2 1 2 2
2 2
2 4 4 2 2
4 4 2 4
2
zz zz
zz zz zz
m L L c m L m L m L L s m L c
I I g
m L m L m L L c m L
I I m L m L L c I
m L L s
m
t u u u
t u u
u
   
   
= + + + + + +
   
\ . \ .
\ . \ .
   
= + + + + + + + +
 
\ . \ .
 
+ ÷ + ÷

\ .
( )
1 1 1 2 2 12
1 2 2 1 2 2 1 1
2 2
m L c m L c
L L s m L c g u u
 
+ + +

\ .
where
( )
2 2
12
i
zzi i i
m
I L h = + .
These dynamics equations of motion are very complicated – imagine how much worse these
equations will be for a spatial 6axis robot such as the PUMA industrial robot.
X
Y
0
0
L
2
L
1
u
1
2
u
80
State Space Representation
For the planar 2R robot, the dynamics equations of motion from the previous page are expressed
in statespace form below:
( ) ( ) ( ) , M V G t = O O+ O O + O
1
2
t
t
t
¦ ¹
=
´ `
¹ )
1
2
u
u
¦ ¹
O =
´ `
¹ )
( )
( )
( )
( )
2 2 2
2
1 1 2 2 2 1 2 2 2 2
1 2 2 1 2 1 2 2 2
2 2
2 1 2 2 2 2 2 2
2 2
2 2 1 2 2
2 2 1 2 2 1 2
2 2 1 2 2
1
1 1 1
2 1 1
4 4 2 4
2 4 4
2
,
2
2
zz zz zz
zz zz
m L m L m L L c m L
I I m L m L L c I
M
m L L c m L m L
I I
m L L s
m L L s
V
m L L s
m L c m
m L c
G
u u u
u
+ + + + + + +
O =
+ + +
¦ ¹
 
÷ + ÷

¦ ¦
¦\ . ¦
O O =
´ `
 
¦ ¦

¦ ¦
\ . ¹ )
+ +
O =
2 2 12
2 2 12
2
2
L c
g
m L c
¦ ¹
¦ ¦
¦ ¦
´ `
¦ ¦
¦ ¦
¹ )
Configuration Space Representation
For the planar 2R robot, the dynamics equations of motion from the previous page are expressed
in configurationspace form below:
( ) ( ) ( ) ( )
2
M B C G t = O O+ O OO + O O + O
( ) ( ) , M G O O same as above
( )
( )
2 1 2 2
1 2
2 1 2 2
2
2 1
2
2 1 2 2 2
0
0
2
0
2
m L L s
B
m L L s
C
m L L s
u u
u
u
÷ ¦ ¹
O OO =
´ `
¹ )
÷
¦ ¹
O O =
´ `
¹ )
81
Numerical Dynamics Example
For the planar 2R robot whose dynamics equations of motion were presented analytically
above, here we calculate the required torques (i.e. solve the inverse dynamics problem) to provide the
commanded motion at every time step in a resolvedrate control scheme. Identical results are obtained
by both analytical equations and numerical Newton/Euler recursion.
Given:
L
1
= 1.0 m
L
2
= 0.5 m
Both links are solid steel with mass density p = 7806
3
kg m and both have width and thickness
dimensions w = t = 5 cm. The revolute joints are assumed to be perfect, connecting the links at their
very edges (not physically possible, just a simplified model).
The initial robot configuration is:
1
2
10
90
u
u
¦ ¹ ¦ ¹
O = =
´ ` ´ `
¹ ) ¹ )
The constant commanded Cartesian velocity is:
0 0
0
0
0.5
x
X
y
¦ ¹ ¦ ¹
= =
´ ` ´ `
¹ ) ¹ )
(m/s)
The dynamics equations require the relative joint accelerations u
; how do we find these? (See
the last page of the Acceleration Kinematics notes in this 429/529 Supplement:
( )
1
J X J u u
÷
= ÷
. In
this example, 0 X =
.)
Simulate motion for 1 sec, with a control time step of 0.01 sec. The plots for various variables of
interest (joint angles, joint rates, joint accelerations, joint torques, and Cartesian pose) for this problem
are given on the following page.
In the last plot, note that the robot travels 0.5 m in the Y
0
direction in 1 sec (which agrees with
the constant commanded rate of 0.5 m/s). The robot does not move in X; o must move to compensate
for the pure Y motion, but we cannot control o independently with only twodof. The first three plots are
kinematics terms related to the resolvedrate control scheme; they are inputs to the inverse dynamics
problem. The joint torques are calculated by the numerical recursive NewtonEuler inverse dynamics
algorithm. These are the joint torques necessary to move this robot’s inertia in the commanded manner.
Notice from the joint angles, joint rates, joint accelerations, and joint torques plots that the robot is
approaching the u
2
= 0 singularity towards the end of this simulated motion.
82
Numerical Dynamics Example: Plots
Dynamics Results Ignoring Gravity Dynamics Results Including Gravity
0 0.2 0.4 0.6 0.8 1
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Cartesian Pose
time (sec)
x
(
r
)
,
y
(
g
)
,
o
(
b
)
(
m
,
r
a
d
)
0 0.2 0.4 0.6 0.8 1
0
10
20
30
40
50
60
70
80
90
Joint Angles
time (sec)
u
1
(
r
)
,
u
2
(
g
)
(
d
e
g
)
0 0.2 0.4 0.6 0.8 1
3
2.5
2
1.5
1
0.5
0
0.5
1
1.5
Joint Rates
time (sec)
j
o
i
n
t
1
(
r
)
,
j
o
i
n
t
2
(
g
)
(
r
a
d
/
s
)
0 0.2 0.4 0.6 0.8 1
20
15
10
5
0
5
10
Joint Accelerations
time (sec)
j
o
i
n
t
1
(
r
)
,
j
o
i
n
t
2
(
g
)
(
r
a
d
/
s
2
)
0 0.2 0.4 0.6 0.8 1
0
10
20
30
40
50
60
70
80
Joint Torques
time (sec)
t
(
1

r
e
d
;
2

g
r
e
e
n
)
(
N
m
)
0 0.2 0.4 0.6 0.8 1
50
0
50
100
150
200
250
Joint Torques
time (sec)
t
(
1

r
e
d
;
2

g
r
e
e
n
)
(
N
m
)
83
Chapter 5. Robot Control Architectures
With the robotic kinematics and dynamics we have learned and simulated, we can simulate robot
control using various popular robot control architectures.
5.1 Inverse Pose Control Architecture
5.2 Inverse Velocity (ResolvedRate) Control Architecture
84
5.3 Inverse Dynamics Control Architecture
The inverse dynamics method is also called the computed torque control method, and it is also
called the feedback linearization control method.
In Chapter 4 we learned that the robot dynamics equations of motion are highly coupled and non
linear. Here they are in matrix/vector form:
( ) ( ) ( ) ( ) , , M V F G t = O O+ O O + O O + O
t vector of applied joint torques
( ) M O
N N × mass matrix; symmetric and positive definite
( )
, V O O
1 N × vector of Coriolis and centrifugal terms
( )
, F O O
1 N × vector of friction torques
( ) G O 1 N × vector of gravity terms
O O O
, , joint angles, rates, and accelerations
Computed Torque Control Architecture
The computedtorque control method is based on canceling the dynamics effects by using the
inverse dynamics method, in order to linearize the robot system for standard controller methods, such as
PID control.
85
5.4 NASA Langley Telerobotics ResolvedRate Control Architecture
86
5.5 NaturallyTransitioning RatetoForce Controller Architecture
Resolvedrate control automatically (naturally) changes to force control when the robot end
effector enters into contact with the environment. A force/torque (F/T) sensor and force/moment
accommodation (FMA) algorithm is required to accomplish this. After contact, the constant velocity
command becomes a constant force command. In addition, if there is a human teleoperator with haptic
interface, the force/moment that the human applies to the interface becomes proportional to the
force/moment that the robot exerts on the environment at contact. This works and feels great in real
world applications, providing telepresence.
This control architecture was implemented by Dr. Bob and teams at NASA Langley Research Center
and WrightPatterson AFB6F
4
.
4
R.L. Williams II, J.M. Henry, M.A. Murphy, and D.W. Repperger, 1999, NaturallyTransitioning RatetoForce Control in Free and
Constrained Motion, ASME Journal of Dynamic Systems, Measurement, and Control, Trans. ASME, 121(3): 425432.
Resolved
Rate
F/T
Sensor X
K
O X O
F
F
F
X
X
M A
M
C
C C
+

Manipulator Master
T
J
t
M
(FMA)
(Force Reflection  Optional)
87
5.6 Single Joint Control
Every controller architecture we’ve considered requires linearized independent (but
simultaneous) single joint angle control, presented in this section.
Manipulator dynamics is extremely complicated considering the number of terms. We can easily
use symbolic MATLAB to crank out the terms but they go on for pages and their structure is difficult to
understand. In this case, numerical NewtonEuler recursion a’la Craig (2005, Chapter 6) is useful to get
around the need for analytical expressions.
How is manipulator dynamics done in industry for control purposes? Answer: in almost all cases
it is ignored. How is this possible? (Large gear ratios tend to decouple the dynamic coupling in motion
of one link on its neighbors – we will see this in modeling soon.) The vast majority of multiaxis
industrial robots are controlled via linearized, independent single joint control. So, robot control in
industry is generally accomplished by using N independent (but simultaneous) linearized joint
controllers, where N is the number of joint space freedoms of the robot.
We now will briefly discuss single joint control. We will focus on a common system, the
armaturecontrolled DC servomotor driving a single robot joint / gear train / link combination, shown
below. This requires dynamics, but it is not coupled nor nonlinear.
OpenLoop System Diagram
V(t) armature voltage t
M
(t) generated motor torque t
L
(t) load torque
L armature inductance u
M
(t) motor shaft angle u
L
(t) load shaft angle
R armature resistance e
M
(t) motor shaft velocity e
L
(t) load shaft velocity
i
A
(t) armature current J
M
lumped motor inertia J
L
(t) total load inertia
v
B
(t) back emf voltage C
M
motor viscous damping C
L
load viscous damping
n gear ratio
88
ClosedLoop Feedback Highlevel Control Diagram
Highlevel Computer Control Diagram
Simplifying Assumptions
 rigid motor, load shafts
 n >> 1 (the large gear ratio n reduces speed and increases torque)
Realworld vs. model characteristics
Real World Our Model (simplified)
nonlinear
linearized
multipleinput, multiple output (MIMO)
singleinput, single output (SISO)
coupled
decoupled
timevarying load inertia treat as a disturbance; the large gear ratio
diminishes this problem
hysteresis, backlash, stiction, Coulomb friction
ignore
discrete and continuous
continuous for control design
89
Single Joint/Link System Modeling
We must derive all linear ordinary differential equations (ODEs) describing the dynamics of an
armaturecontrolled DC servomotor driving a single robot joint / link. This is an electromechanical
system, including a gear train. We must perform modeling, simulation, and control. The system
diagram was shown earlier in this section.
Armature Circuit Diagram
Electrical Model Kirchoff’s voltage law
(1)
Electromechanical coupling
 The generated motor torque is proportional to the armature current
(2)
 The backemf voltage is proportional to the motor shaft angular velocity
(3)
(K
T
= K
B
can be shown)
90
Mechanical Model
Euler’s Equation (the rotational form of Newton’s Second Law):
Freebody diagrams
Load
(4)
Motor
(5)
where ( )
LR
t t is the load torque reflected to the motor shaft.
Gear ratio n
(6)
Substituting (6) into (5) yields (7):
(7)
91
Reflect load inertia and damping to motor shaft
Substitute into (4):
(8)
Substitute (8) into (7) to eliminate t
L
:
(9)
Combine terms:
(10)
define:
2
L
E M
J
J J
n
= + effective inertia, total reflected to motor shaft
2
L
E M
C
C C
n
= + effective damping, total reflected to motor shaft
92
Final Mechanical Model (ODE):
(11)
This is a secondorder ODE in u
M
. Can also be written as firstorder ODE in e
M
:
(12)
For common industrial robots, n >> 1 so
2
1
n
is small; therefore we can choose a nominal J
L
and
assume it is constant without much error in control.
For example, the NASA 8axis ARMII robot downstairs has n = 200 with harmonic gearing.
This is why we can ignore the timevarying robot load inertia and design decoupled independent linear
joint controllers. The gearreduced load inertia variation is then treated as a disturbance to the single
joint controller.
An alternative is to reflect the motor inertia and damping to the load shaft as shown below, but
we will use the first case above.
( ) ( )
2 2
L M L L M L L
J n J C n C u u t + + + =
93
OpenLoop Block Diagram
Use Laplace transforms on different ODE pieces of the system model and then connect the
components together in blocks connected by arrows representing variables. A transfer function goes
inside each block representing component dynamics. This block diagram and MATLAB/Simulink
implementation including a disturbance torque are give below:
94
ClosedLoop Feedback Control Diagram
Assuming perfect encoder sensor for angular position feedback, we include block for the single
joint PID controller. The MATLAB/Simulink implementation is:
95
PID Controller Design
PID Controller characteristics
PID stands for:
Controller transfer function:
Each term does this:
Use PID w/ approximate derivate in Simulink – numerical differentiation can lead to errors and
numerical instability and thus it is to be avoided if possible.
Trial and Error PID Design
Start with the P gain value – start low and increase P to get the desired time nature of response.
Then add the D gain value term for stability. Add the I gain value to reduce steadystate error. Always
use Simulink to simulate control and dynamics response of the single joint/link system for different PID
choices; compare various cases and select a suitable controller in simulation.
A better approach is to perform analytical design for PID controllers using classical control
theory, such as in ME 401.
Controller Performance Criteria
 Stability
 Rise Time
 Peak Time
 Percent Overshoot
 Settling Time
 Steadystate error
These performance criteria provide a rational basis for choosing a suitable controller, at least in
theory and simulation. The real world always provides some additional challenges (noise, modeling
errors, nonlinearities, calibration, backlash, etc.).
96
Include Gravity as a Disturbance Torque
Unmodeled, unexpected disturbances are modeled as disturbances in control systems. It is
convenient to do so at the motor torque level in the block diagram:
Gravity is known and expected. However, in single joint control we can include the effect of gravity as
a disturbance. First let us model the gravity effect for a single joint. Lump all outboard links, joints, and
motors as a single rigid body:
 test with original PID gains
 redesign new PID gains with gravity disturbance considered
97
Chapter 6. Parallel Robots
6.1 Introduction
Serial vs. Parallel robot – demonstrate with wooden model kit.
Example parallel manipulators: Stewart Platform, Variable Geometry Trusses, NASA Hyperredundant
Serpentine Arm, Backhoe.
Serial Robot: the joints and links extend from
the base in a serial fashion to the endeffector. The
overall structure is cantilevered which is not good
for resisting endeffector loads. All joints must be
active.
Parallel Robot: the joints and links are arranged
in parallel from the base to the endeffector. There
are multiple load paths from the endeffector to the
base. There is a combination of active and passive
joints.
Comparison of parallel vs. serial robots
Advantages of parallel robots
 Higher loads
 Higher accelerations
 Lower mass
 Higher stiffness
 Better accuracy, repeatability
 Groundmounted actuators
 Directdrive
 Open structure for cabling
Disadvantages of parallel robots
 SMALLER WORKSPACE
 Generally analysis is more difficult
Mobility – the Grubler (planar) and Kutzbach (spatial) mobility equations (see Section 1.3 in this
429/529 Supplement) for calculating overall manipulator dof are very important for parallel
manipulators to ensure the correct number and type of active and passive joints are used to obtained the
desired dof. There are infinite variations and possible designs for parallel robots.
98
Kinematics problems we will consider for parallel robots
 forward and inverse pose solutions
 workspace
 forward and inverse velocity solutions
Both inverse pose and rate problems have been presented for all possible planar parallel robots with
3 identical 3dof legs8F
5
.
There exists an interesting duality between serial and parallel robots: for parallel robots, the inverse
pose and velocity solutions are generally straightforward and the forward pose and velocity problems
are harder. We know the opposite case is true for serial robots in general.
We could also do parallel robot dynamics but this is of less concern for a parallel robot than for a
serial robot due to the parallel nature and its inherent advantages.
Serial robot kinematics – DH parameters, homogeneous transformation matrices
Parallel robot kinematics – Vector loopclosure equations; phasors; Euler’s identity
Phasors – a powerful ReIm manner to represent a vector.
Euler’s Identity:
Phasors enable compact notation and are very convenient for time derivatives (velocity analysis).
Example Vector loopclosure equation for the fourbar mechanism:
5
R.L. Williams II and B.H. Shelley, Inverse Kinematics for Planar Parallel Manipulators, CD Proceedings of the 1997 ASME Design
Technical Conferences, 23
rd
Design Automation Conference, DETC97/DAC3851, Sacramento, CA, September 1417, 1997.
99
6.2 Planar 3RPR Manipulator Pose Kinematics
Planar 3RPR Manipulator Diagram
Inverse Pose Kinematics (for pose control)
Given:
Find:
Vector loopclosure equations
The vector loopclosure equations are rewritten below:
The inverse pose solution is straightforward:
We can also calculate the intermediate passive joint variables
1 2 3
, , u u u , for use in velocity and dynamics
analyses.
100
3RPR parallel robot Forward Pose Kinematics (for simulation and sensorbased control)
Given: Find:
This is a coupled, nonlinear problem to solve – it is difficult to solve and multiple solutions generally
exist, like the Inverse Pose Kinematics problem for serial robots.
We use the same vector loopclosure equations:
VLCE details expanded for one RPR leg:
Considering all three legs (the problem is coupled), these represent six scalar equations in the six
unknowns
1 2 3
, , , , , x y o u u u .
We will use the NewtonRaphson numerical iteration technique to solve this Forward Pose
Kinematics Problem. We can directly solve the above six equations for the six unknowns. However, we
don’t always need the intermediate variables
1 2 3
, , u u u (also, we can calculate these angles later, using
Inverse Pose Kinematics, if required for velocity, dynamics, or computer simulation). So, to simplify
the Forward Pose Kinematics Problem, square and add each XY equation pair (for all three legs) to
eliminate the intermediate variables
1 2 3
, , u u u . Then we will have three equations to solve for the three
primary unknowns , , x y o :
Bring
2
i
L to the other side and solve via the NewtonRaphson iterative numerical method.
101
6.3 NewtonRaphson Method
The NewtonRaphson Method involved numerical iteration to solve coupled sets of n nonlinear
equations (algebraic/transcendental – not ODEs) in n unknowns. It requires a good initial guess of the
solution to get started and it only yields one of the possible multiple solutions. The NewtonRaphson
method is an extension of Newton’s single function/single variable rootfinding technique to n functions
and n variables. The following is the form of the given functions to solve:
( ) = F X 0
where the n functions are: ( ) ( ) ( ) ( ) { }
1 2
T
n
F F F = F X X X X
and the n variables are: { }
1 2
T
n
x x x = X
Perform a Taylor Series Expansion of F about X:
( ) ( ) ( )
2
1
n
i
i i j
j j
F
F F x O
x
o o o
=
c
+ = + +
c
¯
X X X X 1, 2, , i n =
Where
( )
i
NR NR
j
F
J J
x
c
= =
c
X is the NewtonRaphson Jacobian Matrix, a multidimensional form of the
derivative and a function of X. If o X is small, the higherorder terms
( )
2
O oX from the expansion are
negligible. For solution, we require:
( ) 0
i
F o + = X X 1, 2, , i n =
Now with
( )
2
0 O o ÷ X we have:
( ) ( ) ( )
1
0
n
i
i i j i NR
j j
F
F F x F J
x
o o o
=
c
+ = + = + =
c
¯
X X X X X 1, 2, , i n =
So to calculate the required correction factor o X at each solution iteration step, we must solve
( ) 0
NR
J o + = F X X :
( )
1
NR
J o
÷
= ÷ X F X
Actually, Gaussian elimination on ( )
NR
J o = ÷ X F X is preferable numerically, since it is more efficient
and more robust than matrix inversion.
102
NewtonRaphson Method Algorithm Summary
0) Establish the functions and variables to solve for: ( ) = F X 0
1) Make an initial guess to the solution:
0
X
2) Solve ( ) ( )
NR k k k
J o = ÷ X X F X for
k
o X , where k is the iteration counter
3) Update the current best guess for the solution:
1 k k k
o
+
= + X X X
4) Iterate until
k
o c < X , where we use the Euclidean norm and c is a small, userdefined
solution tolerance. Also halt the iteration if the number of steps becomes too high (which
means the solution is diverging). Generally less than 10 iterations is required for even very
tight solution tolerances.
If the initial guess to the solution
0
X is sufficiently close to an actual solution, the Newton
Raphson technique guarantees quadratic convergence.
Now, for manipulator forward pose problems, the NewtonRaphson technique requires a good
initial guess to ensure convergence and yields only one of the multiple solutions. However, this does
not present any difficulty since the existing known pose configuration makes an excellent initial guess
for the next solution step (if the control rate is high, many cycles per second, the robot cannot move too
far from this known initial guess). Also, except in the case of singularities where the multiple solution
branches converge, the one resulting solution is generally the one you want, closest to the initial guess,
most likely the actual configuration of the real robot.
There is a very interesting and beautiful relationship between numerical pose solution and the
velocity problem for parallel robots: the NewtonRaphson Jacobian Matrix is nearly identical to the
Inverse Velocity Jacobian Matrix for parallel robots. (In the planar case it is identical, in the spatial it is
related very closely.) This reduces computation if you need both forward pose computation and inverse
velocitybased resolvedrate control.
3RPR manipulator forward pose kinematics solution
Use the NewtonRaphson numerical iteration method for solution, with:
{ }
T
x y o = X
The three coupled, nonlinear, transcendental functions F
i
are the squared and added equations for
each RPR leg, with
2
i
L brought to the other side to equate the functions to zero.
103
Derive the required NewtonRaphson Jacobian Matrix:
( )
i
NR
j
F
J
x
c
=
c
X
i
F
x
c
=
c
1, 2, 3 i =
i
F
y
c
=
c
1, 2, 3 i =
i
F
o
c
=
c
1, 2, 3 i =
where:
x B
i
y
A
A
A
¦ ¹
=
´ `
¹ )
and
x H
i
y
P
C
P
¦ ¹
=
´ `
¹ )
The index i was dropped for convenience; use 1, 2, 3 i = in the above definitions in the proper
places in the overall NewtonRaphson Jacobian Matrix.
After the forward pose problem is solved at each motion step, we can calculate the intermediate
variables
1 2 3
, , u u u as in inverse pose kinematics solution above.
Alternate 3RPR manipulator forward pose NewtonRaphson solution
As we said, we could have solved the original six equations in the six unknowns including the
three intermediate variables
1 2 3
, , u u u . The functions are simpler (no squaring and adding) but the size of
the problem is doubled to 1, 2, , 6 i = . Below is the required Jacobian Matrix for this case, where the
odd functions are the X equations and the even functions are the Y equations; also the variable ordering
is { }
1 2 3
T
x y o u u u = X .
104
( )
1 1 1 1
1 1 1 1
2 2 2 2
2 2 2 2
3 3 3 3
3 3 3 3
1 0 0 0
0 1 0 0
1 0 0 0
0 1 0 0
1 0 0 0
0 1 0 0
x y
x y
x y
NR
x y
x y
x y
P s P c L s
P c P s L c
P s P c L s
J
P c P s L c
P s P c L s
P c P s L c
o o u
o o u
o o u
o o u
o o u
o o u
÷ ÷
÷ ÷
÷ ÷
=
÷ ÷
÷ ÷
÷ ÷
X
Alternate analytical 3RPR manipulator forward pose solution
Figure:
Branch
1 1 2 2
ACC A is a 4bar mechanism with input angle u
1
and output angle u
2
. With given
lengths L
1
and L
2
, this fourbar mechanism has 1dof, and it can trace out a coupler curve for point C
3
in
the plane. In general, this coupler curve is a tricircular sextic. The forward pose kinematics solution
may be found by intersecting leg
3 3
AC (a circle of given radius L
3
, centered at A
3
) with the coupler
curve. There are at most six intersections between a circle and tricircular sextic and so there may be up
to six real multiple solutions to the 3RPR parallel robot forward pose kinematics problem. There are
always six solutions, but 0, 2, 4, or 6 of these will be real, depending on the commanded configuration
and robot geometry.
105
6.4 Parallel Manipulator Workspace
Since reduced workspace of parallel robots (when compared to serial robots) is their chief
disadvantage, it becomes very important to determine the workspace of parallel robots and maximize it
through design.
There are two workspaces to consider (the same as for serial robots in Section 2.10 of this
429/529 Supplement):
1) reachable workspace is the volume in 3D space reachable by the endeffector in any
orientation
2) dexterous workspace is a subset of the reachable workspace because it is the volume in 3D
space reachable by the endeffector in all orientations.
For parallel robots, the dexterous workspace is almost always null since the rotation capability
is never full for all three Euler angles; therefore we usually define a reduced dexterous workspace
wherein all Euler angles can reach 30 ±
or some other userdefinable range.
3RPR Example
For planar parallel robots we can generally find the reachable workspace using a geometric
method, figuring out what the endeffector can reach guided by each leg on its own, and then
intersecting the results:
For determination of the dexterous workspace, it is most convenient to numerically or geometrically
generate in MATLAB the reachable workspace for different o values (endeffector orientation) within
the desired limits. Then stack these up and intersect them to find the dexterous workspace, defined for
a reduced desired rotational range
LIMIT
o ± .
106
6.5 Planar 3RPR Manipulator Velocity Kinematics
First the pose configuration variables must all be known. Then we can define and solve two
problems:
Forward velocity kinematics (for simulation)
Given: Find:
Inverse velocity kinematics (for resolvedrate control)
Given: Find:
In both cases we will have intermediate passive joint rate unknowns
1 2 3
, , u u u
involved. Both
velocity kinematics problems use the same rate equations; we will derive these from looking at the three
single RPR legs separately (meeting at the endeffector). Figure:
Vector loopclosure equation using phasors:
XY component equations:
Angle equation:
The velocity equations for one RPR leg are obtained from the first time derivatives of the XY and angle
equations:
107
These equations are written in matrixvector form to yield the RPR leg Jacobian matrix:
Written in compact notation:
{ }
T
z
X x y e =
is the same for all three legs (the Cartesian endeffector rates).
i
p includes one active
and two passive joint rates for each of the three RPR legs, 1, 2, 3 i = . Let us now find the overall robot
Jacobian matrix, using only active rates and ignoring the passive rates. Invert the leg Jacobian matrix:
Invert symbolically:
Pull out only the active joint row of this result:
Assemble all three active joint rows into the overall robot Jacobian matrix:
Note: this above equation solves the Inverse Velocity Problem. No inversion is required and so the
Inverse Velocity problem is never singular. Here it is expressed in compact notation:
108
Forward Velocity Problem
Compact notation (solve numerically from the Inverse Velocity Solution):
Ironically, it is the Forward Velocity Problem that is subject to singularities for parallel robots.

i
for use in velocity equations
Figure:
1 1 1
2 2 2
3 3
120
270
u  o ¸
u  o ¸
u  o
+ = +
+ = + +
+ = +
The second two relationships assume
1 2 3
30 ¸ ¸ ¸ = = =
(i.e. there is an equilateral endeffector triangle).
109
Example Symbolic MATLAB Code
Here is the MATLAB code that was used to symbolically invert the RPR leg Jacobian matrix as
presented earlier. Symbolic computing has a lot of power in robot kinematics, dynamics, and control.
%
% Symbolic MATLAB code to invert the RPR leg Jacobian
%
clear; clc;
% Declare symbolic variables
L = sym('L');
LH = sym('LH');
t1 = sym('t1');
b1 = sym('b1');
c1 = sym('cos(t1)');
s1 = sym('sin(t1)');
cp = sym('cos(t1+b1)');
sp = sym('sin(t1+b1)');
% Jacobian elements
j11 = L*s1LH*sp;
j21 = L*c1+LH*cp;
j13 = LH*sp;
j23 = LH*cp;
% Jacobian matrix
J = sym([j11 c1 j13;j21 s1 j23;1 0 1]);
% Invert
Jinv = inv(J);
% Simplify
Jinvsimp = simple(Jinv);
% Check
Ident1 = simple(Jinvsimp * J);
Ident2 = simple(J * Jinvsimp);
2
EE/ME 429/529 Supplement Table of Contents
CHAPTER 1. INTRODUCTION ........................................................................................................................................... 3 . 1.3 MOBILITY .......................................................................................................................................................................... 3 1.5 MATRICES ........................................................................................................................................................................ 8 . CHAPTER 2. SERIAL MANIPULATOR KINEMATICS ........................................................................................................... 16 2.4 DENAVITHARTENBERG (DH) PARAMETERS – ADDITIONAL EXAMPLES ...................................................................... 16 2.5 FORWARD POSE KINEMATICS ....................................................................................................................................... 25 2.5.1 3D MATLAB Graphics for Animation ................................................................................................................. 25 2.5.2 Spatial 6dof 6R PUMA Robot FPK Example .................................................................................................. 27 2.6 INVERSE POSE KINEMATICS .......................................................................................................................................... 31 2.8 VELOCITY KINEMATICS .................................................................................................................................................. 38 2.8.1 Velocity Introduction ............................................................................................................................................ 38 2.8.2 Velocity Equations Derivations........................................................................................................................... 40 2.8.3 Jacobian Matrices ............................................................................................................................................... 42 2.8.8 Velocity Kinematics Examples ........................................................................................................................... 46 2.8.9 Jacobian Matrix Expressed in Another Frame ................................................................................................. 47 2.8.10 Cartesian Transformation of Velocities and Wrenches ................................................................................ 48 2.9 ACCELERATION KINEMATICS ........................................................................................................................................ 52 2.10 ROBOT WORKSPACE ................................................................................................................................................... 57 CHAPTER 3. KINEMATICALLY‐REDUNDANT ROBOTS ...................................................................................................... 64 3.2 INVERSE VELOCITY (RESOLVEDRATE) SOLUTION ...................................................................................................... 64 3.2.1 Pseudoinversebased .......................................................................................................................................... 64 3.2.3 Klein and Huang’s Algorithm .............................................................................................................................. 64 3.2.4 Singular Value Decomposition ........................................................................................................................... 65 3.2.5 Generalized Inverses ........................................................................................................................................... 67 CHAPTER 4. MANIPULATOR DYNAMICS ........................................................................................................................ 68 4.1 4.2 4.3 4.4 4.5 4.6 MASS DISTRIBUTION: INERTIA TENSOR ....................................................................................................................... 69 NEWTONEULER RECURSIVE ALGORITHM .................................................................................................................... 70 LAGRANGEEULER ENERGY METHOD .......................................................................................................................... 72 SIMPLE DYNAMICS EXAMPLE ........................................................................................................................................ 73 STRUCTURE OF MANIPULATOR DYNAMICS EQUATIONS ............................................................................................... 78 ROBOT DYNAMICS EXAMPLE ......................................................................................................................................... 79
CHAPTER 5. ROBOT CONTROL ARCHITECTURES ............................................................................................................. 83 5.1 5.2 5.3 5.4 5.5 5.6 INVERSE POSE CONTROL ARCHITECTURE .................................................................................................................... 83 INVERSE VELOCITY (RESOLVEDRATE) CONTROL ARCHITECTURE ............................................................................. 83 INVERSE DYNAMICS CONTROL ARCHITECTURE ............................................................................................................ 84 NASA LANGLEY TELEROBOTICS RESOLVEDRATE CONTROL ARCHITECTURE ......................................................... 85 NATURALLYTRANSITIONING RATETOFORCE CONTROLLER ARCHITECTURE .......................................................... 86 SINGLE JOINT CONTROL ................................................................................................................................................ 87
CHAPTER 6. PARALLEL ROBOTS .................................................................................................................................... 97 6.1 6.2 6.3 6.4 6.5 INTRODUCTION ............................................................................................................................................................... 97 PLANAR 3RPR MANIPULATOR POSE KINEMATICS ..................................................................................................... 99 NEWTONRAPHSON METHOD ...................................................................................................................................... 101 PARALLEL MANIPULATOR WORKSPACE ..................................................................................................................... 105 PLANAR 3RPR MANIPULATOR VELOCITY KINEMATICS ............................................................................................ 106
3
Chapter 1. Introduction
1.3 Mobility
Mobility is the number of degreesoffreedom (dof) which a robot or mechanism possesses. Structures have zero (or negative) dof, mechanisms have one dof, and robots have greater than one dof. Degreesoffreedom (dof) – the minimum number of independent parameters required to fully specify the location of a device. For planar or spatial serial robots, to find the dof, one may simply count the number of single degreeoffreedom joints. This is also equal to the number of independent motors to control the robot. More formally, we can also use Kutzbach’s mobility equations for planar or spatial robots. Kutzbach's Planar Mobility Equation
M 3 N 1 2 J1 1J 2
Where: M is the mobility N is the total number of links, including ground J1 is the number of onedof joints J2 is the number of twodof joints J1 – onedof joints: J2 – twodof joints: revolute, prismatic cam joint, gear joint, slottedpin joint
revolute joint R – pin joint, turning pair
prismatic joint P – sliding pair
In general J1 R and P joints are used much more extensively in practical robots than J2 joints. In Kutzbach's planar mobility equation, links give degrees of freedom and joints constrain or remove degrees of freedom. Each moving link has 3dof in the plane – but the ground link is counted and hence must be subtracted in the equation. Onedof joints thus remove 2dof in planar motion and twodof joints thus remove 1dof in planar motion.
but we can still count the required motors. connected by n active 1dof joints: M 3 n 1 1 2n 1 0 3n 2n n dof This agrees with the previous statement. that we can just count the number of active joints to find the mobility.4 Planar robot mobility examples: 2R serial robot 3R serial robot 4R serial robot RRPR serial robot fivebar parallel robot 3RRR parallel robot M 3 5 1 2(5) 1(0) 2dof M 3 8 1 2(9) 1(0) 3dof A general planar nlink serial robot has n+1 links (including the fixed ground link). For parallel robots this does not apply. .
5 Kutzbach's Spatial Mobility Equation M 6 N 1 5 J1 4 J 2 3J 3 2 J 4 1J 5 Where: M is the mobility N is the total number of links. Onedof joints remove 5dof. Each moving link has 6dof in 3D – but the ground link is counted and hence must be subtracted in the equation. prismatic. . cylindrical. gear joint spherical spherical in a slot spatial cam universal joint U spherical joint S In general J1 R and P. twodof joints remove 4dof. and so on. links give degrees of freedom and joints constrain or remove degrees of freedom. In Kutzbach's spatial mobility equation. in spatial motion. threedof joints remove 3dof. and J3 S joints are used much more extensively in practical robots than the other named joints. J2 U. screw universal. including ground J1 is the number of onedof joints J2 is the number of twodof joints J3 is the number of threedof joints J4 is the number of fourdof joints J5 is the number of fivedof joints J1 – onedof joints: J2 – twodof joints: J3 – threedof joints: J4 – fourdof joints: J5 – fivedof joints: revolute.
6 Spatial robot mobility examples: 5R serial robot 6R PUMA serial robot 7R serial FTS robot RRPR Adept SCARA robot 8R serial ARMII robot Stewart platform .
1*J2.ohiou.edu/~williar4/PDF/MechanismAtlas. please see Dr. Bob’s atlas of structures. Result: mob = 1 % for 4bar and slidercrank mechanisms .0).7 Human arm A general spatial nlink serial robot has n+1 links (including the fixed ground link).4. Usage: mob = dof(4.2*J1 . mechanisms. connected by n active 1dof joints: M 6 n 1 1 5n 4 0 3 0 2 0 1 0 6n 5n n dof This agrees with the previous statement. For some more unsolved mobility examples. For parallel robots this does not apply.pdf MATLAB function to calculate mobility % Function for calculating planar mobility % Dr. and robots: oak. EE/ME 429/529 function M = dof(N. that we can just count the number of active joints to find the mobility. Bob.J1.J2) M = 3*(N1) . but we can still count the required motors.cats.
velocity. and dynamics. where m is the number of rows and n in the number of columns.8 1.5 Matrices Matrix: an m x n array of numbers. In robotics matrices are used a lot. a11 a12 a1n a 21 a22 a2 n A am1 am 2 amn Matrices can be used to simplify and standardize the solution of n linear equations in n unknowns (where m = n). Special Matrices square matrix (m = n = 3) a11 a12 A a21 a22 a31 a32 a11 0 A 0 a22 0 0 1 0 0 I 3 0 1 0 0 0 1 a11 a21 A a12 a22 a13 a23 T a13 a23 a33 0 0 a33 diagonal matrix identity matrix transpose matrix a31 a32 a33 a12 a22 a23 (switch rows and columns) symmetric matrix a11 A A a12 a13 T a13 a23 a33 column vector (3x1 matrix) x1 X x2 x3 row vector (1x3 matrix) X T x1 x2 x3 . in pose (position and orientation) description. acceleration.
e. The size of the resulting matrix [C] is from the number of rows m of the lefthand matrix and the number of columns n of the righthand matrix: mxn. the number of columns p in the lefthand matrix must equal the number of rows p in the righthand matrix. If this condition is not met. g c ag bh ci h f dg eh fi i (2x1) (2x3)(3x1) a b C d e Example: Note the inner indices (p = 3) must match. the matrix multiplication is undefined and cannot be done. along the rows of the lefthand matrix and down the columns of the righthand matrix (use your index fingers from the left and right hands). Multiplication proceeds by multiplying like terms and adding them. mxn = 2x1. in a matrix multiplication product. A B B A The row and column indices must line up as follows: C A B (mxn) (mxp)( pxn) That is.9 Matrix Addition add like terms and keep the results in place a b e c d g f a e b f h c g d h Matrix Multiplication with Scalar multiply each term and keep the results in place a b ka kb k c d kc kd Matrix Multiplication C A B In general. as stated above. i. and the dimension of the result is dictated by the outer indices. .
10
Matrix Multiplication Examples
1 2 3 A 4 5 6
7 8 B 9 8 7 6
C A B
7 1 2 3 9 4 5 6 7 7 18 21 28 45 42 8 8 6 8 16 18 46 42 32 40 36 115 108
(2x 2) (2x3)(3x 2)
D B A
7 8 1 2 3 9 8 4 5 6 7 6 7 32 14 40 21 48 39 54 69 9 32 18 40 27 48 41 58 75 7 24 14 30 21 36 31 44 57
(3x3) (3x 2)(2x3)
11
Matrix Inversion Since we cannot divide by a matrix, we multiply by the matrix inverse instead. C A B , solve for [B]: Given
C A B
A C A A B I B B
1 1
B A
1
C
Matrix [A] must be square (m = n) to invert.
A A
1
A
1
A I
where [I] is the identity matrix, the matrix 1 (ones on the diagonal and zeros everywhere else). To calculate the matrix inverse:
A
where: A
1
adjoint( A)
A
determinant of [A]
adjoint( A) cofactor( A)
cofactor(A) minor
T
aij ( 1)i j M ij
minor Mij is the determinant of the submatrix with row i and column j removed.
For another example, given C A B , solve for [A]
C A B
C B
1
A B B A I A
1
A C B
1
In general the order of matrix multiplication and inversion is crucial and cannot be changed.
12
System of Linear Equations We can solve n linear equations in n unknowns with the help of a matrix. For n = 3:
a11 x1 a12 x2 a13 x3 b1 a21 x1 a22 x2 a23 x3 b2 a31 x1 a32 x2 a33 x3 b3
Where aij are the nine known numerical equation coefficients, xi are the three unknowns, and bi are the three known righthandside terms. Using matrix multiplication backwards, this is written as A x b :
a11 a 21 a31
a12 a22 a32
a13 x1 b1 a23 x2 b2 a33 x3 b3
where:
a11 a12 A a21 a22 a31 a32 x1 x x2 x 3 b1 b b2 b 3
a13 a23 a33
is the matrix of known numerical coefficients;
is the vector of unknowns to be solved; and
is the vector of known numerical righthandside terms.
There is a unique solution x A b only if [A] has full rank. If not, A 0 (the determinant of coefficient matrix [A] is zero) and the inverse of matrix [A] is undefined (since it would require dividing by zero; in this case the rank is not full, it is less than 3, which means not all rows/columns of [A] are linearly independent). Actually, Gaussian Elimination is more robust and more computationally efficient than matrix inversion to solve the problem A x b for {x}.
1
x1 2 x2 5 6 x1 4 x2 14 1 2 x1 5 6 4 x 14 2 A 1 2 6 4 1 x x1 x2 b 5 14 x A b A 1 4 2 6 8 The determinant of [A] is nonzero so there is a unique solution. 4 2 1/ 2 1/ 4 6 1 3 / 4 1/ 8 A 1 1 A check: A A A 1 1 A I 2 1 0 0 1 x1 1/ 2 1/ 4 5 1 x2 3 / 4 1/ 8 14 2 answer. Check this solution: substitute the answer {x} into the original equations A x b and make sure the required original {b} results: 1 2 1 1(1) 2(2) 5 6 4 2 6(1) 4(2) 14 checks out .13 Matrix Example – solve linear equations Solution of 2x2 coupled linear equations.
Add = A3 + A4 k = 10.14 Same Matrix Examples in MATLAB %% Matrices. A7 = A5*A6 A8 = A6*A5 A9 b dA9 invA9 x x1 x2 Check xG who whos = = = = = = = = = [1 2.matrix examples % Dr.m . Gaussian Elimination is more efficient (more on this in the dynamics notes later) and more robust numerically.14]. A9*x A9\b % matrixscalar multiplication % matrix transpose (swap rows. Gaussian elimination implementation is given in the third to the last line of the above mfile (with the backslash). [5. % 3x3 diagonal matrix % 3x3 identity matrix % matrix addition A1 = diag([1 2 3]) A2 = eye(3) A3 = [1 2.cols) % define two matrices % matrixmatrix multiplication % % % % % % matrix for linear equations solution define RHS vector calculate determinant of A calculate the inverse of A solve linear equations extract answers % check answer – should be b % Gaussian elimination is more efficient % display the usercreated variables % usercreated variables with dimensions The first solution of the linear equations above uses the matrix inverse – actually. . clc. x(2). Bob. to solve linear equations. MultSca = k*A3 Trans = A4' A5 = [1 2 3. ME 301 %clear.4 5 6].9 8. A4 = [5 6. det(A9) inv(A9) invA9*b x(1).3 4].6 4]. A6 = [7 8.7 8].7 6].
1250 Check = xG = 1 2 .5000 0.2500 0.m A1 = 1 0 0 A2 = 1 0 0 Add = 6 10 0 1 0 0 0 1 0 2 0 0 0 3 8 12 MultSca = 10 20 30 40 Trans = 5 6 A7 = 46 115 A8 = 39 41 31 dA9 = 8 invA9 = 0.15 Output of Matrices.7500 x = 1 2 5 14 7 8 42 108 54 58 44 69 75 57 0.
i 1 2 3 i 1 0 90 90 ai 1 0 0 0 di d1 d2 d3 i 0 90 0 . Serial Manipulator Kinematics 2. 3axis Spatial 3P Cartesian Robot Workspace shown in hatched lines.4 DenavitHartenberg (DH) Parameters – additional examples 1.16 Chapter 2.
17
2. 3axis Spatial PRP Cylindrical Robot (incomplete) Workspace shown in hatched lines.
i
1 2 3
i 1
ai 1
di
i
18
3. 3axis Spatial RRP Spherical Robot (incomplete) Workspace shown in hatched lines.
i
1 2 3
i 1
ai 1
di
i
19
4. 4axis Spatial 4R Articulated Robot (incomplete) Workspace shown in hatched lines.
i
1 2 3 4
i 1
ai 1
di
i
20 5. 4axis Spatial RRPR SCARA Robot (incomplete) Workspace shown in hatched lines. i 1 2 3 4 i 1 ai 1 di i .
6axis Spatial 6R Fanuc S10 Robot (incomplete) i 1 2 3 4 5 6 i 1 ai 1 di i .21 6.
6R Fanuc S10 Robot (continued) – coordinate frames .6.
23 7. 7axis Spatial 7R NASA Flight Telerobotic Servicer (FTS) Robot (incomplete) i 1 2 3 4 5 6 7 i 1 ai 1 di i .
24 8. 8axis Spatial 8R NASA AAI ARMII Robot (incomplete) i 1 2 3 4 5 6 7 8 i 1 ai 1 di i .
xx2 = [0 L1*c1]. Use grid to clarify the resulting image.5 Forward Pose Kinematics 2. for either snapshots or motion animation following Forward Pose Kinematics. zz2 = [L0 L0].zz4.xx2.zz2. Adept 550 SCARA 4dof 3D Robot MATLAB Code: xx1 = [0 0].zz3. Use 'linewidth'. This will generate the basic stickfigure on the screen. {0}. plot3(xx1. . xx4 = [L1*c1+L2*c12 L1*c1+L2*c12]. Draw thinner unit vectors to represent the origins and directions of {B}.xx4.'b'). zz1 = [0 L0]. zz4 = [L0 L0d3]. Specifically we will show how to draw a 3D stickfigure representation for the laboratory SCARA robot below.'b'.EL). Use X Y Z axis labels.yy2. % link from end of L2 to {4} yy4 = [L1*s1+L2*s12 L1*s1+L2*s12]. zz3 = [L0 L0]. Use subplot for 4 views (3 planar projections and orthographic view) – see view(AZ. or other robotics calculations to follow later in the 429/529 NotesBook.25 2.zz1.1 3D MATLAB Graphics for Animation This section presents some basic MATLAB code to draw a 3D robot to the screen. and {4}. xx3 = [L1*c1 L1*c1+L2*c12]. figure.4 to make thicker lines. % link from {B} to {0} yy1 = [0 0].yy4.yy1.'b'.xx3. % link from {0} to {2} yy2 = [0 L1*s1].'b'. % link from {2} to end of L2 yy3 = [L1*s1 L1*s1+L2*s12].5.yy3. here are some addons: Be sure to use axis(‘equal’) or axis(‘square’) with appropriate axis limits.
Otherwise the animation will flash by so fast you cannot see it. If you want to use this SCARA graphics in a motion simulation. you must use pause(dt) within your motion loop. {0}. The thicker robot lines are drawn in blue ('b' in the code) and the {B}. dt can be designed to approximately obtain realtime animations.300 35 . and {4} axes use the convention r g b for the X Y Z unit vectors.26 Here is what the simplified 3D 4dof Adept 550 SCARA robot could look like in MATLAB: This was generated using snapshot FPK for the inputs 1 2 d 3 4 15 25 0. though normal MATLAB and certainly MS Windows are not realtime. . Use of onscreen robot graphics is very helpful in validating that your simulated snapshots and motion trajectories are working correctly.
4 .2 Spatial 6dof 6R PUMA Robot FPK Example Spatial 6R PUMA Robot Forward Pose Kinematics Symbolic Derivations B Given 1 . 6 . 6 i 1 2 3 4 5 6 i 1 0 90 ai 1 0 0 L1 0 0 0 di 0 L0 0 L2 0 0 i 1 2 90 3 90 4 5 6 90 0 90 90 90 . 3 . 2 . 5 .5.27 2. calculate 0T and HT .
use the trigonometric sumofangle formulas (see the 429/529 NotesBook) to simplify. 3 3 1 3 Take advantage of parallel Z axes in consecutive frames {2} and {3} – use trigonometric sumofangles formulas. 6 6 3 6 0T 1 . 3 3T 4 . 2 .e.28 Substitute each row of the DH parameters table into the equation for i 1iT . 3 0T 1 1T 2 . Evaluate the angle offsets also. 2 . 3 1 23 s 23 0 s1 0 0 c23 c1 0 0 0 0 1 0 s23 0 0 1 0 c1s23 s1s23 c23 0 s1 c1 0 0 s23 0 c23 0 0 L1s2 1 L0 0 L1c2 0 1 L0 s1 L1c1s2 L0 c1 L1s1s2 L1c2 1 c23 cos 2 3 s23 sin 2 3 where we used the abbreviations: . 3 1 0 0 c1c23 s c 0 3T 1 . 2 . 2 . i. 5 . c1 s 0 3T 1 . si ci s c ci c i 1 i 1iT i i 1 s s ci s i 1 i i 1 0 0 c1 s 0T 1 1 0 0 c4 0 3 4T s 4 0 s1 0 0 c1 0 0 0 1 0 0 0 1 s4 0 c4 0 0 0 1 L2 0 0 0 1 s2 0 1 2T c 2 0 c5 0 4 5T s 5 0 0 s i 1 c i 1 0 0 0 1 L0 0 0 0 1 0 1 0 0 0 0 0 1 ai 1 di s i 1 di c i 1 1 s3 c 2T 3 3 0 0 s6 0 5 6T c 6 0 c3 s3 0 0 c6 0 s6 0 0 L1 0 0 1 0 0 1 0 1 0 0 0 0 0 1 c2 0 s2 0 s5 0 c5 0 1 0T 0T 1 2T 2 2T 3 3T 4 4T 5 5T 6 6 1 3 4 5 6 0T 0T 1 .
3 . 6 LB and LH are constants.g. 3 . due to the spherical wrist (wrist frames {4}. 5 . the symbolic terms would be worse in complexity than 0T . Therefore. no sumofangles formula simplification is possible. There is also a separate base frame {B}. For this robot we need to control the {H} 6 frame. but the rotation matrix 60 R is a complicated function of all six joint angles. . 6 3 6 Here we will present one aspect of this multiplication. again using 0T 0T 3T . Two additional. the MATLAB Symbolic Toolbox). 6 3T 4 4T 5 5T 6 6 4 5 6 There are no consecutive parallel Z axes in frames {4}. B 6 HT BT LB 0T 1 . the position vector 0 P6 is a function only of the first three joint angles: P . The below overall transform can be evaluated numerically. 4 . different from {0}. 2 . these two matrices can be evaluated and multiplied numerically. and {6} share a common origin). 5 . 6 c c s c s 4 6 4 5 6 0 s4 s6 c4 c5c6 s5c6 c4 s6 s4c5c6 0 c4 s5 c5 s4 s5 0 0 L2 0 1 It is left to the reader to perform the symbolic multiplication 0T 0T 3T . Alternatively. 0 6 1 2 3 0 L0 s1 L1c1s2 L2 c1s23 P4 T P4 L0 c1 L1s1s2 L2 s1s23 L1c2 L2 c23 0 3 3 Where constant vector P 0 3 4 L2 0 . 2 . fixed transforms The basic Forward Pose Kinematics result is 0T . {5}.29 3T 4 . the result is complicated 6 3 6 and can easily be found by using symbolic math on the computer (e. s4c6 c4 c5 s6 s5 s6 3 6T 4 . The overall Forward Pose Kinematics Homogeneous Transformation Matrix is given conceptually below. and {6}. In this way the PUMA FPK symbolic expressions are T partially decoupled: the position vector P 0 6 is only a function of 1 . 6 HT LH 0 6 1 0 B 0T LB 0 0 0 1 0 0 0 0 0 0 1 LB 0 1 1 0 6 HT LH 0 0 0 1 0 0 0 0 0 0 1 LH 0 1 . 5 . {5}. The orientation of {B} is identical to {0} and the orientation of {H} is identical to {6} for all motion.
964 0 0 1 0 0. PUMA Robot FPK Examples Given fixed robot lengths LB 1.023 0. 60 .636 0. 2 .634 0.771 1.015 0.634 B HT 0. since there 0 is no variable associated with these fixed homogeneous transformation matrices.030 0.638 0. 40 .322 0.915 0.715 0.744 0.999 0.036 2. 40 .358 0.899 2.163 0 0 1 0 B 2) Given 60 .181 0 0 1 0 0.636 0. 20 . 4 . calculate 0T and HT .030 0.015 0.076 0.437 0.771 1.296 1. B 1) Given 10 .2.036 3.5 (m). 10 .0.184 0 6T 0.023 0. Instead. they are determined by inspection. 3 3T 4 .634 0. L2 1. 2 . calculate 0T and HT .637 0.771 0. 6 6 3 6 6 B HT BT LB 0T 1 .30 6 Note that BT LB and HT LH are not evaluated by any row in the DH parameter table. using the rotation matrix and position vector components of the basic homogeneous transformation matrix definition.008 0. 20 . 6 0T 0T 1 . 3 . L1 1.899 2.816 0 0 1 0 . LH 0. L0 0.771 0. 6 0.638 0. 6 HT LH 0 6 0. 50 . 50 .3.544 0T 6 0.699 0. 5 . 30 .437 0.637 0.5.296 0.699 0.999 0.008 0.322 1. 30 . 5 .862 B HT 0.715 0.
6 .5 . 6 0T f 1 . 5 .5 . 5 . . We solve for the first three joints first using the position vector input. 4 .3 1 Joints 4.6 T 4 sets of 1 . 6 3 R 1 . there are 8 overall solutions to the inverse position problem for the PUMA. 2 .31 2. the position and orientation subproblems may be decoupled. 4 . three wrist actuators that rotate about a axes intersecting in a common point. 5 . calculate 4 . 1) Translational equations: Given P . 4 . 3 . The majority of industrial robots are in this category. . 6 6 0 R 1 . Some may lie outside joint ranges. calculate 1 . 3 unknowns. Therefore. based on the orientation caused by the first three joints.3 .6 for each.6 values (2 sets). and 6 cannot affect the translation of the wrist origin point. 3 . 6 dependent equations. 2 . In particular. Then we solve for the second three joints next using the given orientation. 4 .6 Inverse Pose Kinematics Decoupled Inverse Pose Kinematics Solution .e. 2 . 3 6 R 1 . 0 0 63 R 4 . 3 independent equations. 2 . The given position vector must point to the wrist point (the shared origin point of three consecutive wrist frames). 5 .2 . calculate .5 . and now knowing 1 . 2 sets of 4 . In this case.3 . 0 6 1 2 3 values (4 sets).2 . 6 0 6T 6 0 0 0 0 P6 1 . We can always transform any endeffector or other tool vector vector to this origin point. 2) Rotational equations: Given 60 R . Generally one would choose the closest solution to the previous position. there exists a closedform (analytical) solution to the inverse position kinematics.2 . 3 independent equations. 3 . PUMA Example (without details): Given 0T . since there are no more active joints beyond the last wrist joint. 3 unknowns. i. 5.3 .Pieper's Solution Pieper proved that if a 6dof robot has any three consecutive joint axes intersecting. many robots have a spherical wrist. 2 .
32 Peeloff homogeneous transformations matrices with unknowns to separate variables. and 6 1 . 5. 1 0T 01T 1 2T 2 2T 3 3T 4 4T 5 5T 6 6 3 4 5 6 LHS 0T is numbers 6 1 0T 1 0T 2T 2 2T 3 3T 4 4T 5 5T 6 1 6 3 4 5 6 1 can solve for 1 and 3 0T 2 0T 3T 4 4T 5 5T 6 3 6 4 5 6 can solve for 2 with 1 and 3 known 1 0T 4 0T 4T 5 5T 6 4 6 5 6 isolate and solve 4.
For inverse pose purposes we will here simplify instead and lock out the redundancy so that m = n = 6. There are some great ways to make use of this redundancy for performance optimization in addition to following commanded Cartesian translational and rotational velocity trajectories. still with multiple solutions (but finite). 8axis Spatial 8R NASA AAI ARMII Robot Inverse Pose Kinematics General Statement Given: Calculate: Since m = 6 (Cartesian dof) and n = 8 (joint dof) we have the kinematicallyredundant underconstrained case. There are infinite solutions (multiple as well). let us choose 3 5 0 for all motion to accomplish this.33 3. . Then we have a determined Inverse Pose Kinematics problem.
we will further simplify by ignoring the wrist joints 68 (5 is already locked to zero) and solve the Inverse Pose Kinematics problem only for the arm joints 1. . Hampton. use symbolic Forward Pose Kinematics): B x5 c1 d3 s2 d5 s24 P5 y5 s1 d3 s2 d5 s24 z d d c d c 3 2 5 24 5 B where: c24 cos 2 4 s24 sin 2 4 (Since 3 0 always. July 1992. The full inverse solution is given in Williams 1. See the previous section that explained the Pieper results for the 6axis PUMA robot. with 3 0 ): This equation yields (the derivation is left to student.L. Now. NASA Technical Memorandum 4377. Williams II. the Z axes of 2 and 4 are always parallel and we used the sumofangles trig formulas. calculate 1 . we can only specify three Cartesian objectives. Note that B P5 B P8 due to the spherical wrist. 4 i . VA. with 3 0 Reduced problem statement: T Given B P5 x5 y5 z5 .34 The Forward Pose Kinematics relationship is: So. 0F Inverse Pose Kinematics Symbolic Solution for Arm Joints Only. NASA Langley Research Center. Kinematic Equations for Control of the Redundant EightDegreeofFreedom Advanced Research Manipulator II. in this case the XYZ location of the origin of {5} with respect to origin of {B} (and expressed in the basis of {B}). The equations to solve for this problem come from the Forward Pose Kinematics relationships for the ARMII robot. the first step should be to simplify the equations as much as possible by calculating the required B 0T to achieve the commanded H T : 8 The problem can be decoupled between the arm joints 14 and the wrist joints 58 since the ARMII has a spherical wrist (all 4 wrist joint Cartesian coordinate frames share the same origin). the translational portion only (further. That is. with only three active joints. and 4.) 1 R. for all possible solution sets i. 2 .2.
it was for 1). Since 1 is now known (two values).35 Solution Process: 1. we solve for 2 (in that section. Solve for 2 by inverting the original Tangent HalfAngle definition: . we can modify the Y and Z equations: Y y5 s1 where: Z z5 d B Isolate the 2 4 terms: Square and add to eliminate 4: The result is one equation in one unknown 2: E cos 2 F sin 2 G 0 E 2Zd3 where: F 2Yd3 G Y 2 Z 2 d32 d52 We can solve this equation for 2 by using the Tangent HalfAngle Substitution. A ratio of the Y to X equations yields: 1 180 is also a valid solution 2. We presented this back in the Inverse Pose Solution of the planar 3R robot.
commanded B also calculate the associated 4 R – all should be different (why?). but it has similar joints when 3 0 ). 2 . plug all solution sets 1 . So there are a total of four 1 . you can check the results. P. there are two values for 2. summarized in the table below. To find 4. There are multiple solutions: two values for 1. for each valid 1 . i 1 2 3 4 1 1 1 2 3 0 0 0 0 4 4 –4 2 1 2 2 1 180 1 180 22 21 4 –4 In all numerical examples.36 Two 2 solutions result from the in the quadratic formula. plus 3 0 ). 4 solution sets for this reduced problem. We can show this with the PUMA model (it’s not the same robot. there is a unique 4. return to original (arranged) translational equations: d5 s24 Y d3 s2 d5c24 Z d3c2 Find the unique 4 for each 2 value (use the quadrantspecific atan2 function in MATLAB): Solutions Summary The solution is now complete for the ARMII robot reduced inverse pose problem (translational joints only. both are correct (there are multiple solutions – elbow up and elbow down). 4 one at a time into the Forward Pose solution and verify that all sets yield the same. These four solution sets occur in a very special arrangement pattern. 2 . 2 . for each 1. B 5 You can .
6572 P5 y5 0. 4 .37 8R ARMII Robot Translational Inverse Pose Kinematics Example Given P .6 46. i 1. 3. 2. B 5 . B 5 1 2 4 i B x5 0.6 20 3 0 0 0 0 4 30 30 30 30 Check all solution sets via Forward Pose Kinematics to ensure all yield the correct. calculate .1159 z 1.6952 5 Answers (deg): i 1 2 3 4 1 10 10 190 190 2 20 46. . . commanded P.
95 in @16 s .8.83 VB 0. Given: the absolute velocity of B is 2 in @90 s the absolute velocity of C is 4 in @ 45 s From the given information: 0 A A VB 2 . 0 2.1 Velocity Introduction 2.38 2. 0 A 2.1.83 0 From the relative velocity equation: A A VC A A VB A B VC A B VC A A VC A A 2.83 .1 Translational Velocity Example This example demonstrates the various indices in k V .8 Velocity Kinematics 2.8. i j A is a fixed reference frame.83 AVC 2.
B. C.41 AVB 1. There are two more types: 1) k V V j k i i j 0 e.83 0 C 1.g.83 VC 2.41 0 C 4 AVC 0 0 C 2. BVA 2 0 A (9 of these) 2) k V 0 i i e. we have given 9 combinations. A B 0 VB 0 0 (9 of these) .58 BVC 1.83 VC 2. these same velocity vectors ( AVB .43 0 There are 27 permutations for A.g. So far.83 0 B B 0. AVC .39 Since velocity is a free vector. BVC ) can be expressed in frames {B} and {C} by using the rotation matrix: k V i j k A R A V i j (or just look at the components) B A 2 VB 0 0 B A 2.
40 2.8.2.2 Spatial 3P Cartesian manipulator X 0 0 J L L d1 d2 d3 T Derive f J i Lj 0 J : Partial derivatives definition f 0 0 px d 3 P3 p y d 2 p d z 1 px 0 d1 px 0 d2 px 1 d3 py 0 d1 pz 1 d1 0 py 1 d2 pz 0 d2 py 0 d3 pz 0 d3 0 0 3 0 0 0 1 0 J 0 1 0 1 0 0 x 0 0 1 d1 y 0 1 0 d 2 z 1 0 0 d 3 .2 Velocity Equations Derivations 2.8.
1) term of the Jacobian matrix zero? ( 1 cannot affect 0 zW ) .41 2.3 y L0 c1 L1s1s2 L2 s1s23 L1c2 L2 c23 z W 0 xW L0 c11 L1s1s21 L1c1c2 2 L2 s1s231 L2 c1c23 2 3 yW L0 s11 L1c1s21 L1s1c2 2 L2c1s231 L2 s1c23 zW L1s2 2 L2 s23 2 3 2 0 0 L1c1c2 L2c1c23 L1s1c2 L2 s1c23 L1s2 L2 s23 3 0 x L0 c1 L1s1s2 L2 s1s23 y L0 s1 L1c1s2 L2c1s23 z 0 W X 0 J L2c1c23 1 L2 s1c23 2 L2 s23 3 0 W Why is the (3. 2 . where W indicates the origin of the wrist frame {W} (recall the position of the wrist frame origin is a function of only the first three joint angles 1 .3 Spatial 3R PUMA robot (translational only) The PUMA translational velocity equations are derived as follows.2. 2 .8.3 : 0 0 x L0 s1 L1c1s2 L2 c1s23 P6 1 .
42 2.3 Jacobian Matrices Four ways to derive the Jacobian matrix: 1) Time derivatives and relative angular velocity equation 2) Partial derivatives definition – closely related to the first method f – vector of m Cartesian functions. . no Cartesian orientation vector exists that we can differentiate to get the angular velocity vector.8. This relationship holds only for translational part: That is.
due to joint i only.  k J 0 X N 1  a) Revolute Joint Columns k  0 X  N 2 0 XN N   k 0 XN i k 0V N i k 0N i ˆ ˆ where k Z i k R i Z i is the third column of i i N k i R and k P i N k i R i P i N where i P is the translational i N part of T .43 3) Column as velocity due to joint i Physical interpretation of the Jacobian matrix: each column i is the Cartesian velocity vector of the endeffector frame with respect to the base frame. for a revolute joint: ˆ k Z k i P k R i Z i i PN ˆ i k J i i k N k i ˆ ˆ Zi i R Zi Here is the Jacobian matrix for an allrevolute manipulator: k N k Z k 1P k ˆ k i k ˆ ˆ1 N Z i PN Z N PN J k ˆ k ˆ k ˆ Zi ZN Z1 k k . Here is Jacobian matrix column i. and with i factored out.
44 b) Prismatic Joint Columns ˆ ˆ where k Z i k R i Z i is the third column of k R . Introduction to Robotics: Mechanics and Control. NN to get NJ. i 0. for a prismatic joint: ˆ ˆ k Z k R iZ k J i i i i 0 0 4) Velocity recursion a'la Craig 2 1F Add translational and rotational velocities serially linkbylink from the base to the endeffector link. factor out from N vN . Then. 3rd edition.J. Use the velocity recursion equations. Pearson Prentice Hall. 2. Craig. 2005. i i Here is Jacobian matrix column i. N 1 . NJ. 2 J. . .1. Upper Saddle River. Revolute: i 1 i 1 vi 1 i 1 i i 1 i ˆ R ii i 1 i 1Z i 1 i 1 R i vi ii i Pi 1 Prismatic: i 1 i 1 vi 1 i 1 i i 1 i R ii i 1 ˆ R i vi ii i Pi 1 d i 1 i 1Z i 1 Start with 00 0 v0 0 (if the robot base is fixed).
0J f 0 P p 0 3 px y 3 3 pi p d j p i i j t j 1 j dt j 1 j i = x. y px 1 px 2 px 3 py 1 0 0 py 2 py 3 0 0 ˆ Z 0 0 0 1 2 3 1 2 3 0 3 1 2 3 0J .45 Derive 0 J : Partial derivatives definition For the planar 3R robot.
8.8.8 Velocity Kinematics Examples 2.2 Spatial 3P Cartesian Manipulator Velocity Example a) Forward Velocity Kinematics 0 X 0 J L 0 x 0 0 1 d1 d3 y 0 1 0 d 2 d 2 z 1 0 0 d d 3 1 b) Inverse Velocity Kinematics (analytical result) 1 L 0 J 0 X d1 0 0 1 x d 2 0 1 0 y d 1 0 0 z 3 0 0 z y x c) Singularity Analysis 0 J 1J 2 J 3 J 1 d) Static Force Analysis no possible singularities f 0J T F 0 0 0 f1 0 0 1 Fx f 2 0 1 0 Fy f 1 0 0 F 3 z Fz Fy F x .8.46 2.
. But simpler analytical expressions are possible for the Jacobian matrix.9 Jacobian Matrix Expressed in Another Frame Here the Jacobian matrix still relates the endeffector frame velocity with respect to the base frame. L1s1 L2 s12 J L1c1 L2c12 1 0 L2 s12 0 L2c12 0 1 1 L2 s2 J L1 L2 c2 1 1 L2 s2 L2c2 1 0 L2 1 0 0 1 0 0 1 c1 s1 0 R s1 c1 0 0 0 1 1 0 L1s2 J L1c2 L2 1 2 c12 R s12 0 2 0 s12 c12 0 0 0 1 L1s23 L2 s3 J L1c23 L2 c3 1 3 L2 s3 0 L2c3 0 1 1 c123 s123 0 R s123 c123 0 0 0 1 3 0 3 J agrees with that derived in the third method above. by choosing an intermediate frame to express the coordinates of the velocity vectors (different basis of expression).8.47 2. k k 0 v 0 R 0 v k 0 0 R k 0 R J 0 k v k J 0 0 J k 0 R k k 0 v 0 J is not dependent on a frame (relative joint rates) k 0 R 0 0J J k 0 0 R Planar 3R Robot The Jacobian was derived in frame {0} (here we go to {3} instead of {H}).
8.8. 0 . For example.10 Cartesian Transformation of Velocities and Wrenches Here we show how to move velocity and wrench vectors (both translational and rotational) from one point to another on a rotating rigid body. We can replace one vector with an equivalent vector acting at a different point. Basic equations: vB v A A A PB B A reversing: v A vB B A PB vB A PB B A B All vectors must be expressed in same frame. choose {A}. For instance.48 2. {B} could be the hand frame {H} where we want motion and {A} would then be the last wrist frame {n} for which the Jacobian matrix is derived.1 Velocity Transformation Here we find the equivalent velocity at {A} corresponding to a given desired motion of {B}.10. A A A A B 0VA B R PB B R 0VB 0 A 0 B B R A 0 A X A B V T B X A A B R A PB B R T A B R 0 A B V 0 where: PB pz py A pz 0 px py px is the cross product matrix. to calculate the velocity (or wrench) at the wrist to produce a desired velocity (or wrench) at the hand. 2.
866 0. Given: B 0 1 VB 1 0 B 0 0 B 0 2 0.5 0 0 0 A 0 0 0 0 0.866 0.5 2.5 0.5 0.6 0.5 0.5 0 R 0.5 0 0.5 0.5 0 0 0 1.866 0 2.866 0 0 0 1 A B A 2.866 0 0 0 0 0 0 0 1 2 2 0 Check: A B VA VB r B ˆ j ˆ 1 i ˆ k 1 0 1 0VA 1 0 0 2 1 6 5 0 3 0 0 0 0 0 .5 0 0 1.5 0 A A PB B R 0 0 2.49 Velocity transformation example Find the equivalent velocity at A corresponding to a given motion of B.6 1.5 1 3.866 0 0 0 2.6 0 0 3 0 0 1 0 X X A A B V T B 0 0 1.366 0.866 0.866 0.834 0 0 A 0 0 0 0 0 1 0 3 0 VA 0 0 0 0.5 0.6 1 3.6 PB 1.
2 Pseudostatic Wrench Transformation Here we find the equivalent wrench at {A} corresponding to a given wrench at {B}. A PB B R is switched with 0. i. while in the wrench transformation. For instance.10. A A B R 0 B FB FA A A M A A PB B R B R M B A W ATW W B B A B R 0 T A A A PB B R B R A B W A Note: ATW is a blocktranspose of ATV . the rotational term is unchanged.e. choose {A}.50 2. .8. B B There is a duality here: in the velocity transformation. {B} could be the hand frame {H} where we want the wrench to be exerted and {A} would then be the last wrist frame {n} for which the Jacobian matrix is derived. the translational term is unchanged. Basic equations: FA FB M A M B PB FB A more properly. k FB All vectors must be expressed in the same frame.
5 0 0 0 M A 0 0 0 2.5 0. calculate the vector values on the same rigid link.366 A 0 1 0 0 0 0 0 FA 0 0 1. A W ATW W B B 0 0 0 0 1 0.866 0. given the end vector values {B}.866 0 0 0 0 1 1. given the inward vector values {A}.6 0. i.866 0 0 0 3 0 0 0 1 0 3 0 Check: FA FB M A r FB Note: the derivations and examples for both velocity and wrench transformations were for the inverse case. We could easily adapt the derivations and transformations to perform the forward calculation. calculate the end vector values on the same rigid link.5 0.866 0.5 0. A PB B R are the same as in the velocity example above. outward towards {B}. A B A B A R .e. Given: B 1 FB 1 0 B 0 M B 0 0 P .366 0.51 Wrench transformation example Find the equivalent wrench at A corresponding to a given wrench at B.5 0. i. but inward toward {A}.e. .
2005): d A B B R Q BA R BVQ AB BAR BQ dt General fivepart acceleration equation: Position: A A A P A PB B R B P P AT B P B Velocity: A A A V A PB B R B P B R B P V V0 VP P Acceleration: d A VB BA R BVP AB BA R B PP dt A A A A A A A A AB B R B AP A B B R BVP A B B R B PP A B B R BVP A B A B B R B PP A A A A A A A A A AB B R B AP 2 A B B R BVP A B B R B PP A B A B B R B PP A A0 AP 2 V P P . Transport Theorem (Craig. expressed in the basis (coordinates) of {k}.52 2. with translational and rotational parts. Acceleration Kinematics Analysis is useful for: Resolved Acceleration Control Acceleration of any point on manipulator Moving objects in workspace – smooth trajectory generation Required for Dynamics Translational Acceleration k i j A is the translational acceleration of the origin of frame {j} with respect to reference frame {i}.9 Acceleration Kinematics The acceleration vector is the first time derivative of the velocity vector (and the second time derivative of the position vector). Both Cartesian acceleration and velocity are vectors. It is linear in acceleration terms but nonlinear in rate components.
expressed in the basis (coordinates) of {k}. Instead we use relative velocity equations: Rotational Velocity: A A C AB B R BC Rotational Acceleration: d d A A A C AB B R BC A B B R BC dt dt A A A B A A B C B B R C B B R C For vectors expressed in the local frame: A A A C B R B A A B C R C B A C B R B A A B C R C B C Combined Translational and Rotational Acceleration a X A where: a 0 0 N 0 both a and are (3x1) vectors 0 N Acceleration Example Planar 2R robot – acceleration of endeffector frame with respect to {0}.53 Rotational Acceleration k i j is the rotational acceleration of frame {j} with respect to reference frame {i}. also expressed in {0}. 1) Craig (2005) acceleration recursion 1 L1 P2 0 0 2 L2 P3 0 0 c2 R s2 0 2 1 s2 c2 0 0 0 1 0 a0 0 0 23 R I 3 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 1 1 0 1 0 1 a1 0 0 . No angular orientation exists for which we can take the time derivative to find the angular velocity.
54 0 2 2 0 1 2 0 2 2 0 1 2 2 a2 1 R 11 1P2 11 11 1P2 1a1 2 2 c2 s2 0 L11 L1c21 L1s21 2 a2 s2 c2 0 L11 L1s212 L1c21 0 0 1 0 0 2 3 a3 23 R 2 2 2 P3 22 22 2 P3 2 a2 0 3 3 2 2 0 1 2 L 2 3 a3 L2 L c L s L s L c 2 1 2 2 1 2 1 1 2 1 1 2 2 1 2 1 1 2 1 0 0 0 a3 3 R 3 a3 L s c 2 L s c 2 2 12 1 2 12 1 2 1 1 1 1 1 0 2 a3 L1 c11 s112 L2 c12 1 2 s12 1 2 0 Rewrite for comparison to results in the next section: L s L2 s12 a3 1 1 L1c1 L2 c12 0 L2 s12 1 L1c11 L2 c12 2 L1s11 L2 c12 1 2 1 L2 s12 1 2 1 2 The second term above can be written as: L1c11 L2 c12 1 2 L1s11 L2 s12 1 2 L2 c12 1 2 1 L2 s12 1 2 2 .
55 Acceleration Example (cont. one must account for r .) Planar 2R robot – acceleration of the endeffector frame with respect to {0}. L1s1 L2 s12 J L1c1 L2c12 1 0 L2 s12 L2c12 1 d J dJ ij J dt dt dJ ij dt J ij d1 J ij d 2 J d ij N 1 dt 2 dt N dt L1c1 L2 c12 1 L2 c12 2 0 J L1s1 L2 s12 1 L2 s12 2 0 L1c11 L2 c12 0 J L1s11 L2 s12 0 X 0 0 dJ ij dt J ij k k 1 k N L2c121 L2c12 2 L2 s121 L2 s12 2 0 L2 c12 1 2 L2 s12 1 2 0 1 2 1 2 J 0 J L1s1 L2 s12 Lc L c 0 X 1 1 2 12 1 L1c11 L2 c12 L2 s12 L2 c12 1 L1s11 L2 s12 1 2 0 1 2 1 2 L2 c12 1 2 L2 s12 1 2 1 2 0 This yields the same result as before. . Alternative method: 2) Differentiation of rate equation X J X J J Do in frame {0} – if in frame {2} or {3}. also expressed in {0}.
Dynamics equations – is required for the Newton/Euler dynamics recursion in this 429/529 NotesBook Supplement. so the analytical approach 1 J X J is better. In this case: J 1 J How can we find the time rate of change of the Jacobian matrix J ? See the previous page for a specific example. but acceleration is commanded instead of 1 X J and double integrate to get the commanded joint velocity. ii. Forward Acceleration Kinematics Analysis – X J J predicts the Cartesian accelerations X given the joint rates and accelerations. numerical instability can result. Resolved Acceleration Control – like resolved rate. . Chapter 4. Solve J angles. If acceleration is calculated via numerical differentiation. iii. if inverse dynamics control is being used in the resolvedrate control algorithm framework.56 Uses for general acceleration equation X J X J J J X J 1 i. assume X is constant and so X 0 . Now.
10 Robot Workspace The workspace (also called work envelope) of a robot manipulator is defined as the volume in space that a robot’s endeffector can reach. Generally large translational and rotational workspaces are desired for a more capable robot. Generally serial robots have a big advantage over parallel robots in terms of larger workspace (exception: parallel cablesuspended robots). The workspace limits are also due to the specific joint limits of each joint. in side (left) and top (right) views. The translational workspace is easier to determine and display.57 2. Workspace is defined for both position (translational workspace) and orientation (rotational workspace). The workspace limits are due to the total reach of the combined robot links (both extended and folded upon each other). The figure below shows the translational workspace of a Cybotech P15 articulated robot. and is the focus of this section. Cybotech P15 Manipulator Translational Workspace .
cylindrical. The next five images represent the reachable. . spherical. therefore a limited dextrous workspace is generally defined (such as all orientations within 30 rotation about all three axes). SCARA. For parallel manipulators the dextrous workspace is often null. to form a volume. The reachable workspace of the Adept 550 SCARA robot is shown below. Adept 550 SCARA Robot Reachable Workspace Adept 550 TableTop Robot User’s Guide. workspaces of five common manipulator arrangements (Cartesian.58 The robot workspace attainable at any orientation is called the reachable workspace. The robot workspace attainable at all orientations is called the dextrous workspace. This topview planar workspace shape is extended 200 mm in the vertical direction by the prismatic joint limits on d3 (into the paper). or translational. 1995. The dextrous workspace is a subset of the reachable workspace. and articulated).
59 Cartesian Manipulator Translational Workspace Cylindrical Manipulator Translational Workspace .
60 Spherical Manipulator Translational Workspace SCARA Manipulator Translational Workspace .
61 Articulated Manipulator Translational Workspace The previous five images came from: societyofrobots.shtml .com/robot_arm_tutorial.
than an equivalent serial robot with a larger workspace.html .org/Reviews/Review011. stiffness. Orthoglide Robot Reachable Workspace parallemic. Imagine how much greater the translational workspace would be if there were only one supporting serial chain to the endeffector. However. with lower moving mass. and accuracy. the tradeoff is that the parallel robot achieves much greater accelerations.62 The figure below presents the translational workspace for a parallel robot (the threeaxis Orthoglide).
Planar Robotic Mechanisms: Analysis and Configuration Comparison. can be designed with arbitrarilylarge workspaces. 3 . VA. August. 1988.D.63 Again. These devices. parallel robots such as the 3RRR shown generally have small workspaces.L. Cable suspended robots with workspaces on the order of several kilometers have been proposed. James Albus). R. Virginia Polytechnic Institute and State University. 3RRR Parallel Robot Sample Limited Workspace 3 4F Optimized 3RRR Parallel Robot Workspace1 A notable exception to the small workspace rule of parallel robots is the cablesuspended robots. Department of Mechanical Engineering. Dissertation. Blacksburg. such as the NIST RoboCrane (right. Ph. Williams II. developed at NIST by Dr.
Singular Value 3. J J w X k J H .2 Inverse Velocity (ResolvedRate) Solution 3.1 Pseudoinversebased Particular Solution – satisfies primary task (satisfies Cartesian trajectory) Minimum Manipulator Kinetic Energy W M .64 Chapter 3. Decomposition (SVD) would ameliorate this problem.3 Klein and Huang’s Algorithm This methods yields the same results as the pseudoinverse with gradient projection into the nullspace.2. but it is more efficient (less computations). but I think with today’s processors. KinematicallyRedundant Robots 3. P the manipulator inertia tensor J X where: J M J J M J 1 T 1 T 1 This MoorePenrose pseudoinverse form is subject to singularities. T solve {w} using Gaussian elimination. T So much of the existing kinematicallyredundant robot literature is dedicated to more efficient redundancy resolution. . then: J w k H . Klein and Huang’s algorithm accomplishes the particular and homogeneous solutions at the same time. this is no longer a problem.2.
i.4 Singular Value Decomposition Singular Value Decomposition (SVD) yields the same results as the pseudoinverse with gradient projection into the nullspace. V is also roworthonormal.2. where n – m is the degree of redundancy.65 3. as opposed to yielding infinite joint rates at singularities. there will always be n – m zero singular values. U U I n (see the SVD example) T Columns of U corresponding to nonzero w j are an orthonormal basis which spans the range of J. however. Any additional zero singular values correspond to degeneracies in J.e. .: T T V V V V I n 1 is set to zero for w j 0 (ain’t wj U U T Im . For underconstrained systems of equations. Columns of V corresponding to zero wj are an orthonormal basis for the nullspace of J. In the above expression. but with singularity robustness. but SVD results are bounded so the motion can drive through singularities. the solution cannot track arbitrary Cartesian trajectories. J U W V J U W V T mxn m x n column orthogonal n x n positive semidefinite diagonal n x n orthogonal 1 wj T U J V diag wj are the singular values of [J]. If [J] is less than full rank. math fun?!?) Both U and V are columnorthonormal (ignoring the last n – m columns of U).
514 0.514 0.288 0.548 0.430 0 0 0 0 0.866 J U W V T 0 0 0.188 0.441 0.381 0 0.903 0.548 0.344 3.118 * In the above.939 0 0 0 0 0 0.420 0 0.732 1.288 0.500 2.366 0.430 1.430 0. the third singular value is zero. so * 1 is set to zero (n – m singular values will always be wj zero).500 0.288 0. This result agrees with J from the MoorePenrose pseudoinverse formula in the 429/529 NotesBook.836 0 0 2.188 0.205 0.467 0.903 0 0.786 0.000 J 0.939 T U T J V diag 1 wj 0 0 0.903 1.836 0 J 0 0.786 0.366 1.66 Singular Value Decomposition (SVD) Example J 1.430 0. Note: U U T I2 1 0 0 U U 0 1 0 I3 0 0 0 T V V T V V I 3 T .344 0.323 0.866 0.903 0.
Mathematically. . overconstrained. G is a generalized inverse of matrix A if: AG A G The MoorePenrose pseudoinverse is just one possible generalized inverse of a matrix. It is the one applied most often to redundancy resolution of manipulators. the following relationships hold for the MoorePenrose pseudoinverse: G AG A * G A G A AG AG * where * indicates the complex conjugate transpose.67 3.5 Generalized Inverses A generalized inverse of a matrix gives an answer to the linear problem even when a true matrix inverse does not exist (underconstrained.2. or rowrankdeficient). In addition to the above relationship.
calculate the actuator forces and torques (this linear algebraic solution is much more straightforward than Forward Dynamics): Given . It is the study of the relationship between forces/torques and motion. Manipulator Dynamics Kinematics is the study of motion without regard to forces. calculate . must be moving with respect to an inertiallyfixed frame. nonlinear ODEs): Given . Methods for deriving the dynamic equations of motion: NewtonEuler recursion (force balance.68 Chapter 4. which are highly coupled and nonlinear. . . calculate (all N x 1 vectors). Kinetics: Translational: Newton's Second Law: Inertial force at center of mass Rotational: Euler's Equation: Inertial moment at center of mass (could be anywhere on body) The kinematics terms. LagrangeEuler formulation (energy method). including inertial forces with D'Alembert's principle). . aCi . (all are N x 1 vectors). Dynamics is composed of kinematics and kinetics. Both problems require the N dynamic equations of motion. Dynamics is the study of motion with regard to forces. Assumptions: serial robot rigid links ignore actuator dynamics no friction no joint or link flexibility . i . compute the resulting motion (this requires the solution of highly coupled. b) Inverse Dynamics (control) – given the desired motion. i . a) Forward Dynamics (simulation) – given the actuator forces and torques. one for each link. The frame of expression {k} needn't be an inertiallyfixed frame.
units: md2 (kgm2). Here are some example inertia tensor components using the parallel axis theorem: A 2 2 I zz C I zz m xC yC A I xy C I xy mxC yC Here is the entire inertia tensor expressed in vectormatrix form using the parallel axis theorem: A I C I m PCT PC I 3 PC PCT . 2) Moments of inertia must be positive.69 4. and the eigenvectors are the principal axes.1 Mass Distribution: Inertia Tensor Spatial generalization of planar scalar moment of inertia. the products of inertia normal to this plane are zero. More interesting facts regarding inertia tensors 1) If two axes of the reference frame form a plane of symmetry for the mass distribution. The invariant eigenvalues of a general AI are the principal moments of inertia. tensor expressed at a given point in the rigid body. the principal axes. relative to frame {A} is: The inertia Mass moments of inertia V I xx y 2 z 2 dv I xy xy dv V I yy x 2 z 2 dv V I zz x2 y 2 dv V Mass products of inertia I xz xz dv V I yz yz dv V Principal moments of inertia A certain orientation of the reference frame {A}. Parallelaxis theorem We can obtain the mass moment of inertia tensor at any point {A} if we know the inertia tensor T at the center of mass {C} (assuming these two frames have the same orientation): PC xC yC zC is the vector giving the location of the center of mass {C} from the origin of {A}. products of inertia may be either sign. yields zero products of inertia. 3) The sum of the three moments of inertia are invariant under rotation transformations.
2 NewtonEuler Recursive Algorithm This is a recursive approach (Craig. Inertial loads Newton and Euler translational and rotational dynamics equations: Force balance Moment balance (about CGi) (using D'Alembert’s principle. 2005) based on freebody diagrams (FBDs) to determine the dynamics relationships linkbylink. . Used numerically it can calculate the inverse dynamics solution efficiently. Freebody diagram of link i: f i: ni: internal force exerted on link i by link i1. internal moment exerted on link i by link i1. the inertial force is –ma).70 4.
from Craig (2005).71 NewtonEuler Recursive Algorithm Summary This methods can be used to find the robot dynamics equations of motion.e. for clarity) Internal forces and moments: f i f i 1 Fi ni ni 1 i PCi Fi i Pi 1 f i 1 N i Externally applied joint torques: i ni Zi Inclusion of gravity forces: 0 a0 g This is equivalent to a fictitious upward acceleration of 1g of the robot base. . It can also be used to directly solve the inverse dynamics problem numerically. which accounts for the downward acceleration due to gravity (i. The summary of equations below. for clarity) Velocities and accelerations (kinematics): ˆ i 1 i i 1Z i 1 ˆ ˆ i 1 i i i 1Z i 1 i 1Z i 1 ai 1 ai i i Pi 1 i i i Pi 1 aCi 1 ai 1 i 1 i 1PCi 1 i 1 i 1 i 1PCi 1 Inertial loading (kinetics): Fi 1 mi 1aCi 1 N i 1 C Ii 1 i 1 C I i 1 Inward iteration for kinetics: i: N 1 (without regard for frames of expression. Outward iteration for kinematics: i : 0 N 1 (without regard for frames of expression. this conveniently includes the weight of all links). assume an all revolutejoint manipulator (prismatic joint dynamics have different equations).
not accelerations. L k u k . to yield N independent dynamics equations of motion. d L L dt This expression may be rewritten using L k . u : d k k u dt . once for each joint variable.3 LagrangeEuler Energy Method This is an alternative method to find the robot dynamics equations of motion. It requires only translational and rotational link velocities. T M . The Lagrangian is formed from the kinetic energy k and potential energy u of the robot system.72 4. Perform this equation N times. and actuator torque. Dynamic equations of motion These are found for each active joint from the following expression involving the Lagrangian. ki i 1 N u ui i 1 N Note: 1 k . joint variable. 2 where M is the manipulator mass matrix. u 1 1 T mVCiVCi iT Ci I ii i 2 2 0 ui uREF mi g 0 PCi ki k .
2) NewtonEuler recursion.73 4. force and moment dynamics balance.4 Simple Dynamics Example Derive the dynamic equation of motion for a planar onelink 1R mechanism by three methods: 1) Sophomore dynamics method – FBD. force balance FreeBody Diagram: FX FY FX mg FY . 3) LagrangeEuler formulation.FBD. g Y X L 1) Sophomore methods .
74 1) Sophomore methods – FBD. force balance (cont.) F a) 0 x maCx L L Fx m s 2 c 2 2 mL 0 Fx s 2c 2 F b) 0 y maCy L L Fy mg m c 2 s 2 2 L 0 Fy mg m c 2 s 2 mL2 I I md I zz 4 C 2 C c) M z0 0 I 0 mL gc 2 mL gc I 2 where: 0 .
75 Simple Dynamics Example (cont.) 2) NewtonEuler recursion Outward iteration: i = 0 (the only iteration) 0 0 P 0 1 0 L 2 1 PC 1 0 0 c 1 0 R s 0 s c 0 0 0 1 I xx C1 I1 0 0 0 I yy 0 0 0 I zz 0 0 0 0 0 1 0 0 0 0 0 0 0 a0 g to account for gravity 0 a1 01R 0 0 0 P 00 00 0 P 0 a0 1 1 c a1 s 0 s c 0 0 0 gs 0 g gc 1 0 0 1 L 2 gs 2 L 1 aC1 11 1PC1 11 11 1PC1 1a1 gc 2 0 L 2 gs 2 L 1 F1 m 1aC1 m gc 2 0 1 0 N1 I1 1 1 I1 1 0 I zz C1 1 1 C1 1 .
76 Inward iteration: i = 1 (the only iteration) 0 2 f 2 0 0 0 2 n2 0 0 L 2 gs 2 L 1 1 2 1 f1 2 R f 2 F1 m gc 2 0 1 n1 21R 2 n2 1PC1 1F1 1P2 21R 2 f 2 1 N1 0 0 1 n1 0 0 mL L I gc zz 2 2 ˆ 1n1 1Z1 I zz gc 4 2 Check with the sophomore dynamics method above: L 2 L 2 L gs 2 2 c 2 s c s 0 0 0 1 s c 0 m gc L m L 2 s L c g f1 1 R f1 2 2 2 0 1 0 0 0 mL2 mL .
77 3) LagrangeEuler formulation 0 1 1 0 C1 I xx I1 0 0 0 I yy 0 0 0 I zz Kinetic and potential energy are scalars invariant with frame of expression – write k in {1} and u in {0}. . 0 L 1 vC1 2 0 1 0 0 1 I1 1 0 0 I zz C1 1 k 1 mL2 2 1 2 I zz 2 4 2 L c 0 2 L L u 0 m g s mg s 2 0 2 0 1 mL2 L L k u 2 I zz mg s 2 4 2 d L L dt L mL2 I zz 4 d L mL2 I zz dt 4 mL2 mL I zz gc 4 2 L L mg c 2 This result agrees with the sophomore and NewtonEuler recursion dynamics methods above to solve this same problem.
G N N mass matrix.78 4. M J 1 J Gx J T G X J X J J . symmetric and positive definite N 1 vector of Cartesian Coriolis and centrifugal terms N 1 vector of gravity terms in Cartesian space Gx where: M x J M J T note: 1 Vx . symmetric and positive definite N N 1 Coriolis matrix 2 N N centrifugal matrix N N 1 vector of gravity terms N N 1 1 : 2 N 1 : 2 1 1 2 1 3 N 1 N 2 22 N T T Cartesian State Space Equation F M x X Vx . Gx M x V . G Configuration Space Equation M B C 2 G M B C G 2 N N mass matrix. x N N Cartesian mass matrix.5 Structure of Manipulator Dynamics Equations State Space Equation M V . J T V . symmetric and positive definite N 1 vector of Coriolis and centrifugal terms N 1 vector of gravity terms M V .
79 4.6 Robot Dynamics Example Here we summarize the dynamics equations of motion for the twolink planar 2R robot when each link is modeled as a homogeneous rectangular solid of dimensions Li. L2 Y0 2 L1 1 X0 NewtonEuler recursion with outward kinematics and inertial calculations. . wi. followed by inward kinetics balances yields: m2 L1L2 c2 m2 L2 m2 L2 m2 L1L2 s2 2 m2 L2 c12 2 2 2 I zz 2 1 g 1 I zz 2 2 2 4 4 2 2 1 I zz1 I zz 2 2 m1L1 m L2 m L L c m L2 2 m2 L1 m2 L1L2 c2 2 2 1 I zz 2 2 1 2 2 2 2 2 4 4 2 4 mLc m LL s m Lc 2 1 2 2 22 m2 L1L2 s2 1 2 1 1 1 m2 L1c1 2 2 12 g 2 2 2 where I zzi mi 2 Li hi2 . 12 These dynamics equations of motion are very complicated – imagine how much worse these equations will be for a spatial 6axis robot such as the PUMA industrial robot. hi of mass mi.
the dynamics equations of motion from the previous page are expressed in configurationspace form below: M B C 2 G M . G same as above m L L s B 2 1 2 2 1 2 0 m LL s 2 1 2 2 2 0 1 2 C 2 2 m2 L1L2 s2 2 0 2 .80 State Space Representation For the planar 2R robot. G 1 2 1 2 m L2 m L2 2 I zz1 I zz 2 1 1 m2 L1 m2 L1L2c2 2 2 4 4 M 2 m LLc m L I zz 2 2 1 2 2 2 2 2 4 m2 L1L2 s2 2 2 m2 L1L2 s2 1 2 2 V . m2 L1L2 s2 2 1 2 mLc m1L1c1 m2 L1c1 2 2 12 2 g G 2 m2 L2c12 2 I zz 2 m2 L1L2c2 m2 L2 2 2 4 m2 L2 2 I zz 2 4 Configuration Space Representation For the planar 2R robot. the dynamics equations of motion from the previous page are expressed in statespace form below: M V .
Notice from the joint angles. joint accelerations.5 0 0 The dynamics equations require the relative joint accelerations . joint torques. and Cartesian pose) for this problem are given on the following page. The plots for various variables of interest (joint angles. note that the robot travels 0. but we cannot control independently with only twodof. they are inputs to the inverse dynamics problem.01 sec. here we calculate the required torques (i. and joint torques plots that the robot is approaching the 2 = 0 singularity towards the end of this simulated motion. joint accelerations. . how do we find these? (See 1 the last page of the Acceleration Kinematics notes in this 429/529 Supplement: J X J . X 0 . The robot does not move in X.) Simulate motion for 1 sec. must move to compensate for the pure Y motion. connecting the links at their very edges (not physically possible. solve the inverse dynamics problem) to provide the commanded motion at every time step in a resolvedrate control scheme. Given: L1 = 1. with a control time step of 0. These are the joint torques necessary to move this robot’s inertia in the commanded manner. The first three plots are kinematics terms related to the resolvedrate control scheme. The revolute joints are assumed to be perfect.0 m L2 = 0. joint rates.5 m/s).5 m Both links are solid steel with mass density = 7806 kg m3 and both have width and thickness dimensions w = t = 5 cm.81 Numerical Dynamics Example For the planar 2R robot whose dynamics equations of motion were presented analytically above.5 m in the Y0 direction in 1 sec (which agrees with the constant commanded rate of 0. The initial robot configuration is: 10 1 2 90 The constant commanded Cartesian velocity is: 0 x 0 X (m/s) y 0. Identical results are obtained by both analytical equations and numerical Newton/Euler recursion. In this example. joint rates. just a simplified model). In the last plot.e. The joint torques are calculated by the numerical recursive NewtonEuler inverse dynamics algorithm.
5 2 2.8 1 0 0 1 (r).2 0.6 time (sec) Joint Accelerations 0.6 0 0.4 1.8 1 Dynamics Results Ignoring Gravity Dynamics Results Including Gravity .5 3 0 0.4 0.2 0.6 time (sec) 0. 2green) (Nm) 60 50 40 30 20 10 0 0 0.8 0.rad) (g) (deg) 1.5 1 0.2 0.6 1.82 Numerical Dynamics Example: Plots Cartesian Pose 2 1.4 0.4 0.6 tim (sec) e 0.8 1 150 100 50 0 50 0 0.6 tim (sec) e 0. 2 0.5 0 0. joint 2 (g) (rad/s) 2 0.4 0.8 1 joint 1 (r).2 0.8 x (r). (b) (m. joint 2 (g) (rad/s ) joint 1 (r). 2green) (Nm) (1red. y (g).2 0.6 time (sec) Joint Torques 80 70 250 200 Joint Angles 0.2 1 90 80 70 60 50 40 30 20 10 0.4 0.6 time (sec) Joint Rates 1.8 1 10 5 0 5 10 15 20 0 0.2 0.4 0.8 1 Joint Torques (1red.5 1 1.
1 Inverse Pose Control Architecture 5. 5.83 Chapter 5. we can simulate robot control using various popular robot control architectures.2 Inverse Velocity (ResolvedRate) Control Architecture . Robot Control Architectures With the robotic kinematics and dynamics we have learned and simulated.
symmetric and positive definite N 1 vector of Coriolis and centrifugal terms N 1 vector of friction torques G N 1 vector of gravity terms .84 5. in order to linearize the robot system for standard controller methods. joint angles. rates. . Here they are in matrix/vector form: M V . such as PID control. F .3 Inverse Dynamics Control Architecture The inverse dynamics method is also called the computed torque control method. . G M V . In Chapter 4 we learned that the robot dynamics equations of motion are highly coupled and nonlinear. and accelerations Computed Torque Control Architecture The computedtorque control method is based on canceling the dynamics effects by using the inverse dynamics method. F . vector of applied joint torques N N mass matrix. and it is also called the feedback linearization control method.
85 5.4 NASA Langley Telerobotics ResolvedRate Control Architecture .
and D. J.Optional) This control architecture was implemented by Dr.M. providing telepresence.86 5. In addition. 6F 4 R. . 121(3): 425432. Henry. NaturallyTransitioning RatetoForce Control in Free and Constrained Motion. Bob and teams at NASA Langley Research Center and WrightPatterson AFB 4. After contact.W. Williams II.L. ASME Journal of Dynamic Systems. and Control. XC + Master XM  Resolved Rate C C XA Manipulator F/T Sensor M JT XF FM (FMA) KF (Force Reflection . Repperger. if there is a human teleoperator with haptic interface. Measurement.A.5 NaturallyTransitioning RatetoForce Controller Architecture Resolvedrate control automatically (naturally) changes to force control when the robot endeffector enters into contact with the environment. the force/moment that the human applies to the interface becomes proportional to the force/moment that the robot exerts on the environment at contact. M. A force/torque (F/T) sensor and force/moment accommodation (FMA) algorithm is required to accomplish this. This works and feels great in realworld applications. 1999. Murphy. the constant velocity command becomes a constant force command. Trans. ASME.
the armaturecontrolled DC servomotor driving a single robot joint / gear train / link combination. In this case. How is this possible? (Large gear ratios tend to decouple the dynamic coupling in motion of one link on its neighbors – we will see this in modeling soon. This requires dynamics. Manipulator dynamics is extremely complicated considering the number of terms. Chapter 6) is useful to get around the need for analytical expressions.) The vast majority of multiaxis industrial robots are controlled via linearized.87 5. shown below. We now will briefly discuss single joint control. presented in this section. So. We will focus on a common system. How is manipulator dynamics done in industry for control purposes? Answer: in almost all cases it is ignored. independent single joint control. OpenLoop System Diagram V(t) L R iA(t) vB(t) armature voltage armature inductance armature resistance armature current back emf voltage M(t) M(t) M(t) JM CM n generated motor torque motor shaft angle motor shaft velocity lumped motor inertia motor viscous damping gear ratio L(t) load torque L(t) load shaft angle L(t) load shaft velocity JL(t) CL total load inertia load viscous damping . but it is not coupled nor nonlinear. robot control in industry is generally accomplished by using N independent (but simultaneous) linearized joint controllers.6 Single Joint Control Every controller architecture we’ve considered requires linearized independent (but simultaneous) single joint angle control. numerical NewtonEuler recursion a’la Craig (2005. We can easily use symbolic MATLAB to crank out the terms but they go on for pages and their structure is difficult to understand. where N is the number of joint space freedoms of the robot.
Coulomb friction ignore discrete and continuous continuous for control design . model characteristics Real World Our Model (simplified) nonlinear multipleinput. stiction. the large gear ratio diminishes this problem hysteresis. backlash. multiple output (MIMO) coupled timevarying load inertia linearized singleinput. load shafts (the large gear ratio n reduces speed and increases torque) n >> 1 Realworld vs.88 ClosedLoop Feedback Highlevel Control Diagram Highlevel Computer Control Diagram Simplifying Assumptions rigid motor. single output (SISO) decoupled treat as a disturbance.
including a gear train. and control. Armature Circuit Diagram Electrical Model Kirchoff’s voltage law (1) Electromechanical coupling The generated motor torque is proportional to the armature current (2) The backemf voltage is proportional to the motor shaft angular velocity (3) (KT = KB can be shown) . The system diagram was shown earlier in this section.89 Single Joint/Link System Modeling We must derive all linear ordinary differential equations (ODEs) describing the dynamics of an armaturecontrolled DC servomotor driving a single robot joint / link. This is an electromechanical system. We must perform modeling. simulation.
Gear ratio n (6) Substituting (6) into (5) yields (7): (7) .90 Mechanical Model Euler’s Equation (the rotational form of Newton’s Second Law): Freebody diagrams Load (4) Motor (5) where LR (t ) is the load torque reflected to the motor shaft.
total reflected to motor shaft effective damping. total reflected to motor shaft .91 Reflect load inertia and damping to motor shaft Substitute into (4): (8) Substitute (8) into (7) to eliminate L: (9) Combine terms: (10) define: JL n2 CL CE CM 2 n JE JM effective inertia.
For common industrial robots.92 Final Mechanical Model (ODE): (11) This is a secondorder ODE in M. n >> 1 so For example. J L n 2 J M L C L n 2C M L L . the NASA 8axis ARMII robot downstairs has n = 200 with harmonic gearing. therefore we can choose a nominal JL and n2 assume it is constant without much error in control. Can also be written as firstorder ODE in M: (12) 1 is small. The gearreduced load inertia variation is then treated as a disturbance to the single joint controller. This is why we can ignore the timevarying robot load inertia and design decoupled independent linear joint controllers. An alternative is to reflect the motor inertia and damping to the load shaft as shown below. but we will use the first case above.
This block diagram and MATLAB/Simulink implementation including a disturbance torque are give below: . A transfer function goes inside each block representing component dynamics.93 OpenLoop Block Diagram Use Laplace transforms on different ODE pieces of the system model and then connect the components together in blocks connected by arrows representing variables.
94 ClosedLoop Feedback Control Diagram Assuming perfect encoder sensor for angular position feedback. The MATLAB/Simulink implementation is: . we include block for the singlejoint PID controller.
95 PID Controller Design PID Controller characteristics PID stands for: Controller transfer function: Each term does this: Use PID w/ approximate derivate in Simulink – numerical differentiation can lead to errors and numerical instability and thus it is to be avoided if possible. at least in theory and simulation. The real world always provides some additional challenges (noise. Add the I gain value to reduce steadystate error. Then add the D gain value term for stability. Always use Simulink to simulate control and dynamics response of the single joint/link system for different PID choices. backlash. such as in ME 401. modeling errors. compare various cases and select a suitable controller in simulation. etc. calibration.). Trial and Error PID Design Start with the P gain value – start low and increase P to get the desired time nature of response. . nonlinearities. Controller Performance Criteria Stability Rise Time Peak Time Percent Overshoot Settling Time Steadystate error These performance criteria provide a rational basis for choosing a suitable controller. A better approach is to perform analytical design for PID controllers using classical control theory.
joints. unexpected disturbances are modeled as disturbances in control systems. Lump all outboard links.96 Include Gravity as a Disturbance Torque Unmodeled. First let us model the gravity effect for a single joint. and motors as a single rigid body: test with original PID gains redesign new PID gains with gravity disturbance considered . However. It is convenient to do so at the motor torque level in the block diagram: Gravity is known and expected. in single joint control we can include the effect of gravity as a disturbance.
All joints must be active.1 Introduction Serial vs. Backhoe. Serial Robot: the joints and links extend from the base in a serial fashion to the endeffector.3 in this 429/529 Supplement) for calculating overall manipulator dof are very important for parallel manipulators to ensure the correct number and type of active and passive joints are used to obtained the desired dof. NASA Hyperredundant Serpentine Arm. Comparison of parallel vs. Parallel Robot: the joints and links are arranged in parallel from the base to the endeffector. Parallel robot – demonstrate with wooden model kit. repeatability Groundmounted actuators Directdrive Open structure for cabling Mobility – the Grubler (planar) and Kutzbach (spatial) mobility equations (see Section 1. Example parallel manipulators: Stewart Platform. There are multiple load paths from the endeffector to the base. Parallel Robots 6. Variable Geometry Trusses. serial robots Disadvantages of parallel robots Advantages of parallel robots Higher loads SMALLER WORKSPACE Higher accelerations Generally analysis is more difficult Lower mass Higher stiffness Better accuracy. There are infinite variations and possible designs for parallel robots. There is a combination of active and passive joints. The overall structure is cantilevered which is not good for resisting endeffector loads.97 Chapter 6. .
98 Kinematics problems we will consider for parallel robots forward and inverse pose solutions workspace forward and inverse velocity solutions Both inverse pose and rate problems have been presented for all possible planar parallel robots with 3 identical 3dof legs 5. CD Proceedings of the 1997 ASME Design Technical Conferences. 5 . Inverse Kinematics for Planar Parallel Manipulators. Williams II and B. DETC97/DAC3851.L. Shelley. phasors. Sacramento. We know the opposite case is true for serial robots in general. Serial robot kinematics – DH parameters.H. We could also do parallel robot dynamics but this is of less concern for a parallel robot than for a serial robot due to the parallel nature and its inherent advantages. CA. Euler’s Identity: Phasors enable compact notation and are very convenient for time derivatives (velocity analysis). 1997. Euler’s identity Phasors – a powerful ReIm manner to represent a vector. homogeneous transformation matrices Parallel robot kinematics – Vector loopclosure equations. 8F There exists an interesting duality between serial and parallel robots: for parallel robots. 23rd Design Automation Conference. September 1417. the inverse pose and velocity solutions are generally straightforward and the forward pose and velocity problems are harder. Example Vector loopclosure equation for the fourbar mechanism: R.
3 .2 .99 6. .2 Planar 3RPR Manipulator Pose Kinematics Planar 3RPR Manipulator Diagram Inverse Pose Kinematics (for pose control) Given: Find: Vector loopclosure equations The vector loopclosure equations are rewritten below: The inverse pose solution is straightforward: We can also calculate the intermediate passive joint variables 1. for use in velocity and dynamics analyses.
we can calculate these angles later.1 .2 .3 (also. dynamics.100 3RPR parallel robot Forward Pose Kinematics (for simulation and sensorbased control) Given: Find: This is a coupled. or computer simulation).3 . So. like the Inverse Pose Kinematics problem for serial robots. We use the same vector loopclosure equations: VLCE details expanded for one RPR leg: Considering all three legs (the problem is coupled). We will use the NewtonRaphson numerical iteration technique to solve this Forward Pose Kinematics Problem. We can directly solve the above six equations for the six unknowns. . these represent six scalar equations in the six unknowns x. However. Then we will have three equations to solve for the three primary unknowns x . if required for velocity. : Bring L2 to the other side and solve via the NewtonRaphson iterative numerical method. to simplify the Forward Pose Kinematics Problem. square and add each XY equation pair (for all three legs) to eliminate the intermediate variables 1. using Inverse Pose Kinematics. 2 . y. i . y .2 . nonlinear problem to solve – it is difficult to solve and multiple solutions generally exist.3 . we don’t always need the intermediate variables 1.
Gaussian elimination on J NR X F X is preferable numerically. 2. . The NewtonRaphson method is an extension of Newton’s single function/single variable rootfinding technique to n functions and n variables. 2. n So to calculate the required correction factor X at each solution iteration step. since it is more efficient and more robust than matrix inversion. we require: Fi X X 0 Now with O X 2 0 we have: i 1. For solution.101 6. . the higherorder terms O X 2 from the expansion are negligible. The following is the form of the given functions to solve: FX 0 where the n functions are: and the n variables are: F X F1 X F2 X Fn X X x1 T x2 xn T Perform a Taylor Series Expansion of F about X: Fi X X Fi X Fi x j O X 2 j 1 x j n i 1. It requires a good initial guess of the solution to get started and it only yields one of the possible multiple solutions. we must solve F X J NR X 0 : 1 X J NR F X Actually. .3 NewtonRaphson Method The NewtonRaphson Method involved numerical iteration to solve coupled sets of n nonlinear equations (algebraic/transcendental – not ODEs) in n unknowns. If X is small. 2. a multidimensional form of the x j Fi X X Fi X Fi x j Fi X J NR X 0 j 1 x j n i 1. n F Where J NR J NR X i is the NewtonRaphson Jacobian Matrix. . n derivative and a function of X.
for manipulator forward pose problems. where we use the Euclidean norm and is a small. the one resulting solution is generally the one you want. the robot cannot move too far from this known initial guess). Also halt the iteration if the number of steps becomes too high (which means the solution is diverging). However. the NewtonRaphson technique guarantees quadratic convergence. with L2 brought to the other side to equate the functions to zero.) This reduces computation if you need both forward pose computation and inversevelocitybased resolvedrate control. this does not present any difficulty since the existing known pose configuration makes an excellent initial guess for the next solution step (if the control rate is high. in the spatial it is related very closely.102 NewtonRaphson Method Algorithm Summary 0) 1) 2) 3) 4) Establish the functions and variables to solve for: F X 0 Make an initial guess to the solution: X0 Solve J NR X k X k F X k for X k . Also. closest to the initial guess. userdefined solution tolerance. Generally less than 10 iterations is required for even very tight solution tolerances. transcendental functions Fi are the squared and added equations for each RPR leg. There is a very interesting and beautiful relationship between numerical pose solution and the velocity problem for parallel robots: the NewtonRaphson Jacobian Matrix is nearly identical to the Inverse Velocity Jacobian Matrix for parallel robots. the NewtonRaphson technique requires a good initial guess to ensure convergence and yields only one of the multiple solutions. Now. with: X x y T The three coupled. where k is the iteration counter Update the current best guess for the solution: X k 1 X k X k Iterate until X k . many cycles per second. 3RPR manipulator forward pose kinematics solution Use the NewtonRaphson numerical iteration method for solution. i . nonlinear. (In the planar case it is identical. most likely the actual configuration of the real robot. If the initial guess to the solution X0 is sufficiently close to an actual solution. except in the case of singularities where the multiple solution branches converge.
After the forward pose problem is solved at each motion step.103 Derive the required NewtonRaphson Jacobian Matrix: F J NR X i x j Fi x Fi y Fi i 1. Alternate 3RPR manipulator forward pose NewtonRaphson solution As we said.2 . we can calculate the intermediate variables 1. Below is the required Jacobian Matrix for this case. 2. 3 where: B Ax Ai Ay and H Px Ci Py The index i was dropped for convenience. 2. where the odd functions are the X equations and the even functions are the Y equations. 2. 3 i 1. we could have solved the original six equations in the six unknowns including the three intermediate variables 1. 3 in the above definitions in the proper places in the overall NewtonRaphson Jacobian Matrix. 2. The functions are simpler (no squaring and adding) but the size of the problem is doubled to i 1.3 as in inverse pose kinematics solution above. 6 . 3 i 1. 2. . . also the variable ordering T is X x y 1 2 3 .3 . use i 1.2 .
this fourbar mechanism has 1dof. The forward pose kinematics solution may be found by intersecting leg A3C3 (a circle of given radius L3. In general. 4. With given 1 lengths L1 and L2. There are at most six intersections between a circle and tricircular sextic and so there may be up to six real multiple solutions to the 3RPR parallel robot forward pose kinematics problem.104 1 0 1 J NR X 0 1 0 0 1 1 1 P x s P1 y c 1 P x c P y s 1 1 P2 x c P2 y s P3 x c P3 y s L1s1 L1c1 0 0 0 0 0 0 L2 s 2 L2 c 2 0 0 0 P2 x s P2 y c 0 P3 x s P3 y c 0 0 0 L3s 3 L3c 3 0 Alternate analytical 3RPR manipulator forward pose solution Figure: Branch AC1C2 A2 is a 4bar mechanism with input angle 1 and output angle 2. or 6 of these will be real. There are always six solutions. . and it can trace out a coupler curve for point C3 in the plane. but 0. this coupler curve is a tricircular sextic. centered at A3) with the coupler curve. depending on the commanded configuration and robot geometry. 2.
3RPR Example For planar parallel robots we can generally find the reachable workspace using a geometric method. There are two workspaces to consider (the same as for serial robots in Section 2. the dexterous workspace is almost always null since the rotation capability is never full for all three Euler angles.10 of this 429/529 Supplement): 1) reachable workspace is the volume in 3D space reachable by the endeffector in any orientation 2) dexterous workspace is a subset of the reachable workspace because it is the volume in 3D space reachable by the endeffector in all orientations. Then stack these up and intersect them to find the dexterous workspace. .4 Parallel Manipulator Workspace Since reduced workspace of parallel robots (when compared to serial robots) is their chief disadvantage.105 6. it becomes very important to determine the workspace of parallel robots and maximize it through design. therefore we usually define a reduced dexterous workspace wherein all Euler angles can reach 30 or some other userdefinable range. For parallel robots. it is most convenient to numerically or geometrically generate in MATLAB the reachable workspace for different values (endeffector orientation) within the desired limits. figuring out what the endeffector can reach guided by each leg on its own. defined for a reduced desired rotational range LIMIT . and then intersecting the results: For determination of the dexterous workspace.
Figure: Vector loopclosure equation using phasors: XY component equations: Angle equation: The velocity equations for one RPR leg are obtained from the first time derivatives of the XY and angle equations: . Both velocity kinematics problems use the same rate equations.5 Planar 3RPR Manipulator Velocity Kinematics First the pose configuration variables must all be known.3 involved. we will derive these from looking at the three single RPR legs separately (meeting at the endeffector).2 . Then we can define and solve two problems: Forward velocity kinematics (for simulation) Given: Find: Inverse velocity kinematics (for resolvedrate control) Given: Find: In both cases we will have intermediate passive joint rate unknowns 1 .106 6.
3 .107 These equations are written in matrixvector form to yield the RPR leg Jacobian matrix: Written in compact notation: X x y z is the same for all three legs (the Cartesian endeffector rates). i 1. No inversion is required and so the Inverse Velocity problem is never singular. Let us now find the overall robot Jacobian matrix. using only active rates and ignoring the passive rates. Here it is expressed in compact notation: . Invert the leg Jacobian matrix: Invert symbolically: Pull out only the active joint row of this result: Assemble all three active joint rows into the overall robot Jacobian matrix: Note: this above equation solves the Inverse Velocity Problem. i includes one active T and two passive joint rates for each of the three RPR legs. 2.
i for use in velocity equations Figure: 1 1 1 2 2 2 120 3 3 270 The second two relationships assume 1 2 3 30 (i. . there is an equilateral endeffector triangle).e. it is the Forward Velocity Problem that is subject to singularities for parallel robots.108 Forward Velocity Problem Compact notation (solve numerically from the Inverse Velocity Solution): Ironically.
sym('LH').109 Example Symbolic MATLAB Code Here is the MATLAB code that was used to symbolically invert the RPR leg Jacobian matrix as presented earlier. Ident2 = simple(J * Jinvsimp). % L LH t1 b1 c1 s1 cp sp % j11 j21 j13 j23 Declare symbolic variables sym('L'). sym('sin(t1+b1)'). sym('t1'). . dynamics. % Check Ident1 = simple(Jinvsimp * J). % Invert Jinv = inv(J). sym('cos(t1+b1)'). Symbolic computing has a lot of power in robot kinematics. sym('cos(t1)'). = L*c1+LH*cp. % % % Symbolic MATLAB code to invert the RPR leg Jacobian clear. = = = = = = = = Jacobian elements = L*s1LH*sp. sym('sin(t1)'). sym('b1'). and control.j21 s1 j23.1 0 1]). % Jacobian matrix J = sym([j11 c1 j13. clc. = LH*cp. % Simplify Jinvsimp = simple(Jinv). = LH*sp.
This action might not be possible to undo. Are you sure you want to continue?