1

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

CHAPTER1.INTRODUCTION............................................................................................................................................3
1.3 MOBILITY..........................................................................................................................................................................3
1.5 MATRICES.........................................................................................................................................................................8
CHAPTER2.SERIALMANIPULATORKINEMATICS...........................................................................................................16
2.4 DENAVIT-HARTENBERG (DH) PARAMETERS ADDITIONAL EXAMPLES......................................................................16
2.5 FORWARD POSE KINEMATICS.......................................................................................................................................25
2.5.1 3D MATLAB Graphics for Animation.................................................................................................................25
2.5.2 Spatial 6-dof 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
CHAPTER3.KINEMATICALLYREDUNDANTROBOTS......................................................................................................64
3.2 INVERSE VELOCITY (RESOLVED-RATE) SOLUTION......................................................................................................64
3.2.1 Pseudoinverse-based..........................................................................................................................................64
3.2.3 Klein and Huangs Algorithm..............................................................................................................................64
3.2.4 Singular Value Decomposition...........................................................................................................................65
3.2.5 Generalized Inverses...........................................................................................................................................67
CHAPTER4.MANIPULATORDYNAMICS........................................................................................................................68
4.1 MASS DISTRIBUTION: INERTIA TENSOR.......................................................................................................................69
4.2 NEWTON-EULER RECURSIVE ALGORITHM....................................................................................................................70
4.3 LAGRANGE-EULER ENERGY METHOD..........................................................................................................................72
4.4 SIMPLE DYNAMICS EXAMPLE........................................................................................................................................73
4.5 STRUCTURE OF MANIPULATOR DYNAMICS EQUATIONS...............................................................................................78
4.6 ROBOT DYNAMICS EXAMPLE.........................................................................................................................................79
CHAPTER5.ROBOTCONTROLARCHITECTURES.............................................................................................................83
5.1 INVERSE POSE CONTROL ARCHITECTURE....................................................................................................................83
5.2 INVERSE VELOCITY (RESOLVED-RATE) CONTROL ARCHITECTURE.............................................................................83
5.3 INVERSE DYNAMICS CONTROL ARCHITECTURE............................................................................................................84
5.4 NASA LANGLEY TELEROBOTICS RESOLVED-RATE CONTROL ARCHITECTURE.........................................................85
5.5 NATURALLY-TRANSITIONING RATE-TO-FORCE CONTROLLER ARCHITECTURE..........................................................86
5.6 SINGLE JOINT CONTROL................................................................................................................................................87
CHAPTER6.PARALLELROBOTS....................................................................................................................................97
6.1 INTRODUCTION...............................................................................................................................................................97
6.2 PLANAR 3-RPR MANIPULATOR POSE KINEMATICS.....................................................................................................99
6.3 NEWTON-RAPHSON METHOD......................................................................................................................................101
6.4 PARALLEL MANIPULATOR WORKSPACE.....................................................................................................................105
6.5 PLANAR 3-RPR MANIPULATOR VELOCITY KINEMATICS............................................................................................106


3

Chapter 1. Introduction

1.3 Mobility

Mobility is the number of degrees-of-freedom (dof) which a robot or mechanism possesses. Structures
have zero (or negative) dof, mechanisms have one dof, and robots have greater than one dof.

Degrees-of-freedom (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 degree-of-
freedom joints. This is also equal to the number of independent motors to control the robot. More
formally, we can also use Kutzbachs 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 one-dof joints
J
2
is the number of two-dof joints

J
1
one-dof joints: revolute, prismatic
J
2
two-dof joints: cam joint, gear joint, slotted-pin 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 3-dof in the plane but the ground link is counted
and hence must be subtracted in the equation. One-dof joints thus remove 2-dof in planar motion and
two-dof joints thus remove 1-dof in planar motion.



4

Planar robot mobility examples:










2R serial robot 3R serial robot 4R serial robot


RRPR serial robot five-bar parallel robot 3-RRR parallel robot

( ) 3 5 1 2(5) 1(0) 2dof M = =

( ) 3 8 1 2(9) 1(0) 3dof M = =



A general planar n-link serial robot has n+1 links (including the fixed ground link), connected by
n active 1-dof 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 one-dof joints
J
2
is the number of two-dof joints
J
3
is the number of three-dof joints
J
4
is the number of four-dof joints
J
5
is the number of five-dof joints

J
1
one-dof joints: revolute, prismatic, screw
J
2
two-dof joints: universal, cylindrical, gear joint
J
3
three-dof joints: spherical
J
4
four-dof joints: spherical in a slot
J
5
five-dof 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 6-dof in 3D but the ground link is counted and
hence must be subtracted in the equation. One-dof joints remove 5-dof, two-dof joints remove 4-dof,
three-dof joints remove 3-dof, 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 n-link serial robot has n+1 links (including the fixed ground link), connected by
n active 1-dof 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. Bobs 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*(N-1) - 2*J1 - 1*J2;


Usage:
mob = dof(4,4,0); % for 4-bar and slider-crank 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 left-hand matrix must equal
the number of rows p in the right-hand 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 left-hand matrix and the
number of columns n of the right-hand 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 right-hand 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 right-hand-side 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 right-hand-side 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 non-zero 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; % matrix-scalar 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 % matrix-matrix 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 user-created variables
whos % user-created 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 m-file (with the back-slash).

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 Denavit-Hartenberg (DH) Parameters additional examples

1. 3-axis 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. 3-axis 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. 3-axis 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. 4-axis 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. 4-axis 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. 6-axis 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. 7-axis 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. 8-axis 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 stick-figure
representation for the laboratory SCARA robot below.

Adept 550 SCARA 4-dof 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 L0-d3];
figure;
plot3(xx1,yy1,zz1,'b',xx2,yy2,zz2,'b',xx3,yy3,zz3,'b',xx4,yy4,zz4,'b');

