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

CHAPTER 1.  INTRODUCTION ............................................................................................................................................ 3 
1.3 MOBILITY .......................................................................................................................................................................... 3 
1.5 MATRICES ......................................................................................................................................................................... 8 
CHAPTER 2.  SERIAL MANIPULATOR KINEMATICS ........................................................................................................... 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 
CHAPTER 3.  KINEMATICALLY‐REDUNDANT ROBOTS ...................................................................................................... 64 
3.2 INVERSE VELOCITY (RESOLVED-RATE) SOLUTION ...................................................................................................... 64 
3.2.1 Pseudoinverse-based .......................................................................................................................................... 64 
3.2.3 Klein and Huang’s Algorithm .............................................................................................................................. 64 
3.2.4 Singular Value Decomposition ........................................................................................................................... 65 
3.2.5 Generalized Inverses ........................................................................................................................................... 67 
CHAPTER  4.  MANIPULATOR DYNAMICS ........................................................................................................................ 68 
4.1 MASS DISTRIBUTION: INERTIA TENSOR ....................................................................................................................... 69 
4.2 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 
CHAPTER 5.  ROBOT CONTROL ARCHITECTURES ............................................................................................................. 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 
CHAPTER  6.  PARALLEL ROBOTS .................................................................................................................................... 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 Kutzbach’s mobility equations for planar or spatial robots.

Kutzbach's Planar Mobility Equation

( )
1 2
3 1 2 1 M N J J = ÷ ÷ ÷

Where:
M is the mobility
N is the total number of links, including ground
J
1
is the number of 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. Bob’s atlas of structures, mechanisms, and
robots:

oak.cats.ohiou.edu/~williar4/PDF/MechanismAtlas.pdf



MATLAB function to calculate mobility
% Function for calculating planar mobility
% Dr. Bob, EE/ME 429/529

function M = dof(N,J1,J2)

M = 3*(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 (it’s not the same robot, but it has similar joints
when
3
0 u = ).

These four solution sets occur in a very special arrangement pattern, summarized in the table
below.

i u
1
u
2
u
3
u
4

1 u
1

1
2
u
0 u
4

2 u
1

2
2
u
0 –u
4

3
1
180 u +


2
2
u ÷
0 u
4

4
1
180 u +


1
2
u ÷
0 –u
4


In all numerical examples, you can check the results; plug all solution sets
( )
1 2 4
, , u u u one at a
time into the Forward Pose solution and verify that all sets yield the same, commanded
{ }
5
B
P . You can
also calculate the associated
4
B
R

– all should be different (why?).



37
8R ARMII Robot Translational Inverse Pose Kinematics Example

Given
{ }
5
B
P , calculate
( )
1 2 4
, , , 1, 2, 3, 4
i
i u u u = .

{ }
5
5 5
5
0.6572
0.1159
1.6952
B
x
P y
z
÷ ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= = ÷
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )



Answers (deg):

i u
1
u
2
u
3
u
4

1 10 20 0 30
2 10 46.6 0 -30
3 190 -46.6 0 30
4 190 -20 0 -30


Check all solution sets via Forward Pose Kinematics to ensure all yield the correct, commanded
{ }
5
B
P .




38
2.8 Velocity Kinematics

2.8.1 Velocity Introduction

2.8.1.1 Translational Velocity Example

This example demonstrates the various indices in
( )
k
i
j
V . A is a fixed reference frame.

Given: the absolute velocity of B is 2 @90
in
s

the absolute velocity of C is 4 @45
in
s




From the given information:

( )
0
2
0
A
A
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
;
( )
2.83
2.83
0
A
A
C
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )


From the relative velocity equation:
( ) ( ) ( )
A A A
A A B
C B C
V V V = +
( ) ( ) ( )
2.83
0.83
0
A A A
B A A
C C B
V V V
¦ ¹
¦ ¦
= ÷ =
´ `
¦ ¦
¹ )
; 2.95 @16
in
s





39


Since velocity is a free vector, these same velocity vectors ( , ,
A A B
B C C
V V V ) can be expressed in
frames {B} and {C} by using the rotation matrix:

( ) ( )
k A
i k i
j A j
V R V = (or just look at the components)


( )
2
0
0
B
A
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )

( )
2.83
2.83
0
B
A
C
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )

( )
0.83
2.83
0
B
B
C
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )



( )
1.41
1.41
0
C
A
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )

( )
4
0
0
C
A
C
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )

( )
2.58
1.43
0
C
B
C
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )


There are 27 permutations for A, B, C. So far, we have given 9 combinations. There are two
more types:

1)
( ) ( )
k k
j i
i j
V V = ÷ e.g.
( )
0
2
0
A
B
A
V
¦ ¹
¦ ¦
= ÷
´ `
¦ ¦
¹ )
(9 of these)