This will generate the basic stick-figure on the screen; here are some add-ons:
- 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 4-dof 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 real-time animations, though normal MATLAB and certainly MS
Windows are not real-time.

Use of on-screen robot graphics is very helpful in validating that your simulated snapshots and
motion trajectories are working correctly.




27
2.5.2 Spatial 6-dof 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 sum-of-angle 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 sum-of-angles
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 sum-of-angles
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 6-dof robot has any three consecutive joint axes intersecting, there exists a
closed-form (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 sub-problems 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 end-effector 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
Peel-off 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. 8-axis 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 kinematically-redundant 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 1-4 and the wrist joints 5-8 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 6-axis PUMA robot.

Now, we will further simplify by ignoring the wrist joints 6-8 (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 sum-of-angles trig
formulas.)


1
R.L. Williams II, Kinematic Equations for Control of the Redundant Eight-Degree-of-Freedom 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 Half-Angle 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 Half-Angle 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 quadrant-specific 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 (its 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 end-effector 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 all-revolute 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 link-by-link from the base to the end-effector
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 end-effector 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 block-transpose 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 non-linear 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 five-part 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 end-effector 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 end-effector 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 resolved-rate 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 robots end-effector 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 cable-suspended 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 top-view 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 Table-Top Robot Users 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 three-axis
Orthoglide). Imagine how much greater the translational workspace would be if there were only one
supporting serial chain to the end-effector. 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 3-RRR shown generally have small workspaces.

3-RRR Parallel Robot Sample Limited Workspace4F
3


Optimized 3-RRR Parallel Robot Workspace
1



A notable exception to the small workspace
rule of parallel robots is the cable-suspended
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. Kinematically-Redundant Robots

3.2 Inverse Velocity (Resolved-Rate) Solution

3.2.1 Pseudoinverse-based

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 Moore-Penrose pseudoinverse form is subject to singularities. Singular Value
Decomposition (SVD) would ameliorate this problem.


3.2.3 Klein and Huangs 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 Huangs 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 kinematically-redundant robot literature is dedicated to more efficient
redundancy resolution, but I think with todays 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 null-space, 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 semi-definite 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 = (aint
math fun?!?)

Both U and V are column-orthonormal (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 non-zero
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 null-space 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 Moore-Penrose 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 row-rank-deficient). Mathematically, G is
a generalized inverse of matrix A if:

| || || | | | A G A G =

The Moore-Penrose 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 Moore-Penrose 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 straight-forward 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 non-linear. Methods for deriving the dynamic equations of motion:
- Newton-Euler recursion (force balance, including inertial forces with D'Alembert's principle).
- Lagrange-Euler 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 inertially-fixed frame.
The frame of expression {k} needn't be an inertially-fixed 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: m-d
2
(kg-m
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.

Parallel-axis 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 vector-matrix 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 Newton-Euler Recursive Algorithm

This is a recursive approach (Craig, 2005) based on free-body diagrams (FBDs) to determine the
dynamics relationships link-by-link. Used numerically it can calculate the inverse dynamics solution
efficiently.

Free-body diagram of link i:
















f
i
: internal force exerted on link i by link i-1.
n
i
: internal moment exerted on link i by link i-1.


Inertial loads Newton and Euler translational and rotational dynamics equations:







Force balance







Moment balance (about CG
i
) (using D'Alemberts principle, the inertial force is ma).




71
Newton-Euler 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 revolute-joint 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 Lagrange-Euler 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 one-link 1R mechanism by three methods:
1) Sophomore dynamics method FBD, force and moment dynamics balance.
2) Newton-Euler recursion.
3) Lagrange-Euler formulation.



1) Sophomore methods - FBD, force balance Free-Body 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) Newton-Euler 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) Lagrange-Euler 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 Newton-Euler 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 two-link 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
.




Newton-Euler 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 6-axis 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 state-space 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 configuration-space 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 resolved-rate 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 two-dof. The first three plots are
kinematics terms related to the resolved-rate control scheme; they are inputs to the inverse dynamics
problem. The joint torques are calculated by the numerical recursive Newton-Euler inverse dynamics
algorithm. These are the joint torques necessary to move this robots 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 (Resolved-Rate) 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 computed-torque 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 Resolved-Rate Control Architecture