2)
( ) { } 0
k
i
i
V = e.g.
( )
0
0
0
A
B
B
V
¦ ¹
¦ ¦
=
´ `
¦ ¦
¹ )
(9 of these)




40
2.8.2 Velocity Equations Derivations

2.8.2.2 Spatial 3P Cartesian manipulator
{ } { }
0 0
X J L =

 
{ } { }
1 2 3
T
L d d d =


Derive
0
J : Partial derivatives definition
| |
i
j
f
J
L
c
c

=




( )
3
0
0
3 2
1
x
y
z
p d
f P p d
p d
¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= = =
´ ` ´ `
¦ ¦ ¦ ¦
¹ ) ¹ )



1
0
x
p
d
c
c
=
2
0
x
p
d
c
c
=
3
1
x
p
d
c
c
=

1
0
y
p
d
c
c
=
2
1
y
p
d
c
c
=
3
0
y
p
d
c
c
=

1
1
z
p
d
c
c
=
2
0
z
p
d
c
c
=
3
0
z
p
d
c
c
=


( ) { }
0
0
3
0 e =

0
0 0 1
0 1 0
1 0 0
J


=





0
1
2
3
0 0 1
0 1 0
1 0 0
x d
y d
z d
¦ ¹
¦ ¹
¦ ¦ ¦ ¦

=
´ ` ´ `

¦ ¦ ¦ ¦

¹ )
¹ )










41
2.8.2.3 Spatial 3R PUMA robot (translational only)

The PUMA translational velocity equations are derived as follows, where W indicates the origin
of the wrist frame {W} (recall the position of the wrist frame origin is a function of only the first three
joint angles ( )
1 2 3
, , u u u :

( ) { }
0
0 1 1 1 2 2 1 23
0
6 1 2 3 0 1 1 1 2 2 1 23
1 2 2 23
, ,
W
x L s L c s L c s
P y L c L s s L s s
z L c L c
u u u
÷ + + ¦ ¹ ¦ ¹
¦ ¦ ¦ ¦
= = + +
´ ` ´ `
¦ ¦ ¦ ¦
+
¹ ) ¹ )


( )
( )
( )
0
0 1 1 1 1 2 1 1 1 2 2 2 1 23 1 2 1 23 2 3
0
0 1 1 1 1 2 1 1 1 2 2 2 1 23 1 2 1 23 2 3
0
1 2 2 2 23 2 3
W
W
W
x L c L s s L c c L s s L c c
y L s L c s L s c L c s L s c
z L s L s
u u u u u u
u u u u u u
u u u
= ÷ ÷ + ÷ + +
= ÷ + + + + +
= ÷ ÷ +
     

     

  



{ } { }
0
0 1 1 1 2 2 1 23 1 1 2 2 1 23 2 1 23 1
0 1 1 1 2 2 1 23 1 1 2 2 1 23 2 1 23 2
1 2 2 23 2 23 3
0
0
0
W
W
x L c L s s L s s L c c L c c L c c
y L s L c s L c s L s c L s c L s c
z L s L s L s
X J
u
u
u
¦ ¹ ÷ ÷ ÷ + ¦ ¹
¦ ¦ ¦ ¦

= ÷ + + +
´ ` ´ `

¦ ¦ ¦ ¦
÷ ÷ ÷
¹ )
¹ )
= O







 


Why is the (3,1) term of the Jacobian matrix zero? (
1
u

cannot affect
0
W
z )




42
2.8.3 Jacobian Matrices

Four ways to derive the Jacobian matrix:

1) Time derivatives and relative angular velocity equation













2) Partial derivatives definition – closely related to the first method



f – vector of m Cartesian functions. This relationship holds only for translational part:





That is, no Cartesian orientation vector exists that we can differentiate to get the angular velocity vector.


43
3) Column as velocity due to joint i
Physical interpretation of the Jacobian matrix: each column i is the Cartesian velocity
vector of the 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 robot’s 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 User’s Guide, 1995.

The next five images represent the reachable, or translational, workspaces of five common
manipulator arrangements (Cartesian, cylindrical, spherical, SCARA, and articulated).


59

Cartesian Manipulator Translational Workspace


Cylindrical Manipulator Translational Workspace



60

Spherical Manipulator Translational Workspace


SCARA Manipulator Translational Workspace



61

Articulated Manipulator Translational Workspace


The previous five images came from:

societyofrobots.com/robot_arm_tutorial.shtml



62
The figure below presents the translational workspace for a parallel robot (the 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 Huang’s Algorithm

This methods yields the same results as the pseudoinverse with gradient projection into the null-
space, but it is more efficient (less computations). Klein and Huang’s algorithm accomplishes the
particular and homogeneous solutions at the same time.

| || |
( )
{ } { } | | ( ) { }
T
J J w X k J H = ÷ V O

;

solve {w} using Gaussian elimination, then:

{ } | | { } ( ) { }
T
J w k H O = + V O

.

So much of the existing kinematically-redundant robot literature is dedicated to more efficient
redundancy resolution, but I think with today’s processors, this is no longer a problem.




65
3.2.4 Singular Value Decomposition

Singular Value Decomposition (SVD) yields the same results as the pseudoinverse with gradient
projection into the 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 = (ain’t
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'Alembert’s 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 robot’s inertia in the commanded manner.
Notice from the joint angles, joint rates, joint accelerations, and joint torques plots that the robot is
approaching the u
2
= 0 singularity towards the end of this simulated motion.



82
Numerical Dynamics Example: Plots






Dynamics Results Ignoring Gravity Dynamics Results Including Gravity


0 0.2 0.4 0.6 0.8 1
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Cartesian Pose
time (sec)
x

(
r
)
,

y

(
g
)
,

o

(
b
)


(
m
,
r
a
d
)
0 0.2 0.4 0.6 0.8 1
0
10
20
30
40
50
60
70
80
90
Joint Angles
time (sec)
u
1

(
r
)
,

u
2

(
g
)

(
d
e
g
)
0 0.2 0.4 0.6 0.8 1
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
1
1.5
Joint Rates
time (sec)
j
o
i
n
t

1

(
r
)
,

j
o
i
n
t

2

(
g
)

(
r
a
d
/
s
)
0 0.2 0.4 0.6 0.8 1
-20
-15
-10
-5
0
5
10
Joint Accelerations
time (sec)
j
o
i
n
t

1

(
r
)
,

j
o
i
n
t

2

(
g
)

(
r
a
d
/
s
2
)
0 0.2 0.4 0.6 0.8 1
0
10
20
30
40
50
60
70
80
Joint Torques
time (sec)
t

(
1
-
r
e
d
;

2
-
g
r
e
e
n
)

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

(
1
-
r
e
d
;

2
-
g
r
e
e
n
)

(
N
m
)


83
Chapter 5. Robot Control Architectures

With the robotic kinematics and dynamics we have learned and simulated, we can simulate robot
control using various popular robot control architectures.

5.1 Inverse Pose Control Architecture

















5.2 Inverse Velocity (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 we’ve considered requires linearized independent (but
simultaneous) single joint angle control, presented in this section.

Manipulator dynamics is extremely complicated considering the number of terms. We can easily
use symbolic MATLAB to crank out the terms but they go on for pages and their structure is difficult to
understand. In this case, numerical Newton-Euler recursion a’la Craig (2005, Chapter 6) is useful to get
around the need for analytical expressions.

How is manipulator dynamics done in industry for control purposes? Answer: in almost all cases
it is ignored. How is this possible? (Large gear ratios tend to decouple the dynamic coupling in motion
of one link on its neighbors – we will see this in modeling soon.) The vast majority of 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 Kirchoff’s 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
Euler’s Equation (the rotational form of Newton’s 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; Euler’s identity

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




Euler’s 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
don’t always need the intermediate variables
1 2 3
, , u u u (also, we can calculate these angles later, using
Inverse Pose Kinematics, if required for velocity, dynamics, or computer simulation). So, to simplify
the Forward Pose Kinematics Problem, square and add each XY equation pair (for all three legs) to
eliminate the intermediate variables
1 2 3
, , u u u . Then we will have three equations to solve for the three
primary unknowns , , x y o :








Bring
2
i
L to the other side and solve via the 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 Newton’s 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);

2

EE/ME 429/529 Supplement Table of Contents
CHAPTER 1.  INTRODUCTION  ........................................................................................................................................... 3  . 1.3 MOBILITY .......................................................................................................................................................................... 3  1.5 MATRICES  ........................................................................................................................................................................ 8  . CHAPTER 2.  SERIAL MANIPULATOR KINEMATICS ........................................................................................................... 16  2.4 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  CHAPTER 3.  KINEMATICALLY‐REDUNDANT ROBOTS ...................................................................................................... 64  3.2 INVERSE VELOCITY (RESOLVED-RATE) SOLUTION ...................................................................................................... 64  3.2.1 Pseudoinverse-based .......................................................................................................................................... 64  3.2.3 Klein and Huang’s Algorithm .............................................................................................................................. 64  3.2.4 Singular Value Decomposition ........................................................................................................................... 65  3.2.5 Generalized Inverses ........................................................................................................................................... 67  CHAPTER  4.  MANIPULATOR DYNAMICS ........................................................................................................................ 68  4.1 4.2 4.3 4.4 4.5 4.6 MASS DISTRIBUTION: INERTIA TENSOR ....................................................................................................................... 69  NEWTON-EULER RECURSIVE ALGORITHM .................................................................................................................... 70  LAGRANGE-EULER ENERGY METHOD .......................................................................................................................... 72  SIMPLE DYNAMICS EXAMPLE ........................................................................................................................................ 73  STRUCTURE OF MANIPULATOR DYNAMICS EQUATIONS ............................................................................................... 78  ROBOT DYNAMICS EXAMPLE ......................................................................................................................................... 79 

CHAPTER 5.  ROBOT CONTROL ARCHITECTURES ............................................................................................................. 83  5.1 5.2 5.3 5.4 5.5 5.6 INVERSE POSE CONTROL ARCHITECTURE .................................................................................................................... 83  INVERSE VELOCITY (RESOLVED-RATE) CONTROL ARCHITECTURE ............................................................................. 83  INVERSE DYNAMICS CONTROL ARCHITECTURE ............................................................................................................ 84  NASA LANGLEY TELEROBOTICS RESOLVED-RATE CONTROL ARCHITECTURE ......................................................... 85  NATURALLY-TRANSITIONING RATE-TO-FORCE CONTROLLER ARCHITECTURE .......................................................... 86  SINGLE JOINT CONTROL ................................................................................................................................................ 87 

CHAPTER  6.  PARALLEL ROBOTS .................................................................................................................................... 97  6.1 6.2 6.3 6.4 6.5 INTRODUCTION ............................................................................................................................................................... 97  PLANAR 3-RPR MANIPULATOR POSE KINEMATICS ..................................................................................................... 99  NEWTON-RAPHSON METHOD ...................................................................................................................................... 101  PARALLEL MANIPULATOR WORKSPACE ..................................................................................................................... 105  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-offreedom joints. This is also equal to the number of independent motors to control the robot. More formally, we can also use Kutzbach’s mobility equations for planar or spatial robots. Kutzbach's Planar Mobility Equation

M  3  N  1  2 J1  1J 2
Where: M is the mobility N is the total number of links, including ground J1 is the number of one-dof joints J2 is the number of two-dof joints J1 – one-dof joints: J2 – two-dof joints: revolute, prismatic cam joint, gear joint, slotted-pin joint

revolute joint R – pin joint, turning pair

prismatic joint P – sliding pair

In general J1 R and P joints are used much more extensively in practical robots than J2 joints. In Kutzbach's planar mobility equation, links give degrees of freedom and joints constrain or remove degrees of freedom. Each moving link has 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.

but we can still count the required motors. connected by n active 1-dof joints: M  3  n  1  1  2n  1 0   3n  2n  n dof This agrees with the previous statement. that we can just count the number of active joints to find the mobility.4 Planar robot mobility examples: 2R serial robot 3R serial robot 4R serial robot RRPR serial robot five-bar parallel robot 3-RRR parallel robot M  3  5  1  2(5)  1(0)  2dof M  3  8  1  2(9)  1(0)  3dof A general planar n-link serial robot has n+1 links (including the fixed ground link). For parallel robots this does not apply. .

5 Kutzbach's Spatial Mobility Equation M  6  N  1  5 J1  4 J 2  3J 3  2 J 4  1J 5 Where: M is the mobility N is the total number of links. One-dof joints remove 5-dof. Each moving link has 6-dof in 3D – but the ground link is counted and hence must be subtracted in the equation. prismatic. . cylindrical. gear joint spherical spherical in a slot spatial cam universal joint U spherical joint S In general J1 R and P. two-dof joints remove 4-dof. and so on. links give degrees of freedom and joints constrain or remove degrees of freedom. In Kutzbach's spatial mobility equation. in spatial motion. three-dof joints remove 3-dof. and J3 S joints are used much more extensively in practical robots than the other named joints. J2 U. screw universal. including ground J1 is the number of one-dof joints J2 is the number of two-dof joints J3 is the number of three-dof joints J4 is the number of four-dof joints J5 is the number of five-dof joints J1 – one-dof joints: J2 – two-dof joints: J3 – three-dof joints: J4 – four-dof joints: J5 – five-dof joints: revolute.

6 Spatial robot mobility examples: 5R serial robot 6R PUMA serial robot 7R serial FTS robot RRPR Adept SCARA robot 8R serial ARMII robot Stewart platform .

1*J2.ohiou.edu/~williar4/PDF/MechanismAtlas. please see Dr. Bob’s atlas of structures. Result: mob = 1 % for 4-bar and slider-crank mechanisms .0).7 Human arm A general spatial n-link serial robot has n+1 links (including the fixed ground link).4. Usage: mob = dof(4.2*J1 . mechanisms. connected by n active 1-dof joints: M  6  n  1  1  5n  4  0   3  0   2  0   1 0   6n  5n  n dof This agrees with the previous statement. For some more unsolved mobility examples. For parallel robots this does not apply.pdf MATLAB function to calculate mobility % Function for calculating planar mobility % Dr. and robots: oak. EE/ME 429/529 function M = dof(N. that we can just count the number of active joints to find the mobility. Bob.J1.J2) M = 3*(N-1) . but we can still count the required motors.cats.

velocity. and dynamics. where m is the number of rows and n in the number of columns.8 1.5 Matrices Matrix: an m x n array of numbers. In robotics matrices are used a lot.  a11 a12  a1n  a   21 a22  a2 n   A           am1 am 2  amn  Matrices can be used to simplify and standardize the solution of n linear equations in n unknowns (where m = n). Special Matrices square matrix (m = n = 3)  a11 a12  A  a21 a22   a31 a32   a11 0  A   0 a22  0 0  1 0 0   I 3   0 1 0   0 0 1     a11 a21  A  a12 a22   a13 a23  T a13  a23   a33   0 0  a33   diagonal matrix identity matrix transpose matrix a31  a32   a33   a12 a22 a23 (switch rows and columns) symmetric matrix  a11  A   A  a12   a13  T a13  a23   a33   column vector (3x1 matrix)  x1   X    x2       x3  row vector (1x3 matrix) X  T   x1 x2 x3  . in pose (position and orientation) description. acceleration.

e. The size of the resulting matrix [C] is from the number of rows m of the left-hand matrix and the number of columns n of the right-hand matrix: mxn. 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. g c     ag  bh  ci  h  f     dg  eh  fi  i      (2x1)  (2x3)(3x1) a b C    d e  Example: Note the inner indices (p = 3) must match. the matrix multiplication is undefined and cannot be done. along the rows of the lefthand matrix and down the columns of the right-hand matrix (use your index fingers from the left and right hands). Multiplication proceeds by multiplying like terms and adding them. mxn = 2x1. in a matrix multiplication product.  A  B    B  A  The row and column indices must line up as follows: C    A B  (mxn)  (mxp)( pxn) That is.9 Matrix Addition add like terms and keep the results in place a b   e c d    g    f  a  e b  f   h  c  g d  h     Matrix Multiplication with Scalar multiply each term and keep the results in place  a b   ka kb  k    c d   kc kd  Matrix Multiplication C    A B  In general. as stated above. i. and the dimension of the result is dictated by the outer indices. .

10

Matrix Multiplication Examples

1 2 3  A  4 5 6  

7 8   B   9 8    7 6  

C    A B 
7 1 2 3    9  4 5 6  7   7  18  21   28  45  42 8  8 6  8  16  18   46 42   32  40  36  115 108   
(2x 2)  (2x3)(3x 2)

 D    B  A
7 8  1 2 3  9 8     4 5 6  7 6      7  32 14  40 21  48  39 54 69    9  32 18  40 27  48   41 58 75      7  24 14  30 21  36   31 44 57     

(3x3)  (3x 2)(2x3)

11

Matrix Inversion Since we cannot divide by a matrix, we multiply by the matrix inverse instead. C    A B  , solve for [B]: Given

C    A B  

 A C    A  A B    I  B   B
1 1

  B    A

1

C 

Matrix [A] must be square (m = n) to invert.

 A A

1

  A

1

 A   I 

where [I] is the identity matrix, the matrix 1 (ones on the diagonal and zeros everywhere else). To calculate the matrix inverse:

 A
where: A

1

adjoint( A)
A

determinant of [A]

adjoint( A)  cofactor( A)
cofactor(A) minor

T

aij  ( 1)i  j M ij
minor Mij is the determinant of the submatrix with row i and column j removed.

For another example, given C    A  B  , solve for [A]

C    A B  

C  B 

1

  A B  B    A I    A

1

  A  C  B 

1

In general the order of matrix multiplication and inversion is crucial and cannot be changed.

12

System of Linear Equations We can solve n linear equations in n unknowns with the help of a matrix. For n = 3:
a11 x1  a12 x2  a13 x3  b1 a21 x1  a22 x2  a23 x3  b2 a31 x1  a32 x2  a33 x3  b3

Where aij are the nine known numerical equation coefficients, xi are the three unknowns, and bi are the three known right-hand-side terms. Using matrix multiplication backwards, this is written as  A x  b :

 a11 a  21  a31 

a12 a22 a32

a13   x1   b1      a23   x2   b2   a33   x3  b3     

where:

 a11 a12  A  a21 a22   a31 a32   x1   x   x2    x   3  b1  b  b2    b   3

a13  a23   a33  

is the matrix of known numerical coefficients;

is the vector of unknowns to be solved; and

is the vector of known numerical right-hand-side terms.

There is a unique solution  x   A b only if [A] has full rank. If not, A  0 (the determinant of coefficient matrix [A] is zero) and the inverse of matrix [A] is undefined (since it would require dividing by zero; in this case the rank is not full, it is less than 3, which means not all rows/columns of [A] are linearly independent). Actually, Gaussian Elimination is more robust and more computationally efficient than matrix inversion to solve the problem  A x  b for {x}.
1

x1  2 x2  5 6 x1  4 x2  14  1 2   x1   5   6 4   x   14    2    A   1 2   6 4 1  x    x1    x2  b   5  14  x   A b A  1  4   2  6   8 The determinant of [A] is non-zero so there is a unique solution.  4 2   1/ 2 1/ 4   6 1    3 / 4 1/ 8      A 1  1 A check:  A A   A 1 1  A   I 2    1 0   0 1   x1   1/ 2 1/ 4   5  1         x2   3 / 4 1/ 8 14  2  answer. Check this solution: substitute the answer {x} into the original equations  A x  b and make sure the required original {b} results: 1 2 1  1(1)  2(2)   5  6 4 2  6(1)  4(2)   14        checks out .13 Matrix Example – solve linear equations Solution of 2x2 coupled linear equations.

Add = A3 + A4 k = 10.14 Same Matrix Examples in MATLAB %------------------------------% Matrices. A7 = A5*A6 A8 = A6*A5 A9 b dA9 invA9 x x1 x2 Check xG who whos = = = = = = = = = [1 2.matrix examples % Dr.m . Gaussian Elimination is more efficient (more on this in the dynamics notes later) and more robust numerically.14]. A9*x A9\b % matrix-scalar multiplication % matrix transpose (swap rows. Gaussian elimination implementation is given in the third to the last line of the above m-file (with the back-slash). [5. % 3x3 diagonal matrix % 3x3 identity matrix % matrix addition A1 = diag([1 2 3]) A2 = eye(3) A3 = [1 2.cols) % define two matrices % matrix-matrix multiplication % % % % % % matrix for linear equations solution define RHS vector calculate determinant of A calculate the inverse of A solve linear equations extract answers % check answer – should be b % Gaussian elimination is more efficient % display the user-created variables % user-created variables with dimensions The first solution of the linear equations above uses the matrix inverse – actually. . clc. x(2). Bob. to solve linear equations. MultSca = k*A3 Trans = A4' A5 = [1 2 3. ME 301 %------------------------------clear.4 5 6].9 8. A4 = [5 6. det(A9) inv(A9) invA9*b x(1).3 4].6 4]. A6 = [7 8.7 8].7 6].

1250 Check = xG = 1 2 .5000 0.2500 -0.m A1 = 1 0 0 A2 = 1 0 0 Add = 6 10 0 1 0 0 0 1 0 2 0 0 0 3 8 12 MultSca = 10 20 30 40 Trans = 5 6 A7 = 46 115 A8 = 39 41 31 dA9 = -8 invA9 = -0.15 Output of Matrices.7500 x = 1 2 5 14 7 8 42 108 54 58 44 69 75 57 0.

i 1 2 3  i 1 0  90   90  ai 1 0 0 0 di d1 d2 d3 i 0  90  0 . Serial Manipulator Kinematics 2. 3-axis Spatial 3P Cartesian Robot Workspace shown in hatched lines.4 Denavit-Hartenberg (DH) Parameters – additional examples 1.16 Chapter 2.

17

2. 3-axis Spatial PRP Cylindrical Robot (incomplete) Workspace shown in hatched lines.

i
1 2 3

i 1

ai 1

di

i

18

3. 3-axis Spatial RRP Spherical Robot (incomplete) Workspace shown in hatched lines.

i
1 2 3

i 1

ai 1

di

i

19

4. 4-axis Spatial 4R Articulated Robot (incomplete) Workspace shown in hatched lines.

i
1 2 3 4

 i 1

ai 1

di

i

20 5. 4-axis Spatial RRPR SCARA Robot (incomplete) Workspace shown in hatched lines. i 1 2 3 4  i 1 ai 1 di i .

6-axis Spatial 6R Fanuc S10 Robot (incomplete) i 1 2 3 4 5 6  i 1 ai 1 di i .21 6.

6R Fanuc S10 Robot (continued) – coordinate frames .6.

23 7. 7-axis Spatial 7R NASA Flight Telerobotic Servicer (FTS) Robot (incomplete) i 1 2 3 4 5 6 7  i 1 ai 1 di i .

24 8. 8-axis Spatial 8R NASA AAI ARMII Robot (incomplete) i 1 2 3 4 5 6 7 8  i 1 ai 1 di i .

xx2 = [0 L1*c1]. Use grid to clarify the resulting image.5 Forward Pose Kinematics 2. for either snapshots or motion animation following Forward Pose Kinematics. zz2 = [L0 L0].zz4.xx2.zz2. Adept 550 SCARA 4-dof 3D Robot MATLAB Code: xx1 = [0 0].zz3. Use 'linewidth'.       This will generate the basic stick-figure on the screen. {0}. plot3(xx1. . xx4 = [L1*c1+L2*c12 L1*c1+L2*c12]. Draw thinner unit vectors to represent the origins and directions of {B}.xx4.'b'). zz1 = [0 L0]. zz4 = [L0 L0-d3]. Specifically we will show how to draw a 3D stick-figure representation for the laboratory SCARA robot below.'b'.EL). Use X Y Z axis labels.yy2. % link from end of L2 to {4} yy4 = [L1*s1+L2*s12 L1*s1+L2*s12]. zz3 = [L0 L0]. Use subplot for 4 views (3 planar projections and orthographic view) – see view(AZ. or other robotics calculations to follow later in the 429/529 NotesBook.25 2.zz1.1 3D MATLAB Graphics for Animation This section presents some basic MATLAB code to draw a 3D robot to the screen. and {4}. xx3 = [L1*c1 L1*c1+L2*c12]. figure.4 to make thicker lines. % link from {B} to {0} yy1 = [0 0].yy4.yy1.'b'.xx3. % link from {0} to {2} yy2 = [0 L1*s1].'b'. % link from {2} to end of L2 yy3 = [L1*s1 L1*s1+L2*s12].5.yy3. here are some add-ons: Be sure to use axis(‘equal’) or axis(‘square’) with appropriate axis limits.

Otherwise the animation will flash by so fast you cannot see it. If you want to use this SCARA graphics in a motion simulation. you must use pause(dt) within your motion loop. {0}.  The thicker robot lines are drawn in blue ('b' in the code) and the {B}. dt can be designed to approximately obtain real-time animations.300 35  . and {4} axes use the convention r g b for the X Y Z unit vectors.26 Here is what the simplified 3D 4-dof Adept 550 SCARA robot could look like in MATLAB: This was generated using snapshot FPK for the inputs 1  2 d 3  4   15  25 0. though normal MATLAB and certainly MS Windows are not real-time. . Use of on-screen robot graphics is very helpful in validating that your simulated snapshots and motion trajectories are working correctly.

 4 .2 Spatial 6-dof 6R PUMA Robot FPK Example Spatial 6R PUMA Robot Forward Pose Kinematics Symbolic Derivations B Given 1 .  6  . 6    i 1 2 3 4 5 6  i 1 0  90  ai 1 0 0 L1 0 0 0 di 0 L0 0 L2 0 0 i 1 2  90 3  90 4 5 6  90 0 90   90  90  . 3 .  2 . 5 .5.27 2. calculate  0T  and  HT  .

use the trigonometric sum-of-angle formulas (see the 429/529 NotesBook) to simplify.  3   3  1  3  Take advantage of parallel Z axes in consecutive frames {2} and {3} – use trigonometric sum-of-angles formulas.  6   6  3 6   0T 1 .  3    3T  4 .  2 .e.28 Substitute each row of the DH parameters table into the equation for  i 1iT  .  3     0T 1    1T  2 . Evaluate the angle offsets   also.  2 . 3     1 23    s 23   0  s1 0 0   c23 c1 0 0   0  0 1 0    s23  0 0 1  0 c1s23  s1s23 c23 0  s1 c1 0 0  s23 0 c23 0 0 L1s2  1 L0   0 L1c2   0 1   L0 s1  L1c1s2  L0 c1  L1s1s2    L1c2  1  c23  cos  2   3  s23  sin  2   3  where we used the abbreviations: . 3     1   0  0 c1c23 s c 0  3T 1 .  2 .  2 . i.  5 . c1 s 0  3T 1 .  si  ci  s c ci c i 1  i 1iT    i i 1    s s ci s i 1 i i 1  0  0 c1 s  0T    1  1 0  0 c4 0 3  4T      s 4  0  s1 0 0  c1 0 0   0 1 0  0 0 1  s4 0 c4 0 0 0  1  L2   0 0   0 1   s2 0 1  2T      c 2  0   c5  0 4  5T      s 5   0 0  s i 1 c i 1 0 0 0 1 L0   0 0  0 1 0 1 0 0 0 0  0  1 ai 1   di s i 1   di c i 1   1    s3 c  2T    3 3   0   0   s6  0 5  6T      c 6   0 c3  s3 0 0 c6 0  s6 0 0 L1  0 0  1 0  0 1 0 1 0 0 0 0  0  1 c2 0  s2 0  s5 0 c5 0 1  0T    0T 1    2T  2    2T  3    3T  4    4T  5    5T  6   6  1  3 4 5 6   0T    0T 1 .

3  . 6  LB and LH are constants.g. 3 . due to the spherical wrist (wrist frames {4}. 5 . the symbolic terms would be worse in complexity than  0T  . Therefore. no sum-of-angles formula simplification is possible. There is also a separate base frame {B}. For this robot we need to control the {H} 6  frame. but the rotation matrix  60 R  is a complicated function of all six joint angles. . 6   3 6  Here we will present one aspect of this multiplication. again using  0T    0T   3T  .   Two additional. the MATLAB Symbolic Toolbox).  6     3T  4    4T 5    5T  6   6  4 5 6  There are no consecutive parallel Z axes in frames {4}. B 6  HT    BT  LB    0T 1 . the position vector  0 P6  is a function only of the first three joint angles:  P  . The below overall transform can be evaluated numerically.  4 . different from {0}.  2 . these two matrices can be evaluated and multiplied numerically. and {6} share a common origin). 5 .  6        c c s c s 4 6 4 5 6  0  s4 s6  c4 c5c6  s5c6 c4 s6  s4c5c6 0 c4 s5 c5 s4 s5 0 0   L2   0   1  It is left to the reader to perform the symbolic multiplication  0T    0T   3T  . Alternatively.    0 6 1 2 3 0  L0 s1  L1c1s2  L2 c1s23    P4    T   P4    L0 c1  L1s1s2  L2 s1s23      L1c2  L2 c23   0 3 3 Where constant vector  P   0 3 4  L2 0 . 2 . fixed transforms The basic Forward Pose Kinematics result is  0T  . {5}.29  3T  4 . the result is complicated 6   3 6  and can easily be found by using symbolic math on the computer (e.   s4c6  c4 c5 s6   s5 s6 3  6T  4 . The overall Forward Pose Kinematics Homogeneous Transformation Matrix is given conceptually below. and {6}. In this way the PUMA FPK symbolic expressions are T partially decoupled: the position vector  P 0 6 is only a function of 1 .  6    HT  LH     0 6   1 0 B  0T  LB       0  0 0 1 0 0 0 0 0 0  1 LB   0 1 1 0 6  HT  LH       0  0 0 1 0 0 0 0 0 0  1 LH   0 1 .  5 . {5}. The orientation of {B} is identical to {0} and the orientation of {H} is identical to {6} for all motion.

964    0 0 1   0  0. PUMA Robot FPK Examples Given fixed robot lengths LB  1.023 0. 60  .636 0.  2 .634 0.771 1.015 0.634  B   HT       0. since there 0    is no variable associated with these fixed homogeneous transformation matrices.030 0.638 0. 40 .322 0.915  0.715 0.744   0.999 0.036 2. 40 .358   0.899 2.163    0 0 1   0 B 2) Given    60 .181   0 0 1   0 0.636 0. 20 .  4 . calculate  0T  and  HT  .030 0.015 0.076  0.437 0.771 1.296 1. B 1) Given   10 .2.036 3.5 (m). 10  .0.184  0   6T       0.023 0. Instead. they are determined by inspection.  3    3T  4 .634 0. L2  1.  2 . calculate  0T  and  HT  .637 0.771 0.  6   6  3 6  6 B  HT    BT  LB    0T 1 .30 6 Note that  BT  LB   and  HT  LH   are not evaluated by any row in the DH parameter table. using the rotation matrix and position vector components of the basic homogeneous transformation matrix definition.008 0. 20 . 6     0T    0T 1 .  3 . L1  1.899 2.816    0 0 1   0 . LH  0. L0  0.771 0. 6     0.638 0.  6    HT  LH     0 6   0. 50 . 50 .3.544    0T     6  0.699 0.  5 . 30 .437 0.637 0.5.296 0.699 0.999 0.008 0.322 1. 30 .  5 .862  B   HT      0.715 0.

 6  .5 . 6   0T   f 1 . 5 .5 .  5 . . We solve for the first three joints first using the position vector input. 4 .3     1  Joints 4.6         T 4 sets of 1 . 6     3 R 1 . there are 8 overall solutions to the inverse position problem for the PUMA. 2 .31 2. the position and orientation sub-problems may be decoupled.  4 . three wrist actuators that rotate about a axes intersecting in a common point.  5 . calculate 4 . 1) Translational equations: Given  P  .  4 .  3 . The majority of industrial robots are in this category. .  6  6     0 R 1 . Some may lie outside joint ranges. calculate 1 . 3 unknowns. Therefore. based on the orientation caused by the first three joints.3 .6 for each.6 values (2 sets). and 6 cannot affect the translation of the wrist origin point. 3 . 6 dependent equations.  2 . In particular. Then we solve for the second three joints next using the given orientation.  4 .6 Inverse Pose Kinematics Decoupled Inverse Pose Kinematics Solution .e. 2 . 3    6 R 1 . 0 0  63 R  4 . 3 independent equations.  2 . The given position vector must point to the wrist point (the shared origin point of three consecutive wrist frames). 5 .2 . calculate  .5 . and now knowing 1 . 2 sets of 4 . In this case.3 . 0 6 1 2 3 values (4 sets).2 .  6   0   6T     6     0 0 0     0 P6 1 . We can always transform any end-effector or other tool vector vector to this origin point. 2) Rotational equations: Given  60 R  . Generally one would choose the closest solution to the previous position. there exists a closed-form (analytical) solution to the inverse position kinematics.2 .   3 independent equations.  3 . PUMA Example (without details): Given  0T  . since there are no more active joints beyond the last wrist joint. 3 unknowns. i. 5.3 .Pieper's Solution Pieper proved that if a 6-dof robot has any three consecutive joint axes intersecting. many robots have a spherical wrist.  2 .

32 Peel-off homogeneous transformations matrices with unknowns to separate variables. and 6 1 . 5. 1  0T    01T 1    2T  2    2T  3    3T  4    4T  5    5T  6   6    3 4 5 6  LHS  0T  is numbers 6  1  0T 1    0T    2T  2    2T 3    3T  4    4T 5    5T  6   1  6   3 4 5 6  1 can solve for 1 and 3  0T  2    0T    3T  4    4T 5    5T  6   3  6  4 5 6  can solve for 2 with 1 and 3 known 1  0T  4    0T    4T 5    5T  6   4  6  5 6  isolate and solve 4.

For inverse pose purposes we will here simplify instead and lock out the redundancy so that m = n = 6. There are some great ways to make use of this redundancy for performance optimization in addition to following commanded Cartesian translational and rotational velocity trajectories. still with multiple solutions (but finite). 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). let us choose 3  5  0 for all motion to accomplish this.33 3. . Then we have a determined Inverse Pose Kinematics problem.

we will further simplify by ignoring the wrist joints 6-8 (5 is already locked to zero) and solve the Inverse Pose Kinematics problem only for the arm joints 1. . Hampton. use symbolic Forward Pose Kinematics):  B  x5  c1  d3 s2  d5 s24       P5    y5    s1  d3 s2  d5 s24   z  d  d c  d c  3 2 5 24   5  B where: c24  cos  2   4  s24  sin  2   4  (Since 3  0 always. July 1992. The full inverse solution is given in Williams 1. See the previous section that explained the Pieper results for the 6-axis PUMA robot. with 3  0 ): This equation yields (the derivation is left to student.L. Now. NASA Technical Memorandum 4377. Williams II. the Z axes of 2 and 4 are always parallel and we used the sum-of-angles trig formulas. calculate 1 . we can only specify three Cartesian objectives. Note that  B P5    B P8  due to the spherical wrist. 4 i . VA. with 3  0 Reduced problem statement: T Given B P5   x5 y5 z5  .34 The Forward Pose Kinematics relationship is: So. 0F Inverse Pose Kinematics Symbolic Solution for Arm Joints Only. NASA Langley Research Center. Kinematic Equations for Control of the Redundant Eight-Degree-of-Freedom Advanced Research Manipulator II. in this case the XYZ location of the origin of {5} with respect to origin of {B} (and expressed in the basis of {B}). The equations to solve for this problem come from the Forward Pose Kinematics relationships for the ARMII robot. the first step should be to simplify the equations as much as possible by calculating the required B  0T  to achieve the commanded  H T  : 8    The problem can be decoupled between the arm joints 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). the translational portion only (further.   That is. with only three active joints. and 4.) 1 R. for all possible solution sets i. 2 .2.

it was for 1). Since 1 is now known (two values).35 Solution Process: 1. we solve for 2 (in that section. Solve for 2 by inverting the original Tangent Half-Angle definition: . we can modify the Y and Z equations: Y  y5 s1 where: Z  z5  d B Isolate the  2   4  terms: Square and add to eliminate 4: The result is one equation in one unknown 2: E cos 2  F sin 2  G  0 E  2Zd3 where: F  2Yd3 G  Y 2  Z 2  d32  d52 We can solve this equation for 2 by using the Tangent Half-Angle Substitution. A ratio of the Y to X equations yields: 1  180 is also a valid solution 2. We presented this back in the Inverse Pose Solution of the planar 3R robot.

commanded B also calculate the associated  4 R  – all should be different (why?). but it has similar joints when 3  0 ).  2  . plug all solution sets 1 . So there are a total of four 1 . you can check the results.    P. there are two values for 2. summarized in the table below. To find 4. There are multiple solutions: two values for 1. for each valid 1 . i 1 2 3 4 1 1 1 2 3 0 0 0 0 4 4 –4 2 1 2 2 1  180 1  180  22  21 4 –4 In all numerical examples.36 Two 2 solutions result from the  in the quadratic formula. plus 3  0 ). 4  solution sets for this reduced problem. We can show this with the PUMA model (it’s not the same robot. there is a unique 4. return to original (arranged) translational equations: d5 s24  Y  d3 s2 d5c24  Z  d3c2 Find the unique 4 for each 2 value (use the quadrant-specific atan2 function in MATLAB): Solutions Summary The solution is now complete for the ARMII robot reduced inverse pose problem (translational joints only. both are correct (there are multiple solutions – elbow up and elbow down). 4  one at a time into the Forward Pose solution and verify that all sets yield the same. These four solution sets occur in a very special arrangement pattern.  2 .  2 . for each 1. B 5 You can .

6572      P5    y5   0. 4 .37 8R ARMII Robot Translational Inverse Pose Kinematics Example Given  P  .6 -46. i  1. 3. 2. B 5 . B 5 1 2 4 i  B  x5  0.6 -20 3 0 0 0 0 4 30 -30 30 -30 Check all solution sets via Forward Pose Kinematics to ensure all yield the correct. calculate  .1159   z   1.6952   5   Answers (deg): i 1 2 3 4 1 10 10 190 190 2 20 46.  .  . commanded  P.

95 in @16 s .8.83   VB   0. Given: the absolute velocity of B is 2 in @90 s the absolute velocity of C is 4 in @ 45 s From the given information: 0  A A   VB  2 .  0    2.1 Velocity Introduction 2.38 2. 0      A 2.1.83    0    From the relative velocity equation: A  A VC   A  A VB   A  B VC  A  B VC   A  A VC   A  A 2.83 .1 Translational Velocity Example This example demonstrates the various indices in k  V .8 Velocity Kinematics 2.8. i j A is a fixed reference frame.83  AVC   2.

B. C.41  AVB   1. There are two more types: 1) k  V V  j k i i j 0   e.83  0    C 1.g.83    VC   2.41    0    C 4   AVC   0   0    C  2.  BVA   2 0   A (9 of these) 2) k  V   0 i i e. we have given 9 combinations. A  B 0    VB   0 0    (9 of these) .58   BVC   1.83    VC   2. these same velocity vectors ( AVB .43    0    There are 27 permutations for A.g. So far.83  0    B  B  0. AVC .39 Since velocity is a free vector. BVC ) can be expressed in frames {B} and {C} by using the rotation matrix: k V  i j k A R A V  i j (or just look at the components) B  A 2   VB   0  0    B  A  2.

40 2.8.2.2 Spatial 3P Cartesian manipulator   X    0 0  J   L  L  d1 d2 d3  T Derive  f  J    i   Lj    0 J : Partial derivatives definition f  0  0  px   d 3      P3    p y   d 2   p  d   z  1  px 0  d1  px 0  d2  px 1  d3  py 0  d1  pz 1  d1 0  py 1  d2  pz 0  d2  py 0  d3  pz 0  d3     0 0 3 0 0 0 1  0  J   0 1 0      1 0 0       x  0 0 1   d1          y    0 1 0  d 2   z  1 0 0   d       3 .2 Velocity Equations Derivations 2.8.

1) term of the Jacobian matrix zero? ( 1 cannot affect 0 zW ) .41 2.3    y    L0 c1  L1s1s2  L2 s1s23      L1c2  L2 c23  z W   0        xW   L0 c11  L1s1s21  L1c1c2 2  L2 s1s231  L2 c1c23  2  3      yW   L0 s11  L1c1s21  L1s1c2 2  L2c1s231  L2 s1c23     zW   L1s2 2  L2 s23  2  3 2 0 0   L1c1c2  L2c1c23 L1s1c2  L2 s1c23  L1s2  L2 s23       3 0  x   L0 c1  L1s1s2  L2 s1s23      y     L0 s1  L1c1s2  L2c1s23 z  0   W   X     0 J      L2c1c23  1    L2 s1c23   2       L2 s23  3  0 W   Why is the (3. 2 . where W indicates the origin of the wrist frame {W} (recall the position of the wrist frame origin is a function of only the first three joint angles 1 .3 Spatial 3R PUMA robot (translational only) The PUMA translational velocity equations are derived as follows.2. 2 .8.3  :  0 0 x  L0 s1  L1c1s2  L2 c1s23      P6 1 .

42 2.3 Jacobian Matrices Four ways to derive the Jacobian matrix: 1) Time derivatives and relative angular velocity equation 2) Partial derivatives definition – closely related to the first method f – vector of m Cartesian functions. . no Cartesian orientation vector exists that we can differentiate to get the angular velocity vector.8. This relationship holds only for translational part: That is.

due to joint i only.  |   k  J    0 X N    1   | a) Revolute Joint Columns k | 0  X | N 2    0    XN   N   |   | k  0  XN  i  k  0V   N i    k    0N   i  ˆ ˆ where k Z i  k R i Z i is the third column of i i N k i R and k  P  i N k i R i P i N where i  P  is the translational i N part of T .43 3) Column as velocity due to joint i Physical interpretation of the Jacobian matrix: each column i is the Cartesian velocity  vector of the end-effector frame with respect to the base frame. for a revolute joint: ˆ  k Z  k  i P    k R i Z i  i PN ˆ i k  J i   i k N    k i    ˆ ˆ    Zi i R Zi          Here is the Jacobian matrix for an all-revolute manipulator: k N   k Z  k 1P  k ˆ k i  k ˆ   ˆ1  N    Z i   PN    Z N   PN      J           k ˆ k ˆ k ˆ       Zi ZN Z1       k k . Here is Jacobian matrix column i. and with  i factored out.

44 b) Prismatic Joint Columns ˆ ˆ where k Z i  k R i Z i is the third column of k R . Introduction to Robotics: Mechanics and Control. NN to get NJ. i  0. for a prismatic joint: ˆ ˆ  k Z   k R iZ  k  J i   i    i i       0   0      4) Velocity recursion a'la Craig 2 1F Add translational and rotational velocities serially link-by-link from the base to the end-effector link. factor out  from N vN . Then. 3rd edition.J.  Use the velocity recursion equations. Pearson Prentice Hall. 2. Craig. 2005. i i Here is Jacobian matrix column i. N  1 . NJ.   2 J. . .1. Upper Saddle River. Revolute: i 1 i 1  vi 1  i 1 i i 1 i  ˆ R ii   i 1 i 1Z i 1 i 1 R  i vi  ii  i Pi 1  Prismatic: i 1 i 1  vi 1  i 1 i i 1 i R ii i 1  ˆ R  i vi  ii  i Pi 1   d i 1 i 1Z i 1 Start with 00  0 v0  0 (if the robot base is fixed).

 0J     f  0  P   p 0 3  px    y 3 3  pi  p d j p   i   i j t j 1  j dt j 1  j i = x. y  px  1  px   2  px  3  py  1 0 0  py   2  py  3 0 0              ˆ           Z 0 0 0 1 2 3 1 2 3 0 3 1 2 3  0J         .45 Derive  0 J  : Partial derivatives definition   For the planar 3R robot.

8.8.8 Velocity Kinematics Examples 2.2 Spatial 3P Cartesian Manipulator Velocity Example a) Forward Velocity Kinematics    0 X    0 J  L   0     x  0 0 1   d1   d3            y   0 1 0  d 2   d 2   z  1 0 0   d   d        3  1 b) Inverse Velocity Kinematics (analytical result) 1   L   0 J   0 X      d1  0 0 1   x          d 2   0 1 0   y    d  1 0 0   z       3  0 0  z     y x   c) Singularity Analysis 0 J  1J  2 J  3 J  1 d) Static Force Analysis no possible singularities  f    0J    T  F 0 0 0  f1  0 0 1   Fx         f 2   0 1 0   Fy    f  1 0 0   F   3    z  Fz     Fy  F   x .8.46 2.

  . But simpler analytical expressions are possible for the Jacobian matrix.9 Jacobian Matrix Expressed in Another Frame Here the Jacobian matrix still relates the end-effector frame velocity with respect to the base frame.   L1s1  L2 s12  J    L1c1  L2c12     1  0  L2 s12 0 L2c12 0  1 1    L2 s2  J    L1  L2 c2     1  1  L2 s2 L2c2 1 0 L2 1 0 0  1  0 0  1   c1 s1 0   R     s1 c1 0       0 0 1   1 0  L1s2  J    L1c2  L2     1  2  c12  R     s12     0  2 0 s12 c12 0 0 0  1   L1s23  L2 s3  J    L1c23  L2 c3     1  3 L2 s3 0 L2c3 0  1 1   c123 s123 0  R     s123 c123 0      0 0 1   3 0  3 J  agrees with that derived in the third method above. by choosing an intermediate frame to express the coordinates of the velocity vectors (different basis of expression).8.47 2. k k 0  v    0 R   0   v         k     0  0 R        k  0 R     J         0  k v  k      J        0  0    J     k  0 R   k k 0 v  0       J       is not dependent on a frame (relative joint rates) k   0 R  0      0J   J        k     0  0 R   Planar 3R Robot The Jacobian was derived in frame {0} (here we go to {3} instead of {H}).

8.8. 0   . For example.10 Cartesian Transformation of Velocities and Wrenches Here we show how to move velocity and wrench vectors (both translational and rotational) from one point to another on a rotating rigid body. We can replace one vector with an equivalent vector acting at a different point. Basic equations: vB  v A   A  A PB B   A reversing: v A  vB  B  A PB  vB  A PB  B  A  B All vectors must be expressed in same frame. choose {A}. For instance.48 2. {B} could be the hand frame {H} where we want motion and {A} would then be the last wrist frame {n} for which the Jacobian matrix is derived.1 Velocity Transformation Here we find the equivalent velocity at {A} corresponding to a given desired motion of {B}.10. A A A A B  0VA    B R   PB   B R    0VB        0      A   0 B   B R   A    0     A  X     A B V T   B  X  A A   B R   A PB   B R          T     A   B R     0  A B V  0  where:  PB    pz    py  A  pz 0 px py    px  is the cross product matrix. to calculate the velocity (or wrench) at the wrist to produce a desired velocity (or wrench) at the hand. 2.

866 0. Given: B  0 1    VB   1  0    B  0 0     B   0    2  0.5 0  0   0   A   0  0 0 0 0.866 0.5 2.5 0.5 0.6   0.5 0.5 0  R    0.5 0  0.5  0.5 0 0 0 1.866 0 2.866 0      0 0 1   A B  A 2.866 0  0   0       0 0 0 0 1  2   2   0 Check: A  B VA  VB    r B ˆ j ˆ 1  i ˆ k  1   0   1   0VA   1   0 0 2  1   6  5         0  3 0 0 0  0   0          .5  0    0 1.5   0 A  A PB   B R    0 0 2.49 Velocity transformation example Find the equivalent velocity at A corresponding to a given motion of B.6           1.5  1   3.866 0  0 0 2.6  0  0 3 0  0 1  0     X      X  A A B V T   B 0 0 1.366  0.866 0.866 0.834  0 0      A 0  0  0   0  0 1 0 3 0  VA  0        0 0 0.5 0.6 1  3.6   PB   1.

2 Pseudostatic Wrench Transformation Here we find the equivalent wrench at {A} corresponding to a given wrench at {B}.  A PB   B R  is switched with 0. i. while in the wrench transformation. For instance.10. A A  B R  0  B  FB   FA          A A  M A    A PB   B R   B R    M B         A W    ATW  W  B  B A   B R  0       T     A A A         PB   B R   B R   A B W A Note:  ATW  is a block-transpose of  ATV  . the rotational term is unchanged.e. choose {A}.50 2. .8.    B  B  There is a duality here: in the velocity transformation. {B} could be the hand frame {H} where we want the wrench to be exerted and {A} would then be the last wrist frame {n} for which the Jacobian matrix is derived. the translational term is unchanged. Basic equations: FA  FB M A  M B  PB  FB A more properly. k FB All vectors must be expressed in the same frame.

5 0  0   0  M A   0  0 0 2.5 0. calculate the vector values on the same rigid link.366       A 0 1 0 0 0  0   0   FA   0        0 1. A W    ATW  W  B  B 0 0 0 0  1  0.866 0. given the end vector values {B}.866 0 0 0 0  1  1. given the inward vector values {A}.6 0. i.866 0  0  0       3 0 0 0 1  0   3   0 Check: FA  FB M A  r  FB Note: the derivations and examples for both velocity and wrench transformations were for the inverse case. We could easily adapt the derivations and transformations to perform the forward calculation. calculate the end vector values on the same rigid link.5  0.866 0.5 0.  A PB   B R      are the same as in the velocity example above. outward towards {B}.   A B A B A R  .e. Given: B 1  FB   1   0    B 0   M B   0      0   P .366  0.51 Wrench transformation example Find the equivalent wrench at A corresponding to a given wrench at B.5 0. i. but inward toward {A}.e. .

2005): d A B  B R Q   BA R BVQ  AB  BAR BQ dt General five-part acceleration equation: Position: A A A P  A PB  B R B P P  AT B P B Velocity: A A   A V  A PB  B R B P  B R B P V  V0  VP    P Acceleration: d A  VB  BA R BVP  AB  BA R B PP  dt A A A A A A A  A AB  B R B AP  A B  B R BVP  A B  B R B PP  A B  B R BVP  A B   A B  B R B PP  A A A A A A A A  A AB  B R B AP  2 A B  B R BVP  A B  B R B PP  A B   A B  B R B PP  A  A0  AP  2  V    P      P  . Transport Theorem (Craig. expressed in the basis (coordinates) of {k}.52 2. with translational and rotational parts. Acceleration Kinematics Analysis is useful for:  Resolved Acceleration Control  Acceleration of any point on manipulator  Moving objects in workspace – smooth trajectory generation  Required for Dynamics Translational Acceleration k i j  A  is the translational acceleration of the origin of frame {j} with respect to reference frame {i}.9 Acceleration Kinematics The acceleration vector is the first time derivative of the velocity vector (and the second time derivative of the position vector). Both Cartesian acceleration and velocity are vectors. It is linear in acceleration terms but non-linear in rate components.

expressed in the basis (coordinates) of {k}. Instead we use relative velocity equations: Rotational Velocity: A A C  AB  B R BC Rotational Acceleration: d d A A A  C   AB  B R BC   A B   B R BC  dt dt A A A B A A B  C   B  B R  C  B  B R C For vectors expressed in the local frame: A  A A C   B R B  A A B   C R C  B A C   B R B  A A B   C R C  B C  Combined Translational and Rotational Acceleration   a   X       A where:       a  0 0 N 0 both a and  are (3x1) vectors 0 N Acceleration Example Planar 2R robot – acceleration of end-effector frame with respect to {0}.53 Rotational Acceleration k i j    is the rotational acceleration of frame {j} with respect to reference frame {i}. also expressed in {0}. 1) Craig (2005) acceleration recursion  1  L1    P2    0  0    2  L2    P3    0  0    c2  R     s2     0  2 1 s2 c2 0 0 0  1  0   a0  0 0    23 R    I 3    0 0    0  0 0    0 0     0  0  0    0 0   1 1   0      1 0   1 1   0     1  0    1 a1  0    0  . No angular orientation exists for which we can take the time derivative to find the angular velocity.

54  0    2 2   0         1 2  0    2 2   0         1 2 2 a2   1 R  11  1P2  11   11  1P2   1a1   2 2   c2 s2 0   L11   L1c21  L1s21      2    a2    s2 c2 0   L11    L1s212  L1c21         0 0 1  0   0    2   3 a3   23 R  2 2  2 P3  22   22  2 P3   2 a2      0    3  3  2 2   0         1 2  L  2  3 a3   L2         L c   L s         L s   L c        2 1 2 2 1 2 1 1 2 1 1 2 2 1 2 1 1 2 1 0    0 0 a3   3 R  3 a3    L s   c  2  L s     c    2       2 12 1 2 12 1 2  1 1 1 1 1   0       2 a3   L1 c11  s112  L2 c12 1   2  s12 1   2  0                           Rewrite for comparison to results in the next section:   L s  L2 s12 a3   1 1  L1c1  L2 c12 0    L2 s12  1    L1c11     L2 c12   2    L1s11        L2 c12 1   2   1         L2 s12 1   2   1   2             The second term above can be written as:      L1c11  L2 c12 1   2        L1s11  L2 s12 1   2        L2 c12 1   2      1      L2 s12 1   2   2      .

55 Acceleration Example (cont. one must account for r .) Planar 2R robot – acceleration of the end-effector frame with respect to {0}.   L1s1  L2 s12  J    L1c1  L2c12     1  0  L2 s12  L2c12   1   d  J   dJ ij   J       dt  dt  dJ ij dt   J ij d1  J ij d 2  J d     ij N 1 dt  2 dt  N dt      L1c1  L2 c12  1  L2 c12 2      0 J      L1s1  L2 s12  1  L2 s12 2    0     L1c11  L2 c12     0 J     L1s11  L2 s12     0    X    0 0 dJ ij dt   J ij  k k 1  k N    L2c121  L2c12 2     L2 s121  L2 s12 2   0     L2 c12 1   2        L2 s12 1 2   0          1 2 1 2       J     0 J          L1s1  L2 s12  Lc L c 0   X    1 1 2 12  1     L1c11  L2 c12  L2 s12       L2 c12   1     L1s11  L2 s12     1  2  0           1 2 1 2    L2 c12 1   2         L2 s12 1   2   1     2   0      This yields the same result as before. . Alternative method: 2) Differentiation of rate equation   X    J    X    J    J     Do in frame {0} – if in frame {2} or {3}. also expressed in {0}.

Dynamics equations –   is required for the Newton/Euler dynamics recursion in this 429/529 NotesBook Supplement. so the analytical approach 1       J  X   J   is better. In this case:     J  1   J       How can we find the time rate of change of the Jacobian matrix  J  ? See the previous page for a   specific example. but acceleration is commanded instead of 1     X   J   and double integrate to get the commanded joint velocity. ii. Forward Acceleration Kinematics Analysis –  X    J     J   predicts the Cartesian    accelerations  X  given the joint rates and accelerations. numerical instability can result. Resolved Acceleration Control – like resolved rate. . Chapter 4. Solve    J         angles. If acceleration is calculated via numerical differentiation.  iii. if inverse dynamics control is being used in the resolved-rate control algorithm   framework.56 Uses for general acceleration equation   X    J     X    J     J         J   X    J     1    i. assume  X  is constant and so  X   0 .        Now.

10 Robot Workspace The workspace (also called work envelope) of a robot manipulator is defined as the volume in space that a robot’s end-effector can reach. Generally large translational and rotational workspaces are desired for a more capable robot. Generally serial robots have a big advantage over parallel robots in terms of larger workspace (exception: parallel cable-suspended robots). The workspace limits are also due to the specific joint limits of each joint. in side (left) and top (right) views. The translational workspace is easier to determine and display.57 2. Workspace is defined for both position (translational workspace) and orientation (rotational workspace). The workspace limits are due to the total reach of the combined robot links (both extended and folded upon each other). The figure below shows the translational workspace of a Cybotech P15 articulated robot. and is the focus of this section. Cybotech P15 Manipulator Translational Workspace .

cylindrical. The next five images represent the reachable. . spherical. therefore a limited dextrous workspace is generally defined (such as all orientations within 30 rotation about all three axes). SCARA. For parallel manipulators the dextrous workspace is often null. to form a volume. The reachable workspace of the Adept 550 SCARA robot is shown below. Adept 550 SCARA Robot Reachable Workspace Adept 550 Table-Top Robot User’s Guide. workspaces of five common manipulator arrangements (Cartesian.58 The robot workspace attainable at any orientation is called the reachable workspace. The robot workspace attainable at all orientations is called the dextrous workspace. This top-view planar workspace shape is extended 200 mm in the vertical direction by the prismatic joint limits on d3 (into the paper). or translational. 1995. The dextrous workspace is a subset of the reachable workspace. and articulated).

59 Cartesian Manipulator Translational Workspace Cylindrical Manipulator Translational Workspace .

60 Spherical Manipulator Translational Workspace SCARA Manipulator Translational Workspace .

61 Articulated Manipulator Translational Workspace The previous five images came from: societyofrobots.shtml .com/robot_arm_tutorial.

than an equivalent serial robot with a larger workspace.html .org/Reviews/Review011. stiffness. Orthoglide Robot Reachable Workspace parallemic. Imagine how much greater the translational workspace would be if there were only one supporting serial chain to the end-effector. However. with lower moving mass. and accuracy. the tradeoff is that the parallel robot achieves much greater accelerations.62 The figure below presents the translational workspace for a parallel robot (the three-axis Orthoglide).

Planar Robotic Mechanisms: Analysis and Configuration Comparison. can be designed with arbitrarilylarge workspaces. 3 . VA. August. 1988.D.63 Again. These devices. parallel robots such as the 3-RRR shown generally have small workspaces.L. Cable suspended robots with workspaces on the order of several kilometers have been proposed. James Albus). R. Virginia Polytechnic Institute and State University. 3-RRR Parallel Robot Sample Limited Workspace 3 4F Optimized 3-RRR Parallel Robot Workspace1 A notable exception to the small workspace rule of parallel robots is the cable-suspended robots. Department of Mechanical Engineering. Dissertation. Blacksburg. such as the NIST RoboCrane (right. Ph. Williams II. developed at NIST by Dr.

Singular Value 3.  J  J  w   X   k  J H   .2 Inverse Velocity (Resolved-Rate) Solution 3.1 Pseudoinverse-based Particular Solution – satisfies primary task (satisfies Cartesian trajectory) Minimum Manipulator Kinetic Energy W    M     .64 Chapter 3. Decomposition (SVD) would ameliorate this problem.3 Klein and Huang’s Algorithm This methods yields the same results as the pseudoinverse with gradient projection into the nullspace.2. but it is more efficient (less computations). but I think with today’s processors. Kinematically-Redundant Robots 3.   P the manipulator inertia tensor       J   X   where:  J    M   J   J  M   J   1 T 1 T  1 This Moore-Penrose pseudoinverse form is subject to singularities. T solve {w} using Gaussian elimination. T So much of the existing kinematically-redundant robot literature is dedicated to more efficient redundancy resolution. . then:     J  w  k H   . Klein and Huang’s algorithm accomplishes the particular and homogeneous solutions at the same time. this is no longer a problem.2.

i.4 Singular Value Decomposition Singular Value Decomposition (SVD) yields the same results as the pseudoinverse with gradient projection into the null-space. V is also roworthonormal.2. where n – m is the degree of redundancy.65 3. as opposed to yielding infinite joint rates at singularities. there will always be n – m zero singular values. U  U    I n  (see the SVD example) T Columns of U corresponding to non-zero w j are an orthonormal basis which spans the range of J. however. Any additional zero singular values correspond to degeneracies in J.e. .: T T V V   V  V    I n  1 is set to zero for w j  0 (ain’t wj U U  T  Im  . For underconstrained systems of equations. Columns of V corresponding to zero wj are an orthonormal basis for the null-space of J. In the above expression. but with singularity robustness. but SVD results are bounded so the motion can drive through singularities. the solution cannot track arbitrary Cartesian trajectories.  J   U W V  J U W V  T mxn m x n column orthogonal n x n positive semi-definite diagonal n x n orthogonal     1  wj  T   U     J   V   diag   wj are the singular values of [J]. If [J] is less than full rank. math fun?!?) Both U and V are column-orthonormal (ignoring the last n – m columns of U).

514 0.514 0.288 0.548 0.430 0   0 0 0  0.866   J   U W V  T 0 0  0.188 0.441 0.381 0  0.903 0.548 0.344  3.118       * In the above.939   0 0 0  0 0   0.420 0  0.732 1.288 0.500   2.366 0.430   1.430 0. the third singular value is zero. so * 1 is set to zero (n – m singular values will always be wj zero).500 0.288 0. This result agrees with  J  from the Moore-Penrose pseudoinverse formula in the 429/529 NotesBook.836  0 0  2.188 0.205 0.467  0.903 0   0.786 0.000  J        0.939         T   U    T  J   V   diag     1  wj 0 0 0.903 1.836 0  J     0    0.786 0.366 1.66 Singular Value Decomposition (SVD) Example J     1.430 0. Note: U U  T  I2  1 0 0  U  U   0 1 0  I3   0 0 0   T V V  T  V  V    I 3  T .344 0.323 0.866 0.903 0.

Mathematically. . overconstrained. G is a generalized inverse of matrix A if:  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. 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.67 3.5 Generalized Inverses A generalized inverse of a matrix gives an answer to the linear problem even when a true matrix inverse does not exist (underconstrained.2. or row-rank-deficient). In addition to the above relationship.

calculate the actuator forces and torques (this linear algebraic solution is much more straight-forward than Forward Dynamics): Given    . It is the study of the relationship between forces/torques and motion. Manipulator Dynamics Kinematics is the study of motion without regard to forces. calculate    . must be moving with respect to an inertially-fixed frame. nonlinear ODEs): Given . Methods for deriving the dynamic equations of motion:  Newton-Euler recursion (force balance.68 Chapter 4. which are highly coupled and non-linear.  . . calculate  (all N x 1 vectors). Kinetics: Translational: Newton's Second Law: Inertial force at center of mass Rotational: Euler's Equation: Inertial moment at center of mass (could be anywhere on body) The kinematics terms.  Lagrange-Euler formulation (energy method). including inertial forces with D'Alembert's principle). . aCi .  (all are N x 1 vectors). Dynamics is composed of kinematics and kinetics. Both problems require the N dynamic equations of motion. Dynamics is the study of motion with regard to forces. Assumptions:  serial robot  rigid links  ignore actuator dynamics  no friction  no joint or link flexibility . i . compute the resulting motion (this requires the solution of highly coupled. b) Inverse Dynamics (control) – given the desired motion. i . a) Forward Dynamics (simulation) – given the actuator forces and torques. one for each link. The frame of expression {k} needn't be an inertially-fixed frame.

units: m-d2 (kg-m2). Here are some example inertia tensor components using the parallel axis theorem: A 2 2 I zz  C I zz  m  xC  yC  A I xy  C I xy  mxC yC Here is the entire inertia tensor expressed in vector-matrix form using the parallel axis theorem: A I  C I  m  PCT PC I 3  PC PCT    . 2) Moments of inertia must be positive.69 4. and the eigenvectors are the principal axes.1 Mass Distribution: Inertia Tensor Spatial generalization of planar scalar moment of inertia. the products of inertia normal to this plane are zero. More interesting facts regarding inertia tensors 1) If two axes of the reference frame form a plane of symmetry for the mass distribution. The invariant eigenvalues of a general AI are the principal moments of inertia. tensor expressed at a given point in the rigid body. the principal axes. relative to frame {A} is: The inertia Mass moments of inertia V I xx    y 2  z 2  dv I xy   xy dv V I yy    x 2  z 2  dv V I zz    x2  y 2  dv V Mass products of inertia I xz   xz dv V I yz   yz dv V Principal moments of inertia A certain orientation of the reference frame {A}. Parallel-axis theorem We can obtain the mass moment of inertia tensor at any point {A} if we know the inertia tensor T at the center of mass {C} (assuming these two frames have the same orientation): PC  xC yC zC  is the vector giving the location of the center of mass {C} from the origin of {A}. products of inertia may be either sign. yields zero products of inertia. 3) The sum of the three moments of inertia are invariant under rotation transformations.

2 Newton-Euler Recursive Algorithm This is a recursive approach (Craig. Inertial loads Newton and Euler translational and rotational dynamics equations: Force balance Moment balance (about CGi) (using D'Alembert’s principle. 2005) based on 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: ni: internal force exerted on link i by link i-1. internal moment exerted on link i by link i-1. the inertial force is –ma).70 4.

from Craig (2005).71 Newton-Euler Recursive Algorithm Summary This methods can be used to find the robot dynamics equations of motion.e. for clarity) Internal forces and moments: f i  f i 1  Fi ni  ni 1  i PCi  Fi  i Pi 1  f i 1  N i Externally applied joint torques:  i  ni  Zi Inclusion of gravity forces: 0 a0  g This is equivalent to a fictitious upward acceleration of 1g of the robot base. . It can also be used to directly solve the inverse dynamics problem numerically. which accounts for the downward acceleration due to gravity (i. The summary of equations below. for clarity) Velocities and accelerations (kinematics):  ˆ i 1  i  i 1Z i 1  ˆ  ˆ  i 1  i  i  i 1Z i 1  i 1Z i 1 ai 1  ai   i  i Pi 1  i  i  i Pi 1  aCi 1  ai 1   i 1  i 1PCi 1  i 1  i 1  i 1PCi 1  Inertial loading (kinetics): Fi 1  mi 1aCi 1 N i 1  C Ii 1  i 1  C I i 1 Inward iteration for kinetics: i: N 1 (without regard for frames of expression. Outward iteration for kinematics: i : 0  N  1 (without regard for frames of expression. this conveniently includes the weight of all links). assume an all revolute-joint manipulator (prismatic joint dynamics have different equations).

not accelerations.  L  k  u  k  . to yield N independent dynamics equations of motion. d  L  L     dt      This expression may be rewritten using L  k  .  u   : d k  k u     dt        . once for each joint variable.3 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.  T M    . The Lagrangian is formed from the kinetic energy k and potential energy u of the robot system.72 4. Perform this equation N times. and actuator torque. Dynamic equations of motion These are found for each active joint from the following expression involving the Lagrangian.    ki i 1   N u     ui i 1 N Note:  1  k  . joint variable. 2   where M    is the manipulator mass matrix.  u   1 1 T mVCiVCi  iT Ci I ii i 2 2 0 ui  uREF  mi g  0 PCi ki     k  .

2) Newton-Euler recursion.73 4. force and moment dynamics balance.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 balance Free-Body Diagram:  FX FY -FX  mg -FY . 3) Lagrange-Euler formulation.FBD. g Y    X L 1) Sophomore methods .

74 1) Sophomore methods – FBD. force balance (cont.) F a) 0 x  maCx L  L  Fx  m    s   2 c  2  2  mL  0 Fx    s   2c 2   F b) 0 y  maCy L L  Fy  mg  m   c   2 s  2 2  L  0  Fy  mg  m  c   2 s 2   mL2 I  I  md  I zz  4 C 2 C c) M z0   0 I    0 mL gc 2  mL gc   I  2 where: 0 .

75 Simple Dynamics Example (cont.) 2) Newton-Euler recursion Outward iteration: i = 0 (the only iteration) 0   0 P  0 1 0   L 2   1 PC 1   0  0      c 1  0 R    s  0  s c 0 0 0  1   I xx C1 I1   0  0  0 I yy 0 0 0  I zz   0 0    0  0  0    1 0 0     0  0  0    0 0    a0   g  to account for gravity   0  a1  01R 0 0  0 P  00   00  0 P   0 a0 1 1  c a1    s   0  s c 0 0  0   gs      0  g    gc   1  0   0        1 L 2    gs  2     L    1 aC1  11  1PC1  11   11  1PC1   1a1   gc    2   0       L 2    gs  2     L    1 F1  m 1aC1  m  gc    2   0       1  0    N1  I1 1  1  I1 1   0  I     zz  C1 1 1 C1 1 .

76 Inward iteration: i = 1 (the only iteration) 0    2 f 2  0  0    0    2 n2  0  0    L 2    gs  2     L    1 1 2 1 f1  2 R f 2  F1  m  gc    2   0       1 n1  21R 2 n2  1PC1  1F1  1P2  21R 2 f 2  1 N1      0  0       1 n1   0  0   mL  L    I     gc      zz   2   2    ˆ   1n1  1Z1     I zz  gc   4  2  Check with the sophomore dynamics method above: L 2  L 2 L      gs  2     2  c  2  s   c  s 0     0 0 1  s c 0 m  gc  L    m   L  2 s  L  c  g      f1  1 R f1       2  2   2  0 1 0    0 0             mL2  mL .

77 3) Lagrange-Euler formulation 0    1 1   0       C1  I xx I1   0  0  0 I yy 0 0 0  I zz   Kinetic and potential energy are scalars invariant with frame of expression – write k in {1} and u in {0}. .  0  L    1 vC1     2   0    1 0   0      1  I1 1   0    0         I zz  C1 1 k 1 mL2  2 1  2   I zz 2 4 2 L   c   0  2  L   L  u  0  m   g    s   mg s 2  0  2     0      1   mL2  L L  k  u   2  I zz    mg s 2  4  2 d  L  L     dt      L  mL2     I zz     4   d   L    mL2     I zz     dt    4    mL2   mL    I zz  gc   4  2  L L  mg c  2 This result agrees with the sophomore and Newton-Euler recursion dynamics methods above to solve this same problem.

   G    N  N mass matrix.78 4.    M    J 1 J  Gx     J T G        X  J     X  J  J . symmetric and positive definite N  1 vector of Cartesian Coriolis and centrifugal terms N  1 vector of gravity terms in Cartesian space Gx    where: M x    J M   J T note: 1    Vx  . symmetric and positive definite N  N  1 Coriolis matrix 2 N  N centrifugal matrix N N  1 vector of gravity terms N  N  1 1 : 2 N 1 :    2 1 1 2    1 3   N 1 N   2  22   N  T T Cartesian State Space Equation   F  M x    X  Vx  .    Gx    M x    V  .   G   Configuration Space Equation       M      B       C     2   G        M   B  C  G            2    N  N mass matrix.   x N  N Cartesian mass matrix.5 Structure of Manipulator Dynamics Equations State Space Equation     M      V  .    J T V  . symmetric and positive definite N  1 vector of Coriolis and centrifugal terms N  1 vector of gravity terms M    V  .

79 4.6 Robot Dynamics Example Here we summarize the dynamics equations of motion for the two-link planar 2R robot when each link is modeled as a homogeneous rectangular solid of dimensions Li. L2 Y0 2 L1 1 X0 Newton-Euler recursion with outward kinematics and inertial calculations. . wi. followed by inward kinetics balances yields:  m2 L1L2 c2 m2 L2    m2 L2    m2 L1L2 s2   2  m2 L2 c12  2 2   2   I zz 2  1   g  1   I zz 2  2   2 4  4  2    2     1   I zz1  I zz 2    2 m1L1 m L2    m L L c m L2   2  m2 L1  m2 L1L2 c2  2 2  1   I zz 2  2 1 2 2  2 2   2 4 4  2 4   mLc   m LL s     m Lc    2 1 2 2   22    m2 L1L2 s2  1 2   1 1 1  m2 L1c1  2 2 12  g 2  2    2 where I zzi  mi 2  Li  hi2  . 12 These dynamics equations of motion are very complicated – imagine how much worse these equations will be for a spatial 6-axis robot such as the PUMA industrial robot. hi of mass mi.

the dynamics equations of motion from the previous page are expressed in configuration-space form below:      M      B       C     2   G        M    . G    same as above  m L L s     B        2 1 2 2  1 2   0   m LL s    2 1 2 2  2 0  1  2  C     2      2     m2 L1L2 s2  2  0   2   .80 State Space Representation For the planar 2R robot.    G         1  2         1   2   m L2 m L2 2 I zz1  I zz 2  1 1  m2 L1  m2 L1L2c2  2 2  4 4 M     2 m LLc m L  I zz 2  2 1 2 2  2 2  2 4   m2 L1L2 s2   2    2   m2 L1L2 s2  1 2    2     V  .       m2 L1L2 s2   2     1   2     mLc   m1L1c1  m2 L1c1  2 2 12    2 g G     2  m2 L2c12      2  I zz 2  m2 L1L2c2 m2 L2  2  2 4   m2 L2  2 I zz 2   4  Configuration Space Representation For the planar 2R robot. the dynamics equations of motion from the previous page are expressed in state-space form below:     M      V  .

Notice from the joint angles. joint accelerations.5  0 0  The dynamics equations require the relative joint accelerations  . joint torques. and Cartesian pose) for this problem are given on the following page. The plots for various variables of interest (joint angles. note that the robot travels 0. but we cannot control  independently with only two-dof. they are inputs to the inverse dynamics problem.01 sec. here we calculate the required torques (i. and joint torques plots that the robot is approaching the 2 = 0 singularity towards the end of this simulated motion. joint accelerations. . how do we find these? (See 1    the last page of the Acceleration Kinematics notes in this 429/529 Supplement:   J X  J . X  0 . The robot does not move in X.) Simulate motion for 1 sec.  must move to compensate for the pure Y motion. connecting the links at their very edges (not physically possible. solve the inverse dynamics problem) to provide the commanded motion at every time step in a resolved-rate control scheme. Given: L1 = 1. with a control time step of 0. These are the joint torques necessary to move this robot’s inertia in the commanded manner. The first three plots are kinematics terms related to the resolved-rate control scheme. The revolute joints are assumed to be perfect.0 m L2 = 0. joint rates.5 m/s).5 m Both links are solid steel with mass density  = 7806 kg m3 and both have width and thickness dimensions w = t = 5 cm.81 Numerical Dynamics Example For the planar 2R robot whose dynamics equations of motion were presented analytically above.5 m in the Y0 direction in 1 sec (which agrees with the constant commanded rate of 0. The initial robot configuration is:   10     1     2  90  The constant commanded Cartesian velocity is: 0  x 0   X       (m/s)   y 0. Identical results are obtained by both analytical equations and numerical Newton/Euler recursion. In    this example. joint rates. just a simplified model). In the last plot.e. The joint torques are calculated by the numerical recursive Newton-Euler inverse dynamics algorithm.

5 -2 -2.8 1 0 0 1 (r).2 0.6 time (sec) Joint Accelerations 0.6 0  0.4 1.8 1 Dynamics Results Ignoring Gravity Dynamics Results Including Gravity .5 -3 0 0.4 0.2 0.6 time (sec) 0. 2-green) (Nm) 60 50 40 30 20 10 0 0 0.8 0.rad) (g) (deg) 1.5 1 0.2 0.6 1.82 Numerical Dynamics Example: Plots Cartesian Pose 2 1.4 0.4 0.6 tim (sec) e 0.8 1 150 100 50 0 -50 0 0.6 tim (sec) e 0.  2 0.5 0 -0. joint 2 (g) (rad/s) 2 0.4 0.8 1 joint 1 (r).2 0.8 x (r).  (b) (m. joint 2 (g) (rad/s ) joint 1 (r). 2-green) (Nm)  (1-red. y (g).2 0.6 time (sec) Joint Torques 80 70 250 200 Joint Angles 0.2 1 90 80 70 60 50 40 30 20 10 0.4 0.6 time (sec) Joint Rates 1.8 1 10 5 0 -5 -10 -15 -20 0 0.2 0.4 0.8 1 Joint Torques  (1-red.5 -1 -1.

1 Inverse Pose Control Architecture 5. 5.83 Chapter 5. we can simulate robot control using various popular robot control architectures.2 Inverse Velocity (Resolved-Rate) Control Architecture . Robot Control Architectures With the robotic kinematics and dynamics we have learned and simulated.

symmetric and positive definite N  1 vector of Coriolis and centrifugal terms N  1 vector of friction torques G   N  1 vector of gravity terms   .84 5. in order to linearize the robot system for standard controller methods.  joint angles. rates. . Here they are in matrix/vector form:      M      V  . such as PID control.    F  .3 Inverse Dynamics Control Architecture The inverse dynamics method is also called the computed torque control method. .    G     M    V  . In Chapter 4 we learned that the robot dynamics equations of motion are highly coupled and nonlinear. and accelerations Computed Torque Control Architecture The computed-torque control method is based on canceling the dynamics effects by using the inverse dynamics method.    F  .   vector of applied joint torques N  N mass matrix. and it is also called the feedback linearization control method.

85 5.4 NASA Langley Telerobotics Resolved-Rate Control Architecture .

and D. J.Optional) This control architecture was implemented by Dr.M. providing telepresence.86 5. In addition. 6F 4 R. . 121(3): 425-432. Henry. Naturally-Transitioning Rate-to-Force Control in Free and Constrained Motion. Bob and teams at NASA Langley Research Center and Wright-Patterson AFB 4. After contact.W. Williams II.L. ASME Journal of Dynamic Systems. and Control. XC + Master XM - Resolved Rate C C XA Manipulator F/T Sensor M JT XF FM (FMA) KF (Force Reflection . Repperger. if there is a human teleoperator with haptic interface. Measurement.A.5 Naturally-Transitioning Rate-to-Force Controller Architecture Resolved-rate control automatically (naturally) changes to force control when the robot endeffector enters into contact with the environment. the force/moment that the human applies to the interface becomes proportional to the force/moment that the robot exerts on the environment at contact. M. A force/torque (F/T) sensor and force/moment accommodation (FMA) algorithm is required to accomplish this. This works and feels great in realworld applications. 1999. Murphy. the constant velocity command becomes a constant force command. Trans. ASME.

the armature-controlled DC servomotor driving a single robot joint / gear train / link combination. In this case. How is this possible? (Large gear ratios tend to decouple the dynamic coupling in motion of one link on its neighbors – we will see this in modeling soon. This requires dynamics. Manipulator dynamics is extremely complicated considering the number of terms. Chapter 6) is useful to get around the need for analytical expressions.) The vast majority of multi-axis industrial robots are controlled via linearized.87 5. shown below. We now will briefly discuss single joint control. presented in this section. So. We will focus on a common system. How is manipulator dynamics done in industry for control purposes? Answer: in almost all cases it is ignored. independent single joint control. Open-Loop System Diagram V(t) L R iA(t) vB(t) armature voltage armature inductance armature resistance armature current back emf voltage M(t) M(t) M(t) JM CM n generated motor torque motor shaft angle motor shaft velocity lumped motor inertia motor viscous damping gear ratio L(t) load torque L(t) load shaft angle L(t) load shaft velocity JL(t) CL total load inertia load viscous damping . but it is not coupled nor non-linear. robot control in industry is generally accomplished by using N independent (but simultaneous) linearized joint controllers.6 Single Joint Control Every controller architecture we’ve considered requires linearized independent (but simultaneous) single joint angle control. numerical Newton-Euler recursion a’la Craig (2005. We can easily use symbolic MATLAB to crank out the terms but they go on for pages and their structure is difficult to understand. where N is the number of joint space freedoms of the robot.

Coulomb friction ignore discrete and continuous continuous for control design . model characteristics Real World Our Model (simplified) non-linear multiple-input. stiction. the large gear ratio diminishes this problem hysteresis. backlash. multiple output (MIMO) coupled time-varying load inertia linearized single-input. load shafts (the large gear ratio n reduces speed and increases torque)  n >> 1 Real-world vs.88 Closed-Loop Feedback High-level Control Diagram High-level Computer Control Diagram Simplifying Assumptions  rigid motor. single output (SISO) decoupled treat as a disturbance.

including a gear train. and control. Armature Circuit Diagram Electrical Model Kirchoff’s voltage law (1) Electromechanical coupling  The generated motor torque is proportional to the armature current (2)  The back-emf voltage is proportional to the motor shaft angular velocity (3) (KT = KB can be shown) . The system diagram was shown earlier in this section.89 Single Joint/Link System Modeling We must derive all linear ordinary differential equations (ODEs) describing the dynamics of an armature-controlled DC servomotor driving a single robot joint / link. This is an electromechanical system. We must perform modeling. simulation.

Gear ratio n (6) Substituting (6) into (5) yields (7): (7) .90 Mechanical Model Euler’s Equation (the rotational form of Newton’s Second Law): Free-body diagrams Load (4) Motor (5) where  LR (t ) is the load torque reflected to the motor shaft.

total reflected to motor shaft effective damping. total reflected to motor shaft .91 Reflect load inertia and damping to motor shaft Substitute into (4): (8) Substitute (8) into (7) to eliminate L: (9) Combine terms: (10) define: JL n2 CL CE  CM  2 n JE  JM  effective inertia.

For common industrial robots.92 Final Mechanical Model (ODE): (11) This is a second-order ODE in M. n >> 1 so For example. J L    n 2 J M   L   C L  n 2C M   L   L . the NASA 8-axis ARMII robot downstairs has n = 200 with harmonic gearing. therefore we can choose a nominal JL and n2 assume it is constant without much error in control. Can also be written as first-order ODE in M: (12) 1 is small. The gear-reduced load inertia variation is then treated as a disturbance to the single joint controller. This is why we can ignore the time-varying robot load inertia and design decoupled independent linear joint controllers. An alternative is to reflect the motor inertia and damping to the load shaft as shown below. but we will use the first case above.

This block diagram and MATLAB/Simulink implementation including a disturbance torque are give below: . A transfer function goes inside each block representing component dynamics.93 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.

94 Closed-Loop Feedback Control Diagram Assuming perfect encoder sensor for angular position feedback. The MATLAB/Simulink implementation is: . we include block for the singlejoint PID controller.

95 PID Controller Design PID Controller characteristics PID stands for: Controller transfer function: Each term does this: Use PID w/ approximate derivate in Simulink – numerical differentiation can lead to errors and numerical instability and thus it is to be avoided if possible. at least in theory and simulation. The real world always provides some additional challenges (noise. Add the I gain value to reduce steady-state error. Then add the D gain value term for stability. Always use Simulink to simulate control and dynamics response of the single joint/link system for different PID choices. backlash. such as in ME 401. modeling errors. compare various cases and select a suitable controller in simulation. etc. calibration.). Trial and Error PID Design Start with the P gain value – start low and increase P to get the desired time nature of response. . non-linearities. 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. A better approach is to perform analytical design for PID controllers using classical control theory.

joints. unexpected disturbances are modeled as disturbances in control systems. Lump all outboard links.96 Include Gravity as a Disturbance Torque Unmodeled. First let us model the gravity effect for a single joint. and motors as a single rigid body:   test with original PID gains redesign new PID gains with gravity disturbance considered . However. It is convenient to do so at the motor torque level in the block diagram: Gravity is known and expected. in single joint control we can include the effect of gravity as a disturbance.

All joints must be active.1 Introduction Serial vs. Backhoe. Serial Robot: the joints and links extend from the base in a serial fashion to the end-effector.3 in this 429/529 Supplement) for calculating overall manipulator dof are very important for parallel manipulators to ensure the correct number and type of active and passive joints are used to obtained the desired dof. NASA Hyper-redundant Serpentine Arm. Comparison of parallel vs. Parallel Robot: the joints and links are arranged in parallel from the base to the end-effector. Parallel robot – demonstrate with wooden model kit. repeatability  Ground-mounted actuators  Direct-drive  Open structure for cabling Mobility – the Grubler (planar) and Kutzbach (spatial) mobility equations (see Section 1. Example parallel manipulators: Stewart Platform. There are multiple load paths from the end-effector to the base. Parallel Robots 6. Variable Geometry Trusses. serial robots Disadvantages of parallel robots Advantages of parallel robots  Higher loads  SMALLER WORKSPACE  Higher accelerations  Generally analysis is more difficult  Lower mass  Higher stiffness  Better accuracy. There are infinite variations and possible designs for parallel robots. There is a combination of active and passive joints. The overall structure is cantilevered which is not good for resisting end-effector loads.97 Chapter 6. .

98 Kinematics problems we will consider for parallel robots  forward and inverse pose solutions  workspace  forward and inverse velocity solutions Both inverse pose and rate problems have been presented for all possible planar parallel robots with 3 identical 3-dof legs 5. CD Proceedings of the 1997 ASME Design Technical Conferences. 5 . Inverse Kinematics for Planar Parallel Manipulators. Williams II and B. DETC97/DAC-3851.L. Shelley. phasors. Sacramento. We know the opposite case is true for serial robots in general. Serial robot kinematics – DH parameters.H. We could also do parallel robot dynamics but this is of less concern for a parallel robot than for a serial robot due to the parallel nature and its inherent advantages. CA. Euler’s Identity: Phasors enable compact notation and are very convenient for time derivatives (velocity analysis). 1997. Euler’s identity Phasors – a powerful Re-Im manner to represent a vector. homogeneous transformation matrices Parallel robot kinematics – Vector loop-closure equations. 8F There exists an interesting duality between serial and parallel robots: for parallel robots. 23rd Design Automation Conference. September 14-17. the inverse pose and velocity solutions are generally straight-forward and the forward pose and velocity problems are harder. Example Vector loop-closure equation for the four-bar mechanism: R.

3 .2 .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. for use in velocity and dynamics analyses.

we can calculate these angles later.1 .2 .3 (also. dynamics.100 3-RPR parallel robot Forward Pose Kinematics (for simulation and sensor-based control) Given: Find: This is a coupled. or computer simulation).3 . So. like the Inverse Pose Kinematics problem for serial robots. We use the same vector loop-closure equations: VLCE details expanded for one RPR leg: Considering all three legs (the problem is coupled). 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.  . these represent six scalar equations in the six unknowns x. However. Then we will have three equations to solve for the three primary unknowns x . if required for velocity.  : Bring L2 to the other side and solve via the Newton-Raphson iterative numerical method. to simplify the Forward Pose Kinematics Problem. square and add each XY equation pair (for all three legs) to eliminate the intermediate variables 1. using Inverse Pose Kinematics. 2 . y. i . y .2 . nonlinear problem to solve – it is difficult to solve and multiple solutions generally exist.3 . we don’t always need the intermediate variables 1.

Gaussian elimination on J NR X  F  X  is preferable numerically. 2.  . The Newton-Raphson method is an extension of Newton’s single function/single variable root-finding technique to n functions and n variables. 2. n So to calculate the required correction factor  X at each solution iteration step. since it is more efficient and more robust than matrix inversion. we require: Fi  X   X   0 Now with O  X 2   0 we have: i  1. For solution.101 6. . the higher-order terms O  X 2  from the expansion are negligible. The following is the form of the given functions to solve: FX  0 where the n functions are: and the n variables are: F  X   F1  X  F2  X   Fn  X  X  x1 T x2  xn  T Perform a Taylor Series Expansion of F about X: Fi  X   X   Fi  X    Fi  x j  O  X 2  j 1 x j n i  1. It requires a good initial guess of the solution to get started and it only yields one of the possible multiple solutions. we must solve F  X   J NR X  0 : 1  X   J NR F  X  Actually.  .3 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. If  X is small. 2. a multi-dimensional form of the  x j    Fi  X   X   Fi  X    Fi  x j  Fi  X   J NR X  0 j 1 x j n i  1. n  F  Where J NR  J NR  X    i  is the Newton-Raphson Jacobian Matrix.  . n derivative and a function of X.

for manipulator forward pose problems. where we use the Euclidean norm and  is a small. the one resulting solution is generally the one you want. the robot cannot move too far from this known initial guess). Also halt the iteration if the number of steps becomes too high (which means the solution is diverging). However. the NewtonRaphson technique guarantees quadratic convergence. with L2 brought to the other side to equate the functions to zero.) This reduces computation if you need both forward pose computation and inversevelocity-based resolved-rate control. this does not present any difficulty since the existing known pose configuration makes an excellent initial guess for the next solution step (if the control rate is high. in the spatial it is related very closely.102 Newton-Raphson Method Algorithm Summary 0) 1) 2) 3) 4) Establish the functions and variables to solve for: F  X   0 Make an initial guess to the solution: X0 Solve J NR  X k   X k  F  X k  for  X k . Also. closest to the initial guess. user-defined solution tolerance. Generally less than 10 iterations is required for even very tight solution tolerances. transcendental functions Fi are the squared and added equations for each RPR leg. There is a very interesting and beautiful relationship between numerical pose solution and the velocity problem for parallel robots: the Newton-Raphson Jacobian Matrix is nearly identical to the Inverse Velocity Jacobian Matrix for parallel robots. the Newton-Raphson technique requires a good initial guess to ensure convergence and yields only one of the multiple solutions. Now. with: X  x y  T The three coupled. where k is the iteration counter Update the current best guess for the solution: X k 1  X k   X k Iterate until  X k   . many cycles per second. 3-RPR manipulator forward pose kinematics solution Use the Newton-Raphson numerical iteration method for solution. i . nonlinear. (In the planar case it is identical. most likely the actual configuration of the real robot. If the initial guess to the solution X0 is sufficiently close to an actual solution. except in the case of singularities where the multiple solution branches converge.

After the forward pose problem is solved at each motion step.103 Derive the required Newton-Raphson Jacobian Matrix:  F  J NR  X    i   x j    Fi  x Fi  y Fi   i  1. Alternate 3-RPR manipulator forward pose Newton-Raphson solution As we said.2 . we can calculate the intermediate variables 1. Below is the required Jacobian Matrix for this case. 2. 3 where: B  Ax  Ai     Ay  and H  Px  Ci     Py  The index i was dropped for convenience. 2. where the odd functions are the X equations and the even functions are the Y equations. 2. 3 i  1. we could have solved the original six equations in the six unknowns including the three intermediate variables 1. 3 in the above definitions in the proper places in the overall Newton-Raphson Jacobian Matrix. 2. The functions are simpler (no squaring and adding) but the size of the problem is doubled to i  1.3 as in inverse pose kinematics solution above. 6 . 3 i  1. 2. .  . also the variable ordering T is X  x y  1  2  3  .3 . use i  1.2 .

this four-bar mechanism has 1-dof. The forward pose kinematics solution may be found by intersecting leg A3C3 (a circle of given radius L3. In general. 4. With given 1 lengths L1 and L2. There are at most six intersections between a circle and tricircular sextic and so there may be up to six real multiple solutions to the 3-RPR parallel robot forward pose kinematics problem.104 1 0  1 J NR  X    0 1  0  0 1 1 1  P x s  P1 y c 1 P x c  P y s 1 1 P2 x c  P2 y s P3 x c  P3 y s L1s1  L1c1 0 0 0 0 0 0 L2 s 2  L2 c 2 0 0 0  P2 x s  P2 y c 0  P3 x s  P3 y c  0   0   0  L3s 3    L3c 3   0 Alternate analytical 3-RPR manipulator forward pose solution Figure: Branch AC1C2 A2 is a 4-bar mechanism with input angle 1 and output angle 2. or 6 of these will be real. There are always six solutions. . and it can trace out a coupler curve for point C3 in the plane. but 0. this coupler curve is a tricircular sextic. centered at A3) with the coupler curve. depending on the commanded configuration and robot geometry. 2.

3-RPR Example For planar parallel robots we can generally find the reachable workspace using a geometric method. There are two workspaces to consider (the same as for serial robots in Section 2. the dexterous workspace is almost always null since the rotation capability is never full for all three Euler angles.10 of this 429/529 Supplement): 1) reachable workspace is the volume in 3D space reachable by the 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. Then stack these up and intersect them to find the dexterous workspace. .4 Parallel Manipulator Workspace Since reduced workspace of parallel robots (when compared to serial robots) is their chief disadvantage.105 6. it becomes very important to determine the workspace of parallel robots and maximize it through design. therefore we usually define a reduced dexterous workspace wherein all Euler angles can reach  30  or some other user-definable range. For parallel robots. it is most convenient to numerically or geometrically generate in MATLAB the reachable workspace for different  values (end-effector orientation) within the desired limits. figuring out what the end-effector can reach guided by each leg on its own. defined for a reduced desired rotational range LIMIT . and then intersecting the results: For determination of the dexterous workspace.

Figure: Vector 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: . Both velocity kinematics problems use the same rate equations.5 Planar 3-RPR Manipulator Velocity Kinematics First the pose configuration variables must all be known.3 involved. we will derive these from looking at the three single RPR legs separately (meeting at the end-effector).2 . 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 .106 6.

3 .107 These equations are written in matrix-vector form to yield the RPR leg Jacobian matrix: Written in compact notation:   X  x   y z  is the same for all three legs (the Cartesian end-effector rates). i  1. No inversion is required and so the Inverse Velocity problem is never singular. Let us now find the overall robot Jacobian matrix. using only active rates and ignoring the passive rates. Here it is expressed in compact notation: . Invert the leg Jacobian matrix: Invert symbolically: Pull out only the active joint row of this result: Assemble all three active joint rows into the overall robot Jacobian matrix: Note: this above equation solves the Inverse Velocity Problem. i includes one active T and two passive joint rates for each of the three RPR legs. 2.

i for use in velocity equations Figure: 1  1     1 2  2     2  120 3  3    270 The second two relationships assume  1   2   3  30 (i. . there is an equilateral end-effector triangle).e. it is the Forward Velocity Problem that is subject to singularities for parallel robots.108 Forward Velocity Problem Compact notation (solve numerically from the Inverse Velocity Solution): Ironically.

sym('LH').109 Example Symbolic MATLAB Code Here is the MATLAB code that was used to symbolically invert the RPR leg Jacobian matrix as presented earlier. Ident2 = simple(J * Jinvsimp). % L LH t1 b1 c1 s1 cp sp % j11 j21 j13 j23 Declare symbolic variables sym('L'). sym('sin(t1+b1)'). sym('t1'). . dynamics. % Check Ident1 = simple(Jinvsimp * J). % Invert Jinv = inv(J). sym('cos(t1+b1)'). Symbolic computing has a lot of power in robot kinematics. sym('cos(t1)'). = L*c1+LH*cp. % % % Symbolic MATLAB code to invert the RPR leg Jacobian clear. = = = = = = = = Jacobian elements = -L*s1-LH*sp. sym('sin(t1)'). sym('b1'). and control.j21 s1 j23.1 0 1]). % Jacobian matrix J = sym([j11 c1 j13. clc. = LH*cp. % Simplify Jinvsimp = simple(Jinv). = -LH*sp.