86
5.5 Naturally-Transitioning Rate-to-Force Controller Architecture

Resolved-rate 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 Wright-Patterson AFB6F
4
.



4
R.L. Williams II, J.M. Henry, M.A. Murphy, and D.W. Repperger, 1999, Naturally-Transitioning Rate-to-Force Control in Free and
Constrained Motion, ASME Journal of Dynamic Systems, Measurement, and Control, Trans. ASME, 121(3): 425-432.
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 weve 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 Newton-Euler recursion ala 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 multi-axis
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
armature-controlled DC servomotor driving a single robot joint / gear train / link combination, shown
below. This requires dynamics, but it is not coupled nor non-linear.

Open-Loop 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
Closed-Loop Feedback High-level Control Diagram











High-level Computer Control Diagram











Simplifying Assumptions
- rigid motor, load shafts
- n >> 1 (the large gear ratio n reduces speed and increases torque)


Real-world vs. model characteristics

Real World Our Model (simplified)
non-linear

linearized
multiple-input, multiple output (MIMO)

single-input, single output (SISO)
coupled

decoupled
time-varying 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
armature-controlled 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 Kirchoffs voltage law




(1)






Electromechanical coupling
- The generated motor torque is proportional to the armature current

(2)
- The back-emf voltage is proportional to the motor shaft angular velocity

(3)

(K
T
= K
B
can be shown)



90
Mechanical Model
Eulers Equation (the rotational form of Newtons Second Law):



Free-body 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 second-order ODE in u
M
. Can also be written as first-order 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 8-axis ARMII robot downstairs has n = 200 with harmonic gearing.
This is why we can ignore the time-varying robot load inertia and design decoupled independent linear
joint controllers. The gear-reduced 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
Open-Loop 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
Closed-Loop 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 steady-state 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
- Steady-state 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, non-linearities, 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 Hyper-redundant
Serpentine Arm, Backhoe.







Serial Robot: the joints and links extend from
the base in a serial fashion to the end-effector. The
overall structure is cantilevered which is not good
for resisting end-effector loads. All joints must be
active.
Parallel Robot: the joints and links are arranged
in parallel from the base to the end-effector. There
are multiple load paths from the end-effector 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
- Ground-mounted actuators
- Direct-drive
- 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 3-dof legs8F
5
.


There exists an interesting duality between serial and parallel robots: for parallel robots, the inverse
pose and velocity solutions are generally straight-forward 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 loop-closure equations; phasors; Eulers identity

Phasors a powerful Re-Im manner to represent a vector.




Eulers Identity:




Phasors enable compact notation and are very convenient for time derivatives (velocity analysis).




Example Vector loop-closure equation for the four-bar 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/DAC-3851, Sacramento, CA, September 14-17, 1997.



99
6.2 Planar 3-RPR Manipulator Pose Kinematics

Planar 3-RPR Manipulator Diagram











Inverse Pose Kinematics (for pose control)

Given:

Find:

Vector loop-closure equations







The vector loop-closure equations are rewritten below:







The inverse pose solution is straight-forward:







We can also calculate the intermediate passive joint variables
1 2 3
, , u u u , for use in velocity and dynamics
analyses.



100
3-RPR parallel robot Forward Pose Kinematics (for simulation and sensor-based 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 loop-closure 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 Newton-Raphson numerical iteration technique to solve this Forward Pose
Kinematics Problem. We can directly solve the above six equations for the six unknowns. However, we
dont 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 Newton-Raphson iterative numerical method.



101
6.3 Newton-Raphson Method

The Newton-Raphson 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 Newton-Raphson
method is an extension of Newtons single function/single variable root-finding 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 Newton-Raphson Jacobian Matrix, a multi-dimensional form of the
derivative and a function of X. If o X is small, the higher-order 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
Newton-Raphson 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, user-defined
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 Newton-Raphson 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 Newton-Raphson 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-
velocity-based resolved-rate control.

3-RPR manipulator forward pose kinematics solution

Use the Newton-Raphson 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 Newton-Raphson 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 Newton-Raphson 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 3-RPR manipulator forward pose Newton-Raphson 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 3-RPR manipulator forward pose solution


Figure:














Branch
1 1 2 2
ACC A is a 4-bar mechanism with input angle u
1
and output angle u
2
. With given
lengths L
1
and L
2
, this four-bar mechanism has 1-dof, 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 3-RPR 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 end-effector in any
orientation
2) dexterous workspace is a subset of the reachable workspace because it is the volume in 3D
space reachable by the end-effector 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 user-definable range.

3-RPR Example
For planar parallel robots we can generally find the reachable workspace using a geometric
method, figuring out what the end-effector 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 (end-effector 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 3-RPR 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 resolved-rate 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 end-effector). Figure:






Vector loop-closure 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 matrix-vector 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 end-effector 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 end-effector 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*s1-LH*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);