Advanced Robotics DR Bob

ME 604
ADVANCED ROBOTICS I

CLASS NOTES

DR. BOB

Mechanical Engineering

Ohio University

Dr. Bob Productions
2
Table of Contents

INTRODUCTION TO ROBOTICS ............................................................................. 3
MATRIX INTRODUCTION...................................................................................... 12
MATLAB INTRODUCTION....................................................................................... 17
MOBILITY AND MOTION DESCRIPTION........................................................... 21
ORTHONORMAL ROTATION MATRICES .......................................................... 24
HOMOGENEOUS TRANSFORMATION MATRICES.......................................... 33
DENAVIT-HARTENBERG (DH) PARAMETERS.................................................. 40
FORWARD POSE KINEMATICS ............................................................................ 49
INVERSE POSE KINEMATICS................................................................................ 60
VELOCITY ANALYSIS AND JACOBIAN MATRICESS ...................................... 80
ACCELERATION KINEMATICS ..........................................................................110
MANIPULATOR DYNAMICS .................................................................................116
SINGLE JOINT CONTROL .....................................................................................133
KINEMATICALLY-REDUNDANT MANIPULATORS ........................................142
PARALLEL MANIPULATORS ...............................................................................154
3
Introduction to Robotics

For a snazzier Intro to Robotics, please see:

http://www.ent.ohiou.edu/~bobw/PDF/IntroRob.pdf

4
Some Definitions:

1) Robot An electromechanical machine with more than one degrees-of-freedom
(DOF) which is programmable to perform a variety of tasks. From R.U.R. by the
Czechoslovakian playwright Karl Kapek; robot - worker or serf.

2) Anthropomorphic: Similar to Humans.

3) Manipulator - mechanical arm, with several DOF.

4) Degrees-of-Freedom - the number of independently controllable motions in a
mechanical device. The number of motors in a serial manipulator.

5) Mechanism - a 1-DOF machine element.

6) Fixed Automation - designed to perform a single repetitive task.

7) Flexible Automation - can be programmed to perform a variety of tasks.

8) Robot system - manipulator(s), sensors, actuators, communication, computers,
interface, hand controllers to accomplish a programmable task.

9) Actuator - motor that drives a joint; generally rotary (revolute) or linear (prismatic);
electric, hydraulic, pneumatic, piezoelectric.

10) Cartesian Coordinate frame - dextral, orthogonal, XYZ, k j i
.

11) Kinematics - the study of motion without regard to forces. Cartesian Pose: position
and orientation of a coordinate frame.

a) Forward Kinematics - given the joint variables, calculate the Cartesian pose.

b) Inverse Kinematics - given the Cartesian pose, calculate the joint variables.

12) Position (Translation) - measure of location of a body in a reference frame.

13) Orientation (Rotation) - measure of attitude of a body (e.g. Roll, Pitch, Yaw) in a
reference frame.

14) Singularity - a configuration where the manipulator momentarily loses one or more
degrees-of-freedom due to its geometry.
5

15) Actuator Space - vector of actuator commands, connected to joint through gear train
or other drive.

16) Joint Space - vector of joint variables; basic control parameters.

17) Cartesian Space - Position vector and orientation representation of end-effector;
natural for humans.

18) End-effector - tool or hand at the end of a robot.

19) Workspace - The volume in space that a robots end-effector can reach, both in
position and orientation.

20) Dynamics - the study of motion with regard to forces (the study of the relationship
between forces/torques and motion). Composed of kinematics and kinetics.

a) Forward Dynamics (simulation) - given the actuator forces and torques,
compute the motion.

b) Inverse Dynamics (control) - given the desired motion, calculate the actuator
forces and torques.

21) Control - causing the robot system to perform the desired task. Different levels.

a) Teleoperation - human moves master, slave manipulator follows.

b) Automation - computer controlled (using sensors).

c) Telerobotics - combination of the b) and c)

22) Haptics - From the Greek, meaning to touch. Haptic interfaces give human
operators the sense of touch and forces from the computer, either in virtual or real,
remote environments. Also called force reflection.
6
Robot Applications

Traditionally, robots are applied anywhere one or more of the 3Ds exist: in any job which
is too: Dirty, Dangerous, and/or Dull for a human to perform.

Industry
Manufacturing, assembly, part handling, palletizing, maintenance, inspection, welding,
spray painting, deburring, machining.

Remote operations
Undersea, nuclear environment, bomb disposal, law enforcement, outer space, other
hazardous environments.

Service
Hospital helpmates, handicapped assistance, retail, household servants, lawnmowers,
security guards.

7
Common Robot Configurations

Cartesian
Robots which have three linear (P, as opposed to rotational R joints) axes of movement
(X, Y, Z). Used for pick and place tasks and to move heavy loads. They can trace out
rectangular volumes in 3D space.

Cylindrical
The positions of these robots are controlled by a radius, a height and an angle (that is, two
P joints and one R joint). These robots are commonly used in assembly tasks and can
trace out concentric cylinders in 3D space.

Spherical
Spherical robots have two rotational R axes and one translational P (radius) axis. The
robots end-effectors can trace out concentric spheres in 3D space.

Articulated
The positions of articulated robots are controlled by three angles, via R joints. These
robots resemble the human arm (anthropomorphic). They are the most versatile robots,
but also the most difficult to program.

8
SCARA (Selective Compliance Articulated Robot Arm)
SCARA robots are a blend of the articulated and cylindrical robots, providing the benefits
of each. The robot arm unit can move up and down, and at an angle around the axis of
the cylinder just as in a cylindrical robot, but the arm itself is jointed like a revolute
coordinate robot to allow precise and rapid positioning. The robot consists of three R and
one P joints; an example is shown below.

OU RoboCup Player

Mobile robots
Mobile robots have wheels, legs, or other means to navigate around the workspace under
control. Mobile robots are applied as hospital helpmates and lawn mowers, among other
possibilities. These robots require good sensors to see the workspace, avoid collisions,
and get the job done. The following six images show Ohio Universitys involvement
with mobile robots playing soccer, in the international RoboCup competition
(www.robocup.org).

Humanoid robots
Many students (and U.S. Senators) expect to see C3PO (from Star Wars) walking around
when visiting a robotics laboratory. Often they are disappointed to learn that the state-of-
the-art in robotics still largely focuses robot arms. There is much current research work
aimed at creating human-like robots that can walk, talk, think, see, touch, etc. Generally
Hollywood and science fiction lead real technology by at least 20 or 30 years.

Discussion: will robots ever replace human beings?!?

9

Honda Humanoid Robot

Parallel robots
Most of the robots discussed so far are serial robots, where joints and links are
constructed in a serial fashion from the base, with one path leading out to the end-
effector. In contrast, parallel robots have many legs with active and passive joints and
links, supporting the load in parallel. Parallel robots can handle higher loads with greater
accuracy, higher speeds, and lighter robot weight; however, a major drawback is that the
workspace of parallel robots is severely restricted compared to equivalent serial robots.
Parallel robots are used in expensive flight simulators, as machining tools, and can be
used for high-accuracy, high-repeatability, high-precision robotic surgery.

10
Craig Notation

1) Uppercase variables are matrices or vectors. Lowercase variables are scalars.

2) Leading sub- and superscripts indicate which coordinate system the quantity is
expressed in; e.g.:
P
A
is a position vector P expressed in Cartesian coordinate frame {A}.
B
A
P is a position vector from the origin of frame {A} to the origin of frame {B}
expressed in the basis of frame {A}.
R
A
B
is an orthonormal rotation matrix giving the orientation of frame {B} relative to
frame {A}.
T
A
B
is a homogeneous transformation matrix giving the pose (position and orientation)
of frame {B} relative to frame {A}.

3) Trailing superscript indicates inverse or transpose of a matrix:
1
R or
T
R .

4) Trailing subscripts indicate vector components (x,y,z) or are descriptive.

5) Trigonometric functions are often abbreviated:

i i
i i
i i
t
s
c
=
=
=
tan
sin
cos

Required Mathematics

Vectors, matrices, linear algebra

Trigonometry, geometry

Calculus

Ordinary differential equations

11
Brief Review of Trigonometry & Calculus

IV Quadrants - degrees and radians

Sine, Cosine, Tangent functions

Inverse Functions - include atan2

Some trig identities:

( ) ( )
( ) ( ) a a
a a
cos cos
sin sin
=
=

( )
( ) casb sacb b a
sasb cacb b a
=
=
sin
cos m

1
2 2
= +
i i
s c

Law of Cosines: c AB B A C cos 2
2 2 2
+ =

Law of Sines:
C
c
B
b
A
a sin sin sin
= =

Some Relations from Calculus
derivatives (including chain rule), integration
dt
dx
dx
df
dt
df
=

) (
) (
) (
2
2
t x
dt
s d
dt
dv
a
t x
dt
ds
v
t x s
& &
&
= = =
= =
=

= = =
= =
=
) (
) (
) (
t x a v s
t x a v
t x a
&
& &
12
Matrix Introduction

Matrix: m x n array of numbers, where m is the number of rows and n in the number of
columns.
[ ]
(
(
(
(
=
mn m m
n
n
a a a
a a a
a a a
A
L
M O M M
L
L
2 1
2 22 21
1 12 11

Used to simplify and standardize solution of n linear equations in n unknowns (where
m=n). Used in velocity, acceleration, and dynamics analysis linear equations (not
position! - non-linear).

Special Matrices

Square (m=n=3) [ ]
(
(
(
=
33 32 31
23 22 21
13 12 11
a a a
a a a
a a a
A Diagonal [ ]
(
(
(
=
33
22
11
0 0
0 0
0 0
a
a
a
A
Identity [ ]
(
(
(
=
1 0 0
0 1 0
0 0 1
I Transpose [ ]
(
(
(
=
33 23 13
32 22 12
31 21 11
a a a
a a a
a a a
A
T

Symmetric [ ] [ ]
(
(
(
= =
33 23 13
23 22 12
13 12 11
a a a
a a a
a a a
A A
T

Column Vector (3x1 matrix) { }
=
3
2
1
x
x
x
X
Row Vector (1x3 matrix) { } { }
3 2 1
x x x X
T
=

Matrix Addition Just add up like terms
(
+ +
+ +
=
(
+
(
h d g c
f b e a
h g
f e
d c
b a

13
Matrix Multiplication with Scalar Just multiply each term
(
=
(
kd kc
kb ka
d c
b a
k

Matrix Multiplication [ ] [ ][ ] [ ][ ] A B B A C =

Row, Column indices have to line up as follows:

[ ] [ ][ ]
( ) ( )( ) pxn mxp mxn
B A C
=

That is, the number of columns in the left-hand matrix must equal the number of rows in
the right-hand matrix; if not, the multiplication is undefined and cannot be done!
Multiplication proceeds by multiplying and adding terms 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:
[ ]
( ) ( )( ) 1 3 3 2 1 2 x x x
fi eh dg
ci bh ag
i
h
g
f e d
c b a
C
)
`
+ +
+ +
=
=

note the inner indices (p=3) must match, as stated above.

Matrix Multiplication Examples

[ ]
(
=
6 5 4
3 2 1
A [ ]
(
(
(
=
6 7
8 9
8 7
B

[ ] [ ][ ]
(
=
(
+ + + +
+ + + +
=
(
(
(
= =
108 115
42 46
36 40 32 42 45 28
18 16 8 21 18 7
6 7
8 9
8 7
6 5 4
3 2 1
B A C
( ) ( )( ) 2 3 3 2 2 2 x x x

14
[ ] [ ][ ]
(
(
(
=
(
(
(
+ + +
+ + +
+ + +
=
(
(
(
(
= =
57 44 31
75 58 41
69 54 39
36 21 30 14 24 7
48 27 40 18 32 9
48 21 40 14 32 7
6 5 4
3 2 1
6 7
8 9
8 7
A B D
( ) ( )( ) 3 2 2 3 3 3 x x x

Matrix Inversion Matrix division [ ] [ ][ ] B A C = solve for [B]

[ ] [ ][ ] [ ] [ ] [ ] [ ][ ] [ ][ ] [ ] [ ] [ ] [ ] C A B B B I B A A C A B A C
1 1 1
= = = = =

Matrix [A] must be square.

[ ][ ] [ ] [ ] [ ] I A A A A = =
1 1
(identity matrix, the matrix 1).

[ ]
( ) [ ]
A
A Adjo
A
int
1
=

A Determinant of [A]
( ) [ ] ( ) [ ]
T
A Cofactor A Adjo = int
Cofactor of ( )
ij
j i
ij ij
M a
+
= 1
Minor
ij
M of
ij
a is determinant of submatrix with row i and column j removed.

System of Linear Equations n equations in n unknowns.

For n=3:
3 3 33 2 32 1 31
2 3 23 2 22 1 21
1 3 13 2 12 1 11
b x a x a x a
b x a x a x a
b x a x a x a
= + +
= + +
= + +

Using matrix multiplication (backwards), this is written as:

[ ]{ } { } b x A = where:

[ ]
(
(
(
=
33 32 31
23 22 21
13 12 11
a a a
a a a
a a a
A (known coefficients)
15
{ }
=
3
2
1
x
x
x
x (unknowns to be solved) { }
=
3
2
1
b
b
b
b (known right-hand sides)

Unique solution { } [ ] { } b A x
1
= only if [A] has full rank. If not, 0 = A and the inverse of
matrix [A] is undefined (dividing by zero).

Matrix Example

Solution of simultaneous linear equations.

14 4 6
5 2
= +
= +
y x
y x

)
`
=
)
`
14
5
4 6
2 1
2
1
x
x

[ ]
(
=
4 6
2 1
A { }
)
`
=
2
1
x
x
x { }
)
`
=
14
5
b

{ } [ ] { } b A x
1
=

( ) ( ) 8 6 2 4 1 = = A Determinant non-zero; unique solution!

[ ]
(
=
(
8 / 1 4 / 3
4 / 1 2 / 1
1 6
2 4
1
1
A
A

check: [ ][ ] [ ] [ ] [ ]
(
= = =

1 0
0 1
2
1 1
I A A A A

)
`
=
)
`
=
)
`
2
1
14
5
8 / 1 4 / 3
4 / 1 2 / 1
2
1
x
x
Answer.

check: Plug answer into original equations and compare {b}.
16
Vector and Matrix Matlab Examples

P1 = [1;2;0]; % Define two vectors
P2 = [3;2;0];
sum1 = P1+P2; % Vector addition
sum2 = P2+P1;
dot1 = dot(P1,P2); % Vector dot product
dot2 = dot(P2,P1);
cross1 = cross(P1,P2); % Vector cross product
cross2 = cross(P2,P1);

A = [1 2;6 4]; % Define given matrix and vector
b = [5;14];
dA = det(A); % Calculate the determinant of A
invA = inv(A); % Calculate the inverse of A
x = invA*b; % Solve linear equations
x1 = x(1); % Extract answers
x2 = x(2);
AT = A; % Matrix transpose

17

Matlab Introduction

MATrix LABoratory

Control systems simulation and design software. Very widespread in other fields.
Introduction to basics, programming, plots, animation, matrices, vectors. Based on C
language, programming is vaguely C-like, but much simpler to use. Sold by Mathworks
(http://www.mathworks.com).

Can buy student version software and manual for about the price of one textbook (can use
it for many classes!). ENT college has a Matlab license; it is installed in most computer
labs.

Double-click on Matlab icon to get started. Type

>>demo

to get a comprehensive overview of Matlab including built-in functions. Try all the
categories under Matlab first; you can ignore Toolboxes, Simulink, and Stateflow for
now. (Exception: there is Symbolic Math under Toolboxes for the adventurous student!).

Type in commands (such as the Vector/Matrix examples given earlier) at the Matlab
prompt >>. Press <Enter> to see result or ; <Enter> to suppress result.

Recommended operation mode: M-Files. Put your sequence of MATLAB statements in
an ASCII file name.m (can create with the beautiful Matlab Editor/Debugger - this is
color-coordinated, tab-friendly, with parentheses alignment help and debugging
capabilities). Make sure the file exists where the Matlab search path can find it. If you
use the 1.4M disk drive, at the >> prompt, type:

>>cd a:

This instructs Matlab that your default directory is on the a: disk; you are free to specify
a directory structure under a:. At the >> prompt type the M-File name name, without the
.m. Matlab language is interpretive and executes line-by-line. Use the ; at the end of
statements to suppress intermediate results. If you use this suppression, the variable
name still holds the resulting value(s) - just type the variable name at the prompt after the
program runs to see the value(s). If there is a syntax or programming logic error, it will
give a message at the bad line and then quit. Type:

>>who

18

to show you what variables you have defined;

>>whos

will show the variables, plus their matrix dimensions (scalar, vector array, or matrix),
very useful for debugging. Plus, after running a file, place the cursor over different
variables in the M-File inside the Editor/Debugger to see the values! On-line help is
generally great:

>>help

Example M-Files (given on the following two pages)
1) Matlb1.m: Input, programming, plots, animation.
2) Matlb2.m: Matrix and vector definition, multiplication, transpose, and solution of
linear equations.

19

%
% Matlab Example Code 1: Matlb1.m
% Matrix, Vector examples
% Dr. Bob, ME 604
%

clc; clear; % Clear the cursor and clear all previously defined variables

%
% Matrix and Vector definition, multiplication, and transpose
%

A = [1,2, 3; ... % Define 2x3 matrix [A] (... is continuation line)
1,1,-1];
x = [1;2;3]; % Define 3x1 vector {x}

v = A*x; % 2x1 vector {v} is the product of [A] times {x}

AT = A'; % Transpose of matrix [A]
vT = v'; % Transpose of vector {v}

%
% Solution of linear equations Ax=b
%

A = [1,2, 3; ... % Redefine matrix [A] to be 3x3: coefficient matrix
1,1,-1; ...
8,2,10];

b = [3;2;1]; % Define right-hand side vector of knowns {b}

detA = det(A); % First check to see if det(A) is near zero

x = inv(A)*b; % Redefine {x} to be the solution of Ax=b by inversion

check = A*x; % Check results;

z = b - check; % Better be zero!

%
% Display the created variables (who), with dimensions (whos)
%

who
whos

20

%
% Matlab Example Code 2: Matlb2.m
% Menu, Input, FOR loop, IF logic, Animation, and Plotting
% Dr. Bob, ME 604
%
clc; clear; % Clear the cursor and clear all previously defined variables

r = 1; L = 2; DR = pi/180; % Constants

%
% Input
%
anim = menu('Animate Single Link?','Yes','No') % Menu to screen

the = input('Enter [th0, dth, thf] (deg): ') % User type input
th0 = the(1)*DR; dth = the(2)*DR; thf = the(3)*DR; % Initial, delta, final thetas
th = [th0:dth:thf]; % Assign theta array

%
% Animate single link
%

figure; % Give a blank graphics window
if anim == 1 % Do if user wants to
for i = 1:thf/dth+1;
x2 = [0 L*cos(th(i))]; % Single link coordinates
y2 = [0 L*sin(th(i))];
plot(x2,y2); % Animate to screen
axis('square'); grid; axis([-2 2 -2 2]);
pause(1/4);
if i==1 % Pause to maximize window
pause; % User hits Enter to continue
end
end
end

%
% Loop
%

for i = 1:thf/dth+1,
xc(i) = r*cos(th(i)); % Circle coordinates
yc(i) = r*sin(th(i));
f(i) = cos(th(i)); % Function of theta (cosine)
end

%
% plots
%
figure;
plot(th/DR,f); % Plot cosine function
axis([0 360 -1 1]); grid; title('Function of Theta');
xlabel('\theta'); ylabel('cos(\theta)');

figure;
plot(xc,yc); % Plot circle
axis(['square']); grid; axis([-1.5 1.5 -1.5 1.5]); title('Circle');
xlabel('\itX'); ylabel('\itY');

21
Mobility and Motion Description

Mobility

Mobility - the number of degrees-of-freedom which a robot or mechanism possesses.

For serial robots, simply count up the number of (single degree-of-freedom) joints.

Grubler's Criterion: Planar robots

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

Where:
M is the mobility
N is the total # of links, including ground
J
1
is the number of one-degree-of-freedom joints
J
2
is the number of two-degree-of-freedom joints

One-degree-of-freedom joints: Revolute, Prismatic
Two-degree-of-freedom joints: Cam joint (rolling and sliding), Gear joint

General planar n-link serial robot has n+1 links (including fixed ground link), connected
by n active 1-dof joints:

( ) ( ) n n n n n M = = + = 2 3 0 1 2 1 1 3 dof

(just count number of active joints)

Planar Examples:

22
Kutzbach's Criterion: Spatial robots

( )
5 4 3 2 1
1 2 3 4 5 1 6 J J J J J N M =

Where:
M is the mobility
N is the total # of links, including ground
J
1
is the number of one-degree-of-freedom joints
J
2
is the number of two-degree-of-freedom joints
J
3
is the number of three-degree-of-freedom joints
J
4
is the number of four-degree-of-freedom joints
J
5
is the number of five-degree-of-freedom joints

One-degree-of-freedom joints: Revolute, Prismatic, Screw
Two-degree-of-freedom joints: Cylindrical, Gear joint
Three-degree-of-freedom joints: Spherical
Four-degree-of-freedom joints: Spherical in a slot
Five-degree-of-freedom joints: Spatial cam

General spatial n-link serial robot has n+1 links (including fixed ground link), connected
by n active 1-dof joints:

( ) ( ) ( ) ( ) ( ) n n n n n M = = + = 5 6 0 1 0 2 0 3 0 4 5 1 1 6 dof

(just count number of active joints)

Spatial Examples:

23
Motion Description

We need to describe the spatial position & orientation (pose) of links, tools, end-
effectors, sensors, workpieces, etc.

We attach a separate, independent right-handed Cartesian coordinate frame to each
moving body of interest. These frames are fixed in the moving body (e.g. link) and are
moved relative to each other via active joints.

Drawing (moving frame {B} relative to reference fame {A}):

B
A
P and R
A
B
are required to describe the position (translations) and orientation
(rotations) of {B} with respect to {A} together this is called the pose.

Position
{ }
T
roll pitch yaw z y x X =
Note: position is vector but rotation IS NOT A VECTOR!!

Velocity
{ }
T
z y x
z y x X & & &
&
=

Acceleration
{ }
T
z y x
z y x X & & & & & &
& &
=

Velocity and acceleration are vector representations, both position and orientation

The 3D representation of orientation (rotations) CANNOT BE EXPRESSED AS A
VECTOR!!!

24
Orthonormal Rotation Matrices

Position
=
z
y
x
P
A point has no orientation, just position described by vectors in 3D space. Position
vector addition is commutative, i.e.
1 2 2 1
P P P P + = +

Orientation
For orientation, need two frames; let us describe the orientation of {B} w.r.t. {A}. Spatial
(3D) orientations have 3 dof (e.g. roll, pitch, yaw show airplane coordinates)

Planar - easy, just ; this is a vector representation.

Spatial - hard, artificial, cannot be a vector representation.

3 Rotations about fixed frame
3 Rotations about moving frame (Euler Angles)
Axis - angle rotation (Screw theory)
Quaternions (Similar to Axis angle, but requires 4 parameters, not 3)

All descriptions lead to the Orthonormal Rotation Matrix - 3 dof for orientation,
above descriptions somewhat artificial, many possibilities leading to the same unique
Orthonormal Rotation Matrix to describe the orientation of {B} w.r.t. {A}.

1. Demonstrate that spatial rotations are not described by vectors (spatial rotations
are not commutative); the ol book rotation trick:

25
2. Form of Orthonormal Rotation Matrices:

R
A
B
is a 3x3 matrix describing the orientation of frame {B} with respect to frame
{A}. Remember 3D orientations have only 3 dof (e.g. roll, pitch, yaw); however, the
rotation matrix has 9 elements why? (Lets answer this later.)

Demonstrate projection of {B} axes onto {A} basis; e.g.
B
A
X
:

What vector operation is used for projecting one vector onto another?

(
(
(

=
A B A B A B
A B A B A B
A B A B A B
A
B
Z Z Z Y Z X
Y Z Y Y Y X
X Z X Y X X
R

So the Orthonormal Rotation Matrix is also called the Direction Cosine Matrix. Note the
dot product is commutative, so another way to define it is row-wise:

(
(
(

=
(
(
(

=
A
B
A
B
A
B
B A B A B A
B A B A B A
B A B A B A
A
B
Z
Y
X
Z Z Y Z X Z
Z Y Y Y X Y
Z X Y X X X
R

26
3. Inverse Orthonormal Rotation Matrix

The inverse matrix is simply shifting our reference let us describe the orientation of {A}
w.r.t. {B}:

1
= R R
A
B
B
A
how do we calculate this?

(
(
(
=
(
(
(

=
| | |

| | |

A
B
A
B
A
B
B A B A B A
B A B A B A
B A B A B A
B
A
Z Y X
Z Z Z Y Z X
Y Z Y Y Y X
X Z X Y X X
R

Compare this form with the last relationship of the previous page; we see:

So, to find the inverse of an Orthonormal Rotation Matrix, we need only take the
transpose, which is cheap computationally and never subject to singularities!! This is a
beautiful property and only holds for this very special type of matrix!!

4. Simple example (for the following sketch, find R
A
B
; also find
1
= R R
A
B
B
A
):

27
5. Single axis, single angle rotations

To describe general 3D orientations we will use a series of three single axis, single
angle rotations. Therefore, we need to be able to derive and understand the rotation
matrix representing a single angle rotation about a single primary axis: X, Y, and Z.

We will now derive the rotation matrix representing the orientation of {B} w.r.t. {A}
when {B} is rotated about the Z axis of {A} by angle . Figure:

From the geometry of this situation, the formula is:

The student is left to derive in a similar manner ( )
Y
R and ( )
X
R which are also
required.

6. Description of general (compound) spatial orientation

Remember description of orientation is rather artificial; many paths will lead to the
same numerical description for a general R
A
B
. There are twelve distinct ways to describe
3 general rotations about fixed axes and also twelve distinct ways to describe 3 general
rotations about moving axes (called Euler angles):

X-Y-X
X-Y-Z
X-Z-X
X-Z-Y
Y-X-Y
Y-X-Z
Y-Z-X
Y-Z-Y
Z-X-Y
Z-X-Z
Z-Y-X
Z-Y-Z

Note: X-Y-Y, etc., is not allowed since a repeated Y rotation is not independent.

28
7. Euler Angles. Since there are so many artificial paths to reach the same result, let us
choose one convention and stick with it always:

Z-Y-X () Euler angles (about moving axes) demonstrate using frames {B}
and {A}:

Start with frames {B} and {A} aligned (what is R
A
B
for that special
configuration?). Assume {B} is the moving frame and {A} is the reference frame.

a. First rotate moving frame {B} by an angle about the axis
B
Z
(which is
identical to
A
Z
for this first rotation only).

b. Next rotate moving frame {B} by an angle about the axis
B
Y
(which was
moved away from
A
Y
in the a. rotation).

c. Last rotate moving frame {B} by an angle about the axis
B
X
(which has
been twice rotated away from
A
X
).

Craig derives the overall orientation description for this Euler rotation convention:

( ) ( ) ( )
(
(
(
(
(
(
(
(
(

=
=

c s
s c
c s
s c
c s
s c
R
R R R R
A
B
X Y Z
A
B
0
0
0 0 1
0
0 1 0
0
1 0 0
0
0

(
(
(
+ +
+ +
=

c c s c s
c s s s c s s s c c c s
c s c s s s s c c s c c
R
A
B

Given , it is easy to find R
A
B
- just substitute and evaluate.

29
8. The inverse problem is not so easy: Given a valid R
A
B
, find .

Craig gives this solution for Z-Y-X () Euler angles in Chapter 2, in the X-Y-Z
() Fixed angles section (read the book turns out these two conventions are
mathematically identical in symbolic formula results). The inverse problem is solved by
forming three independent equations or combinations of equations from the symbolic
expression of R
A
B
:

(
(
(
+ +
+ +
=
(
(
(

c c s c s
r r r
r r r
r r r
33 32 31
23 22 21
13 12 11

ij
r are nine given, consistent (see constraint discussion below) numbers of R
A
B
.
are the 3 unknowns.

Using the first column and the facts that 1
2 2
= + s c and cos / sin tan = :

|
.
|
\
|
+ =
2
21
2
11 31
, 2 r r r atan
Using the 21 and 11 terms:
|
|
.
|
\
|
=

c
r
c
r
atan
11 21
, 2
Using the 32 and 33 terms:
|
|
.
|
\
|
=

c
r
c
r
atan
33 32
, 2
Note that dividing by c on the numerator & denominator of the last two solutions may
seem silly, but the sign of c makes a difference. Read Craig for singular solution when
0 = c There are two overall inverse solutions, making use of the in the solution.
Both solutions must yield back the original R
A
B
when substituted into the forward
rotation matrix expression.

30
9. Orthonormal Rotation Matrix Constraints

Remember spatial rotations have 3 dof (e.g. roll-pitch-yaw, or ), but the
Orthonormal Rotation Matrix has 9 numbers. Therefore, there must be 6 (scalar)
constraints on these nine numbers. These six constraints come from the name
Orthonormal.

There are three Orthogonal constraints: all three columns (and rows) are mutually
perpendicular to each other for all possible orientations. This makes sense since X-Y-Z
of frame {B} are permanently mutually perpendicular for all motion. We can use one
vector cross product or three vector dot products to express these three constraints:

There are three Normalized constraints: all three columns (and rows) are unit vectors.
We express these three constraints with three (scalar) vector length equations:

31
10. Position vector rotational transformations

We now introduce a very useful matrix-vector equation for transforming the
coordinates of a given position vector P
B
known in the {B} frame to the same vector
P
A
described in the {A} frame coordinates (changing the basis of the vector from {B} to
{A}).
The XYZ components of a vector are the projections of that vector onto the XYZ
axes of the frame of interest. Let's project a vector P onto the {A} frame using the dot
product.
P Z p
P Y p
P X p
A Z
A
A Y
A
A X
A
=
=
=

Now, let's change the basis (Cartesian coordinate frame wherein the coordinates of
a vector are expressed) of vector P from {B} to {A}. Right-hand side vectors of above
equations can be expressed in {B} coordinates:
P Z p
P Y p
P X p
B
A
B
Z
A
B
A
B
Y
A
B
A
B
X
A
=
=
=

Now,
A
B
A
B
A
B
Z Y X
are the rows of R

A
B
, so:
P R P
B A
B
A
=

Note the two vectors are identical in direction and length, just the basis for expression of
the XYZ components are different. So, we can use the Orthonormal Rotation Matrix to
rotate coordinates of a vector from one Cartesian coordinate frame to another.

Example:

32
11. Euler Angles Forward and Inverse Examples

We conclude this section on Orthonormal Rotation Matrices by presenting
examples for the forward and inverse problems for the Z-Y-X () Euler angles
convention.

a. Forward calculation: Given Z-Y-X Euler angles
o
50 = ,
o
40 = , and
o
30 = , find R
A
B
.

Answer:
(
(
(
=
663 . 0 383 . 0 643 . 0
105 . 0 803 . 0 587 . 0
741 . 0 457 . 0 492 . 0
R
A
B

Be sure to check the six constraints to ensure this is a valid Orthonormal Rotation Matrix
result.

b. Inverse solution:
Given
(
(
(
=
663 . 0 383 . 0 643 . 0
105 . 0 803 . 0 587 . 0
741 . 0 457 . 0 492 . 0
R
A
B
, solve for the Z-Y-X Euler angles
, , (both solution sets).

Answer:

Solution

1
o
50
o
40
o
30
2
o
230
o
140
o
210

Be sure to check the second solution set.

33
Homogeneous Transformation Matrices

One convenient 4x4 matrix representation to give pose (position and orientation) of
one moving Cartesian coordinate frame with respect to a reference frame. Figure:

Vector loop closure equation:

This is a basis shift (remember P R P
B A
B
A
= from earlier), plus a translation since
now
B
A
P is considered.

General form of Homogenous Transformation Matrix (4 x 4):

Perspective, scaling when last row is not [ ] 1 0 0 0 - used in computer graphics.
For robotics purposes, this last row never changes - we don't want to scale or skew rigid
links moved by active joints!

Must append a "1" to all (3 x 1) position vectors to use with Homogeneous
Transformation Matrices. This leads to a dummy equation of 1=1, but this is necessary
for matrix multiplication using Homogeneous Transformation Matrices.

34
3 Interpretations for Homogeneous Transformation Matrices

Common Example for all 3 interpretations: Given:

1) Description of a frame

T
A
B
describes the pose (position and orientation) of moving Cartesian coordinate
frame {B}, w.r.t. to a reference frame, {A}. Cartesian coordinate frame {A} can be
moving too, but we station an observer on {A} to watch how {B} is translating and
rotating with respect to {A}.

{ }
B
A
P is the position vector giving the location of the origin of {B} w.r.t. the origin
of {A}, expressed in the basis (coordinates) of {A}.

[ ] R
A
B
is the rotation matrix giving the orientation of {B} w.r.t. {A}; columns are
the XYZ unit vectors of {B} projected onto the XYZ {A} unit directions.
[ ]
(
(
(
=
| | |

| | |
B
A
B
A
B
A A
B
Z Y X R

Figure for first interpretation:

35
2) Transform Mapping:

Matrix T
A
B
maps P P
A B
. Describes a vector known in one Cartesian coordinate
frame {B} in another frame {A}. There is both position and orientation (basis) difference
in general. Equation:

Given: P
B
, find P
A

Note: P R P P
B A
B B
A A
+ =

Figure for second interpretation:

36
3) Transform Operator:

T operates on
1
P
A
to yield
2
P
A
. Same Cartesian coordinate frame {A}, there is no
{B} for this interpretation. The original vector
1
P
A
has been translated and rotated to
new vector
2
P
A
. Order of translation and rotation doesn't matter if we assume rotations
always occur about the tail of vectors. Equation:

Given:
1
P
A
, find
2
P
A

Figure for third interpretation:

37
Invert Homogeneous Transformation Matrices

Given T
A
B
, find the opposite Homogeneous Transformation Matrix, i.e. the pose
of Cartesian coordinate frame {A} w.r.t. {B}:
1
= T T
A
B
B
A

Can just use matrix inversion (Matlab function inv), but this is computationally
expensive, may be numerically unstable, and doesn't take advantage of the structure of
Homogeneous Transformation Matrices. Alternatively, Gaussian elimination is less
computationally expensive and more robust numerically, but it still doesn't take
advantage of the structure of Homogeneous Transformation Matrices.

[ ] [ ]= =
T T
B
A
A
B
1

Beautiful Property from Orthonormal Rotation Matrices:

Translation part: Without regard for frame of expression:

With regard,

[ ] =
1
T
A
B

Example: Given: find
1
= T T
A
B
B
A

Figure for example:

38
Transform Equations

We will represent a robot by a series of coordinate frames, one moving Cartesian
coordinate frame, rigidly attached to each moving rigid link at each active joint.

The pose of consecutive Cartesian coordinate frames are represented by T
i
i
1
.

A vector P
i
is mapped back to P
i 1
by T
i
i
1
:

For a general robot, there are many such frames.

Example: Frames {A}, {B}, {C}, {D} lie along a serial robot chain.

Given P
D
(e.g. hand location in local frame), find P
A
(e.g. hand location in base
frame):

Associated Transform Graph:

Now, given any three of these Homogeneous Transformation Matrices, we should be able
to calculate the fourth using linear algebra and matrix inversion; show one example:

Associated Transform Graph:

39
Transform Equations: Robotics Example

Frames: Figure:

{WO} World
{B} Base
{WR} Wrist
{H} Hand
{G} Goal

Given: Fixed matrices: T T T
WO
G
WR
H
WO
B
, ,

1) Given T
H
G
from machine vision algorithm. Calculate T
B
WR
(Can compare with T
B
WR

calculated by forward pose kinematics and joint angle encoders.)

Transform loop closure equation:
T T T T T
H
G
WR
H
B
WR
WO
B
WO
G
=

T T T T T
WR
H
B
WR
H
G
WO
G
WO
B
=
1 1

1 1 1
= T T T T T
WR
H
H
G
WO
G
WO
B
B
WR

or,
1 1
= T T T T
WR
G
WO
G
WO
B
B
WR

where ( )
1 1
1
1
= = T T T T T
WR
H
H
G
H
G
WR
H
WR
G

2) Want {H}to be same as {G} - to grasp the goal object. Calculate T
B
WR
to achieve that
pose.

I T
H
G
=

1 1
= T T T T
WR
H
WO
G
WO
B
B
WR

Use transform graphs to check results for both cases.

40
Denavit-Hartenberg (DH) Parameters

Standard description of the geometric configuration of joints and links in a serial
robot. Paul (1983) interpretation of DH Parameters different from Craig's (1989).

4 parameters to describe completely, in general, the pose relationship between
consecutive frames {i} w.r.t. {i-1}. Joints {i-1} and {i} connected by link {i-1}.
Cartesian coordinate frame {i-1} rigidly attached to link {i-1}; moves with joint i-1.

J. Denavit and R.S. Hartenberg, 1955, "A Kinematic Notation for Lower-Pair
Mechanisms Based on Matrices", Journal of Applied Mechanics, pp. 215 - 221.

4 parameters (two rotations, two translations). One control variable out of the 4. 4
transform operators to change from {i-1} to {i} (or, give pose of {i} w.r.t. {i-1}).

Frame Attachment Conventions:

1) Z axis is along the rotation direction for revolute joints, along the translation
direction for prismatic joints.

2) X axis is directed along the unique common normal between consecutive Z
axes. If consecutive Z axes intersect, X must be perpendicular to both, and there is
considerable freedom in defining X.

3) Y axis formed by right hand rule (Z and X known).

Showing only Z and X is sufficient, drawing is made clearer by NOT showing Y.

First Link:

Choose
0
Z along
1
Z , and ensure frames {0} and {1} are identical when the first
variable is 0.

Simplify Kinematics:

Kinematic base vs. physical base of robot.
Hand frame vs. wrist frame for terminating basic kinematic equations.

41
Drawing of General Link Connecting 2 Joints:

D-H Parameters:

1 i
: Angle between
1
i
Z to
i
Z
measured about
1
i
X

1 i
a : Distance from
1
i
Z to
i
Z
measured along
1
i
X

i
d : Distance from
1
i
X to
i
X
measured along
i
Z

i
: Angle between
1
i
X to
i
X
measured about
i
Z

Screw Pairs:
1 i
,
1 i
a align Z axes about and along
1
i
X

i
d ,
i
align X axes about and along
i
Z

General Result: Table of DH Parameters
Columns are joint/link index i and the 4 DH parameters.
Rows are the 4 DH parameters for each active joint/link in the serial chain: total of
n active joints/moving links.

i
1 i

1 i
a
i
d
i

1
2

i

n

In deriving DH parameters, it is useful to imagine what frame and link moves with each
active joint - do this along the entire serial chain. (First joint moves all others . . .)

42
DH Parameters Examples

Complete Examples

1. 3-axis Planar 3R Articulated Robot
X
Y
Z
0
0
1
X
1
X
2
X
3
X
H
Z
0
Z
2
Z
3
Z
H
Y
H
L
2
L
3
L
1
1
2

i
1 i

1 i
a
i
d
i

1 0 0 0
1

2 0
1
L 0
2

3 0
2
L 0
3

Issues:
Links in different planes, but shift all X axes to align (d
i
=0)
What happened to L
3
?

43
2. 3-axis Spatial PRP Cylindrical Robot

X
0
Z
0
1
X
Z Z
1 2
L
2
X
2
Z
3
1
L
X
3
2 1
Z Z
Top
Front

i
1 i

1 i
a
i
d
i

1 0 0
1
L 0
2 0 0 0
o
90
3
o
90
0
2
L 0

44
3. 6-axis Spatial 6R Unimation PUMA Robot

i
1 i

1 i
a
i
d
i

1 0 0 0
1

2
o
90
0
0
L
o
90
2

3 0
1
L 0
o
90
3
+
4
o
90
0
2
L
4

5
o
90
0 0
5

6
o
90
0 0
o
90
6
+

45
Issue: Joint angle offsets

The active joint angle variable for a revolute joint is defined as:

i
: Angle between
1
i
X to
i
X
measured about
i
Z

The kinematic diagrams generally show the zero value definition for each joint angle
parameter. In the zero angle configuration, if the
1
i
X and
i
X
axes are NOT identical

when 0 =
i
, then a (constant) joint angle offset is required to account for this. Examples
of this may be seen in the 2
nd
row for the Cylindrical Robot and the 2
nd
, 3
rd
, and 6
th
rows
for the PUMA Robot above.

46
Incomplete Examples
(only coordinate frames are shown - DH Parameters intentionally left blank)

4. 7-axis Spatial 7R Fanuc S10 Robot

i
1 i

1 i
a
i
d
i

1
2
3
4
5
6
7

47
5. 8-axis Spatial 8R NASA AAI ARMII Robot

i
1 i

1 i
a
i
d
i

1
2
3
4
5
6
7
8

48
Very Incomplete Examples for practice!
(not even coordinate frames are shown - DH Parameters intentionally left blank)

6. 7-axis Spatial 7R NASA Flight Telerobotic Servicer (FTS) Robot

i
1 i

1 i
a
i
d
i

1
2
3
4
5
6
7

49
Forward Pose Kinematics

Given numerical values for all the joint variables, find the pose (position and
orientation) of the end-effector Cartesian coordinate frame (or other frame of interest)
with respect to the base Cartesian coordinate frame.

Given ; Find (4x4 Homogeneous Transformation Matrix)

Above assumes all revolute joints with given joint angles if there are prismatic joints, a
joint length value must be given for those joints.
Non-linear (transcendental) expressions, but solution is straight-forward all terms
(joint angles) inside sines and cosines are given. We will use concatenation of matrices
(consecutive link transformations) matrix multiplication to find result. Result can be
evaluated numerically or symbolically. Preferred: symbolic expressions of the Forward
Pose Kinematics result that can be implemented numerically in Matlab.
Forward Pose Kinematics is useful for robot simulation and sensor-based control.
The problem is made easy by isolating the problem to the pose of one frame w.r.t. its
previous neighbor along the serial chain use one row of the DH parameters table to
determine this. Then simply repeat for all moving links/joints w.r.t their previous
neighbor and multiply the whole thing together.

Derivation of Consecutive Link Transformations:
Define frame {i} with respect to frame {i-1}: T
i
i
1
. Attach 3 intermediate frames
{P}, {Q}, and {R}. Figure:

From {i-1} to {i}: Rotate
1 i
from
1
i
Z to
P
Z
about
1
i
X .
Translate
1 i
a from
P
Z
to
Q
Z
along
P
X
.
Translate
i
d from
Q
X
to
R
X
measured along
Q
Z
.
Rotate
i
from
Q
X
to
R
X
about
R
Z
.

50

(
(
(
(
1 0 0 0
0
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
c d c s c s s
s d s c c c s
a s c
T

Physical interpretation:
i
i i
i
R P
1 1
,
(Craig Equation 3.6)

( ) ( )
i i Z i i X
i
i
d Screw a Screw T , ,
1 1
1

=
( ) ( ) ( ) ( ) ( )
(
(
(
(
= = =

1 0 0 0
0 0
0 0
0 0 1
,
1 1
1 1
1
1 1 1 1 1 1
i i
i i
i
i X i X i X i X i i X
c s
s c
a
R a D a D R a Screw

( ) ( ) ( ) ( ) ( )
(
(
(
(

= = =
1 0 0 0
1 0 0
0 0
0 0
,
i
i i
i i
i Z i Z i Z i Z i i Z
d
c s
s c
R d D d D R d Screw

Caution: screws commute (order of translation and rotation doesn't matter), but matrices
don't in general. Also, cannot commute the X and Z screws, they must appear as in the
equations above.

51
Forward Pose Kinematics Result:

Concatenate Consecutive Homogeneous Transformation Matrices:

T
N
0
gives the pose (position and orientation) of the last active joint/link Cartesian
coordinate frame {N} with respect to the kinematic base Cartesian coordinate frame,
attached to link {0}. It is a function of all N joint variables:

There are 2 different representations:
N N
X T
0 0
, where
{ }
T
N
z y x X =
0
, and we use Z-Y-X () Euler angles convention.

Sum of Angles Simplification

If any two (or more) consecutive Z axes are parallel (i.e. consecutive
i
rotate
about parallel axes), we can simplify the resulting symbolic Forward Pose Kinematics
expressions by using sum of angle formulas:

( )
( ) casb sacb b a
sasb cacb b a
=
=
sin
cos m

Many common industrial robots have this parallel axes characteristic for at least one pair.
First multiply together any two individual Homogeneous Transformation Matrices that
represent consecutive parallel axes (take care to keep the proper matrix multiplication
order; i.e. use the associative property of matrix multiplication, DO NOT commute the
order of matrices!). Then use the above trig formulas to simplify before completing the
other matrix multiplications.

For more details see the Planar 3R and PUMA examples that follow.

52
Additional, fixed transforms

The above T
N
0
result is for the active joints only often we need to expand this
result to include additional transformations that are constant. For example, the kinematic
base frame {0} may be located at the shoulder of the robot, while another base frame {B}
mounted on the floor may be convenient. Also, the Forward Pose Kinematics
expressions will be simplest if {N} is located at the last active wrist joint; if a tool,
gripper, or other end-effector is attached we need another frame of interest (say {H} for
hand) attached; {H} is rigidly connected to {N} (i.e. no more joints in between) but offset
some distance.

The overall Forward Pose Kinematics Homogeneous Transformation Matrix is
given in generic form below. Note that the fixed matrices T
B
0
and T
N
H
are not
determined by DH parameters since there is no active joint represented by them. Simply
determine these matrices by inspection. Make the orientation identical if at all possible.
Please see the examples for more details. Overall transform equation:

53
Forward Pose Kinematics Examples

Forward Pose Kinematics Symbolic Derivations
Given
3 2 1
, , , calculate T
0
3
and T
H
0
. Plug each row of DH table into Eq. 3.6.

i
1 i

1 i
a
i
d
i

1 0 0 0
1

2 0
1
L 0
2

3 0
2
L 0
3

(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0 0
1 1
1 1
0
1
c s
s c
T
(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0
2 2
1 2 2
1
2
c s
L s c
T
(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0
3 3
2 3 3
2
3
c s
L s c
T

( ) ( ) ( )
(
(
(
(
+
+
= =
1 0 0 0
0 1 0 0
0
0
12 2 1 1 123 123
12 2 1 1 123 123
3
2
3 2
1
2 1
0
1
0
3
s L s L c s
c L c L s c
T T T T (interpret geometrically)

Simplification of rotation part using sum of angles formulae (all 3 active Z axes are
always parallel): e.g. [1,1] term:

Original multiplication: ( ) ( )
3 2 1 2 1 3 2 1 2 1
s c s s c c s s c c +

First simplification:
3 12 3 12
s s c c

Second simplification:
123
c

( )
( )
3 2 1 123
3 2 1 123
sin
cos

+ + =
+ + =
s
c

( )
( )
2 1 12
2 1 12
sin
cos

+ =
+ =
s
c

Trigonometric Sum-of-Angles Formulae:
( )
( ) casb sacb b a
sasb cacb b a
=
=
sin
cos m

54
What happened to L
3
?: Additional, fixed transform

[ ]
(
(
(
(
=
1 0 0 0
0 1 0 0
0 0 1 0
0 0 1
3
3
L
T
H
(by inspection from the planar 3R figure)

For this robot we need to control the {H} frame. However, there is no separate base
frame {B}, i.e. {0} is sufficient. The overall Forward Pose Kinematics Homogeneous
Transformation Matrix is given below. L
3
is a constant since the third link is rigid.

[ ] ( ) [ ] ( ) [ ]
3
3
3 2 1
0
3
0
, , L T T T
H H
=

[ ]
(
(
(
(
(
(
(
(
+
+
=
1 0 0 0
0 1 0 0
0 0 1 0
0 0 1
1 0 0 0
0 1 0 0
0
0
3
12 2 1 1 123 123
12 2 1 1 123 123
0
L
s L s L c s
c L c L s c
T
H

[ ]
(
(
(
(
+ +
+ +
=
1 0 0 0
0 1 0 0
0
0
123 3 12 2 1 1 123 123
123 3 12 2 1 1 123 123
0
s L s L s L c s
c L c L c L s c
T
H

Position vector significantly more complicated; orientation identical.

55
Forward Pose Kinematics Example

1) Given 1 , 2 , 3
3 2 1
= = = L L L and
o o o
35 , 25 , 15
3 2 1
= = = , calculate T
H
0
.

T T T
H H
3 0
3
0
=

(
(
(
(

=
1 0 0 0
0 1 0 0
062 . 2 0 259 . 0 966 . 0
430 . 4 0 966 . 0 259 . 0
0
3
T ;
[ ]
(
(
(
(
=
1 0 0 0
0
0
1
3
I
T
H

(
(
(
(

=
1 0 0 0
0 1 0 0
028 . 3 0 259 . 0 966 . 0
689 . 4 0 966 . 0 259 . 0
0
T
H

2) Given 1 , 2 , 3
3 2 1
= = = L L L and
o o
0 , 90
3 2 1
= = = , calculate T
H
0
.

(
(
(
(

=
1 0 0 0
0 1 0 0
5 0 0 1
0 0 1 0
0
3
T same T
H
=
3

(
(
(
(

=
1 0 0 0
0 1 0 0
6 0 0 1
0 0 1 0
0
T
H

This second example is sufficiently simple to check your results by a sketch be sure to
include the {H} and {0} axes to check the orientation R
H
0
in addition to the position
vector
H
P
0
. Actually, since the robot is planar, you should check the first example too
using a sketch!

56
Given
2 1
, , L L , calculate T
0
3
. Plug each row of DH table into Eq. 3.6.

i
1 i

1 i
a
i
d
i

1 0 0
1
L 0
2 0 0 0
o
90
3
o
90
0
2
L 0

(
(
(
(
=
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1
0
1
L
T
(
(
(
(
=
1 0 0 0
0 1 0 0
0 0
0 0
1
2

s c
c s
T
(
(
(
(
=
1 0 0 0
0 0 1 0
1 0 0
0 0 0 1
2 2
3
L
T

(
(
(
(
=
1 0 0 0
0 1 0
0
0
1
2
2
0
3
L
s L s c
c L c s
T

(interpret geometrically)

In this case the point of interest is the origin of {3} so no {H} frame is required.

57
Forward Pose Kinematics Example

1) Given 2 , 30 , 3
2 1
= = = L L
o
, calculate T
0
3
.

(
(
(
(
=
1 0 0 0
3 0 1 0
1 5 . 0 0 866 . 0
732 . 1 866 . 0 0 5 . 0
0
3
T

2) Given 2 , 90 , 3
2 1
= = = L L
o
, calculate T
0
3
.

(
(
(
(
=
1 0 0 0
3 0 1 0
2 1 0 0
0 0 0 1
0
3
T

Check both results with sketches (top and front views) be sure to include the {3} and
{0} axes to check the orientation R
0
3
in addition to the position vector
3
0
P .

58
3. 6-axis Spatial 6R Unimation PUMA Robot
Given
6 5 4 3 2 1
, , , , , , calculate T
0
6
and T
B
H
. Plug each row of DH table into
Eq. 3.6.

i
1 i

1 i
a
i
d
i

1 0 0 0
1

2
o
90
0
0
L
o
90
2

3 0
1
L 0
o
90
3
+
4
o
90
0
2
L
4

5
o
90
0 0
5

6
o
90
0 0
o
90
6
+

(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0 0
1 1
1 1
0
1
c s
s c
T
(
(
(
(
=
1 0 0 0
0 0
1 0 0
0 0
2 2
0
2 2
1
2
s c
L
c s
T
(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0
3 3
1 3 3
2
3
s c
L c s
T

(
(
(
(
=
1 0 0 0
0 0
1 0 0
0 0
4 4
2
4 4
3
4
c s
L
s c
T
(
(
(
(
=
1 0 0 0
0 0
0 1 0 0
0 0
5 5
5 5
4
5
c s
s c
T
(
(
(
(

=
1 0 0 0
0 0
0 1 0 0
0 0
6 6
6 6
5
6
s c
c s
T

[ ] ( ) [ ] ( ) [ ] ( ) [ ] ( ) [ ] ( ) [ ] ( ) [ ]
6
5
6 5
4
5 4
3
4 3
2
3 2
1
2 1
0
1
0
6
T T T T T T T =

[ ] ( ) [ ] ( ) [ ]
6 5 4
3
6 3 2 1
0
3
0
6
, , , , T T T =

( ) [ ] ( ) [ ] ( ) [ ]
3 2
1
3 1
0
1 3 2 1
0
3
, , , T T T =
(take advantage of parallel Z axes, 2 and 3 sum-of-angles formula)

( ) [ ]
(
(
(
(
(
(
(
(

=
1 0 0 0
0
1 0 0
0
1 0 0 0
0 1 0 0
0 0
0 0
, ,
2 1 23 23
0
2 1 23 23
1 1
1 1
3 2 1
0
3
c L c s
L
s L s c
c s
s c
T

59
( ) [ ]
(
(
(
(

+
+
=
1 0 0 0
0
, ,
2 1 23 23
2 1 1 1 0 1 23 1 23 1
2 1 1 1 0 1 23 1 23 1
3 2 1
0
3
c L c s
s s L c L c s s c s
s c L s L s s c c c
T

( ) [ ] ( ) [ ] ( ) [ ] ( ) [ ]
6
5
6 5
4
5 4
3
4 6 5 4
3
6
, , T T T T =
(no parallel Z axes 4, 5, and 6 NO sum-of-angles formula)

( ) [ ]
(
(
(
(

=
1 0 0 0
0
0
, ,
5 4 6 5 4 6 4 6 5 4 6 4
2 5 6 5 6 5
5 4 6 5 4 6 4 6 5 4 6 4
6 5 4
3
6
s s c c s s c s c s c c
L c c s s s
s c c c c s s s c c c s
T

It is left to the reader to perform the symbolic multiplication [ ] [ ][ ] T T T
3
6
0
3
0
6
= ; the result is
very complicated use symbolic math on the computer (e.g. Matlab Symbolic Toolbox).
Alternatively, these two matrices can be evaluated and multiplied numerically.

Two additional, fixed transforms

The basic Forward Pose Kinematics result is [ ] T
0
6
. 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 [ ] T
0
6
. 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.

[ ] ( ) [ ] ( ) [ ] ( ) [ ]
H H B
B B
H
L T T L T T
6
6 5 4 3 2 1
0
6 0
, , , , , =

( ) [ ]
(
(
(
(
=
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
0
B
B
B
L
L T ( ) [ ]
(
(
(
(
=
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
6
H
H H
L
L T

60
Inverse Pose Kinematics

Given numerical values for the pose (position and orientation) of the end-effector
Cartesian coordinate frame (or other frame of interest) with respect to the base Cartesian
coordinate frame, find the required joint variables to achieve this pose.

Given: Find:

Non-linear, coupled equations to solve in trancendentals of the unknowns; solution
is generally difficult. Multiple solutions exist. Analytical solutions do not exist for some
manipulator structures. Inverse Pose Kinematics is required for robot pose control. The
required equations to solve are from Forward Pose Kinematics.

Forward Position Kinematics of Serial Robots
Just concatenate consecutive link transformations, regardless of number of joints.
There is a unique solution that always exists. Can be found numerically or symbolically.

Inverse Position Kinematics of Serial Robots

Task space (Cartesian) freedoms m vs. Joint space freedoms n

a) m > n; overdetermined, NO solution exists (trying to operate with a
limited robot - can only cover a subspace of the task space).

b) m = n; determined, finite solutions exist (spans the task space just
right). This is the case we will consider in class.

c) m < n; underdetermined, infinite solutions exist (kinematic
redundancy). Can optimize performance in addition to following general pose
trajectories.

Number of solutions
For m = n, there are generally (finite) multiple solutions. For example, elbow up
or down. There are more than one manipulator configuration to achieve a given
Cartesian pose. Demonstrate with PUMA model.

61
Existence of solutions

For m = n or m < n, no real solution exists when the given Cartesian pose is out of
the manipulator's workspace. The solutions are complex (imaginary) in this case.

For m > n, a solution exists if the pose in within the workspace, and ONLY IF the
commanded pose is within the limited task space spanned by the manipulator. For
example, a 5-DOF (n=5) robot cannot turn a ratchet in general, but is sufficient for axis-
symmetric applications (such as spray painting). Actually this is still the m = n case
because m is reduced to m=5 for axis-symmetric Cartesian tasks not requiring the robot
roll freedom.

Solution Methods

Numerical e.g. Newton-Raphson. Requires a good initial guess and only finds
one solution, closest to the initial guess.

Analytical (closed-form)

Algebraic vs. Geometric, combination of the two

Pose Kinematics of Parallel Robots

There is an interesting duality with serial robots: For parallel robots, the forward
kinematics solution is generally coupled and non-linear, while the inverse kinematics
solution is generally more straight-forward. This is reversed for serial robots. (See the
notes section on Parallel Manipulators.)

Tan-half angle substitution useful in solving inverse pose kinematics (stay tuned)

Pieper's solution if three consecutive axes are intersecting in a serial robot, the inverse
pose solution is guaranteed to exist analytically.

62
General Solution Procedure for Inverse Pose Kinematics of Serial Robots

Solution of the Inverse Pose Kinematics starts with the same expressions from
Forward Pose Kinematics. Write 16 equations (equate two Homogeneous
Transformation Matrices). The LHS matrix are known, given numbers. The RHS matrix
elements are symbols (from the Forward Pose Kinematics Homogeneous Transformation
Matrix). Now the Cartesian pose T
N
0
is known (commanded) and the joint variables
{ }
T
N
L , , ,
3 2 1
are unknown:

Spatial Serial Robots
16 equations, 4 trivial (row 4) = 12 equations.

3 translational equations (column 4) all 3 are independent.

9 rotational equations - only 3 are independent.

Planar
16 equations, 4 trivial (row 4) = 12 equations.

6 more equations are trivial (row 3, plus column 3) = 6 equations.

2 translational equations (column 4) both are independent.

4 rotational equations - only 1 is independent.

If m=n (the number of Cartesian and joint freedoms match) we can solve the problem, in
principle. Maybe not in closed-form (analytical); maybe the solution has to be numerical.

63
Inverse Pose Kinematics Examples


X
Y
Z
0
0
1
X
1
X
2
X
3
X
H
Z
0
Z
2
Z
3
Z
H
Y
H
L
2
L
3
L
1
1
2

Inverse Pose Kinematics Symbolic Solution

Given: , calculate:

Forward Kinematics Expressions:

Inverse Pose Kinematics Solution Methods:
1) Graphical (demonstrate with model kit)
2) Geometric
3) Algebraic/trigonometric we will now follow this method to solve the
problem.

64
First, Simplification take advantage of fixed L
3
transform

Inverse Pose Equations to Solve

Subspace of general 3D pose space:

Note that the Forward Pose Kinematics result can be represented by T
0
3
or by
{ }
T
y x X
3 3
= . The given LHS matrix cannot be a general 3D pose since this is a
planar problem. The expression of the subspace of general 6-dof poses spanned by the
XY plane and described by T
0
3
is given below:

Equations to Solve:

Write 2 independent translational equations:

Write 2 rotational equations, only 1 is independent:

Rotational equations:

65

Actually, the simplest rotational equation to use is: =

We have three equations to solve for the three unknowns. Thanks to the fixed transform
simplification, the two translational equations involve only
2 1
, (fully coupled in these
unknowns); the rotational equation involves all three unknowns. Therefore, let us solve
for
2 1
, first from the translational equations and find
3
last.

Rearrange translational equations:

Square and add these rearranged translational equations to eliminate
2
:

The result is one equation in one unknown
1
:

We can solve this equation for
1
by using the Tangent Half-Angle Substitution.

2
tan
1
= t
2
2
1
1
1
cos
t
t
+
=
2
1
1
2
sin
t
t
+
=

Substitute into the EFG equation:

0
1
2
1
1
2 2
2
= + |
.
|
\
|
+
+
|
|
.
|
\
|
+
G
t
t
F
t
t
E ( ) ( ) ( ) 0 1 2 1
2 2
= + + + t G t F t E

( ) ( ) ( ) 0 2
2
= + + + E G t F t E G quadratic formula:
E G
G F E F
t
+
=
2 2 2
2 , 1

66

Solve for
1
by inverting the original Tangent Half-Angle definition:

Two
1
solutions result from the in the quadratic formula; both are correct
(multiple solutions elbow up and elbow down).

To find
2
, return to original arranged translational equations:

Find the unique
2
for each
1
value (use quadrant-specific atan2 function):

Now we have two solutions for ( )
2 1
, ; return to rotational equation:

There are 2 overall solutions to the inverse pose kinematics problem for the planar
3R robot. Be sure to keep subindices lined up, i.e. ( )
1
3 2 1
, , and ( )
2
3 2 1
, , .

67

1) Given T
H
0
and 1 , 2 , 3
3 2 1
= = = L L L , calculate ( ) 2 , 1 , , ,
3 2 1
= i
i
.

(
(
(
(

=
1 0 0 0
0 1 0 0
028 . 3 0 259 . 0 966 . 0
689 . 4 0 966 . 0 259 . 0
0
T
H

1 3 0
3
0 0
3

= = T T T T T
H H
H
H

[ ]
(
(
(
(

=
(
(
(
(

(
(
(
(

=
1 0 0 0
0 1 0 0
062 . 2 0 259 . 0 966 . 0
430 . 4 0 966 . 0 259 . 0
1 0 0 0
0
0
1
1 0 0 0
0 1 0 0
028 . 3 0 259 . 0 966 . 0
689 . 4 0 966 . 0 259 . 0
0
3
I
T

1) Answers (deg):

i 1

2

3

1 15 25 35
2 35 -25 65

The first line is expected since this is the same pose as the planar 3R robot Forward Pose
Kinematics example 1). Check the second solution to ensure it is correct (plug into
Forward Pose Kinematics and ensure you get the same T
H
0
back).

68
2) Given T
H
0
and 1 , 2 , 3
3 2 1
= = = L L L , calculate ( ) 2 , 1 , , ,
3 2 1
= i
i
.

(
(
(
(

=
1 0 0 0
0 1 0 0
6 0 0 1
0 0 1 0
0
T
H
;
(
(
(
(

=
1 0 0 0
0 1 0 0
5 0 0 1
0 0 1 0
0
3
T

2) Answers (deg):

i 1

2

3

1 90 0 0

There is only one solution (why?).

3) Given T
H
0
and 1 , 2 , 3
3 2 1
= = = L L L , calculate ( ) 2 , 1 , , ,
3 2 1
= i
i
.

(
(
(
(

=
1 0 0 0
0 1 0 0
928 . 6 0 866 . 0 5 . 0
0 . 4 0 5 . 0 866 . 0
0
T
H
;
(
(
(
(

=
1 0 0 0
0 1 0 0
428 . 6 0 866 . 0 5 . 0
134 . 3 0 5 . 0 866 . 0
0
3
T

3) Answers (deg):

Impossible! (why? complex result) Out of manipulator workspace.

69
Inverse Pose Kinematics Examples (cont.)

X
0
Z
0
1
X
Z Z
1 2
L
2
X
2
Z
3
1
L
X
3
2 1
Z Z
Top
Front

Inverse Pose Kinematics Symbolic Solution
Given T
0
3
, calculate
2 1
, , L L .
Forward Kinematics Solution:
(
(
(
(
=
1 0 0 0
0 1 0
0
0
1
2
2
0
3
L
s L s c
c L c s
T

Inverse pose equations come from here forward pose expressions:

(
(
(
(
=
(
(
(
(
1 0 0 0
0 1 0
0
0
1 0 0 0
1
2
2
33 32 31
23 22 21
13 12 11
L
s L s c
c L c s
p r r r
p r r r
p r r r
z
y
x

70
This robot has spatial motion, but rotation is limited to planar. Subspace of general 6-dof
pose (where the pose is represented by T
0
3
or by { }
T
z y x X
3 3 3
= :

(
(
(
(
=
(
(
(
(
1 0 0 0
0 1 0
0
0
1 0 0 0
0 1 0
0
0
1
2
2
3
3
3
L
s L s c
c L c s
z
y s c
x c s

We have a problem there are only 3 joints (n=3) but there are m=4 Cartesian values.
This is an overconstrained problem and no solution exists in general a dependency
exists among { }
T
z y x X
3 3 3
= and all four cannot be commanded independently.
Therefore, let us command only 3 Cartesian values { }
T
red
z y x P X
3 3 3 3
0
= = ; we will
treat this robot as a translational freedom robot. is not independent but is related to x
3

and y
3
. The three equations to solve for the three unknowns are then taken only from the
translational equations:

1 3
2 3
2 3
L z
s L y
c L x
=
=
=

Inverse Pose Kinematics Solution:

1)
3 1
z L =

2)
3
3
2
2
tan
cos
sin
x
y
L
L
= =
( )
3 3
, x y atan2 =

3)
2
3
2
3
2 2
2
2 2
2
sin cos y x L L + = +
2
3
2
3 2
y x L + =

Mathematically, there are two solution sets; the
2
L solution is not a practical choice.
(If
2
L is allowed, then + is the angle solution).

This is not a general spatial manipulator, i.e.
3
0
P and R
0
3
cannot be specified
independently.

71


1) Given
3
0
P , calculate
2 1
, , L L .
=
3
1
732 . 1
3
3
3
3
0
z
y
x
P

1) Answers (m and deg):

i
1
L

2
L

1 3 30 2
2 3 210 -2

The first line is expected since this is the same position as the planar 3R robot Forward
Pose Kinematics example 1). Check the second solution to ensure it is correct (plug into
Forward Pose Kinematics and ensure you get the same
3
0
P back). Also, for both cases
you can calculate R
0
3
- they will be different (why?).

2) Given
3
0
P , calculate
2 1
, , L L .
=
3
2
0
3
3
3
3
0
z
y
x
P

2) Answers (m and deg):

i
1
L

2
L

1 3 -90 2
2 3 90 -2

72

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 - three wrist actuators rotate about
a common point. In this case, the position and orientation sub-problems may be
decoupled. Solve the position first (1st 3 joints) and then solve the orientation second
(2nd 3 joints, the wrist) based on the orientation of the first 3 joints. Coordinate frame of
resolution must be located at the wrist point - this is general, because can make
transformation from any other frame rigidly connected to the last joint.

PUMA Example (no details):

Given T
0
6
, calculate
6 5 4 3 2 1
, , , , , .

( )
6 5 4 3 2 1
0
6
, , , , , f T =

( ) ( )
(
(
(
(
=
1 0 0 0
, , , , , , ,
3 2 1 6
0
6 5 4 3 2 1
0
6 0
6
P R
T

Joints 4, 5, and 6 cannot affect translation of wrist origin.

1) Translational equations: Given
6
0
P , calculate
3 2 1
, , values (4 sets).
3 independent equations, 3 unknowns.

2) Rotational equations: Given R
0
6
, and now knowing
3 2 1
, , , calculate
6 5 4
, , values (2 sets).
9 dependent equations, 3 unknowns.

( ) ( ) ( )
6 5 4 3 2 1
0
6 3 2 1
0
3 6 5 4
3
6
, , , , , , , , , R R R
T
=

73
4 sets of
3 2 1
, , ; 2 sets of
6 5 4
, , for each. Therefore, there are 8 overall
solutions to the inverse position problem for the PUMA. Some may lie outside joint
ranges. Generally choose closest solution to previous position.

Peel-off homogeneous transformations matrices with unknowns to separate
variables.

( ) ( ) ( ) ( ) ( ) ( )
6
5
6 5
4
5 4
3
4 3
2
3 2
1
2 1
0
1
0
6
T T T T T T T= ; LHS is numbers.

( ) [ ] ( ) ( ) ( ) ( ) ( )
6
5
6 5
4
5 4
3
4 3
2
3 2
1
2
0
6
1
1
0
1
T T T T T T T =
; can get 1 and 3.

( ) [ ] ( ) ( ) ( )
6
5
6 5
4
5 4
3
4
0
6
1
2
0
3
T T T T T =
; get 2, with 1 and 3 known.

( ) [ ] ( ) ( )
6
5
6 5
4
5
0
6
1
4
0
4
T T T T =
; isolate and solve 4, 5, and 6.

74


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

75
m=n=6; let us choose 0
5 3
= = for all motion to accomplish this. Then we have a
determined Inverse Pose Kinematics problem, still with multiple solutions (but finite).
The Forward Pose Kinematics relationship is:

So, the first step should be to simplify the equations as much as possible by calculating
the required [ ] T
0
8
to achieve the commanded [ ] T
B
H
:

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:

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.

Inverse Pose Kinematics Symbolic Solution for Arm Joints Only, with 0
3
=

Reduced problem statement:
Given { } { }
T B
z y x P
5 5 5 5
= , calculate ( )
i
4 2 1
, , , 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 { } { }
8 5
P P
B B
= due to the spherical wrist.

76
The equations to solve for this problem come from the Forward Pose Kinematics
relationships for the ARMII robot, the translational portion only (further, with 0
3
= ):

This equation yields (proof is left to student, use symbolic Forward Pose Kinematics):

{ }
( )
( )
+ +
+
+
=
=
24 5 2 3
24 5 2 3 1
24 5 2 3 1
5
5
5
5
c d c d d
s d s d s
s d s d c
z
y
x
P
B
B

where:
( )
( )
4 2 24
4 2 24
sin
cos

+ =
+ =
s
c

(Since 0
3
= always, the Z axes of 2 and 4 are always parallel and we used the sum-of-
angles trig formulas.)

Solution Process:

1. Ratio of Y to X yields:

+
1
is also a valid solution

2. Since
1
is now known (two values), we can modify the Y and Z equations:

where:
B
d z Z
s
y
Y
=
=
5
1
5
(case-sensitive!)

Isolate ( )
4 2
+ terms:

Square & Add to eliminate
4
:

77

The result is one equation in one unknown
2
:
0 sin cos
2 2
= + + G F E

where:
2
5
2
3
2 2
3
3
2
2
d d Z Y G
Yd F
Zd E
+ =
=
=

We can solve this equation for
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 q
2

(in that section, it was for q
1
).

Solve for
2
by inverting the original Tangent Half-Angle definition:

Two
2
solutions result from the in the quadratic formula; both are correct
(multiple solutions elbow up and elbow down).
To find
4
, return to original (arranged) translational equations:

2 3 24 5
2 3 24 5
c d Z c d
s d Y s d
=
=

Find the unique
4
for each
2
value (use quadrant-specific atan2 function):

78
Solutions Summary

The solution is now complete for the ARMII robot reduced inverse pose problem
(translational joints only, plus 0
3
= ).

There are multiple solutions: two values for
1
; for each
1
, there are two values
for
2
; for each valid ( )
2 1
, , there is a unique
4
. So there are a total of 4 ( )
4 2 1
, ,
solution sets for this reduced problem. We can show this with the PUMA model (its not
the same robot, but close enough . . .).

But wait, theres more! These 4 solution sets occur in a very special arrangement
pattern, summarized in the table below.

i 1

2

3

4

1 1

1
2

0 4

2 1

2
2

0 4

3
+
1

2
2

0 4

4
+
1

1
2

0 4

Dont take my word for it in all numerical examples, you can check the results;
plug all solution sets ( )
4 2 1
, , one at a time into the Forward Pose expressions and
verify that all sets yield the same, commanded { }
5
P
B
. You can also calculate the
associated R
B
4
- all should be different (why?).

79


Inverse Pose Kinematics Example

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

=
6952 . 1
1159 . 0
6572 . 0
5
5
5
5
z
y
x
P
B

Answers

i 1

2

3

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
P
B
.

80

Velocity Analysis and Jacobian Matricess

Up to now, our robotics mathematics has been just in the pose domain. Velocity is
the first time derivative of position (translational and rotational velocity are both 3x1
vectors in the 3D world). Velocity requires linearized analysis it is more straight-
forward than pose solutions.

Velocity is useful for many topics in robotics:
Resolved Rate Control
Velocity of any point on manipulator
Moving objects in workspace
Dynamics

Pose
Translational: Rotational, e.g. 3-2-1 Euler angles , ,

Position is a vector Orientation is not a vector!

Velocity
Vector time rate of change of position and orientation.

Translational Orientation, e.g. 3-2-1 Euler &
&
& , , : nonholonomic

Translational Velocity
Three coordinate frames involved (2 points and a frame): ( )
j
i
k
V is the translational
velocity of point j (origin of {j}) w.r.t. point i (origin of {i}), expressed in the basis
(coordinates) of {k}. Actually, for position vectors, three frames were also involved, just
one was implicit:
{ }
j
i
P is the position vector from the origin of {i} to the origin of {j}, expressed in
{i} coordinates. So, this is more properly { }
j
i
k
P , where {k} is implicitly assumed to be
{i}, unless otherwise stated.

81

Translational velocity example
To demonstrate the various indices in ( )
j
i
k
V . A is fixed reference frame.

Given: Absolute velocity of B
o
90 @ 2
s
in
Absolute velocity of C
o
45 @ 4
s
in

Figure:

From the given information:
( )
=
0
2
0
B
A
A
V ; ( )
=
0
83 . 2
83 . 2
C
A
A
V

From relative velocity equation:
( ) ( ) ( )
C
B
A
B
A
A
C
A
A
V V V + = ( ) ( ) ( )
= =
0
83 . 0
83 . 2
B
A
A
C
A
A
C
B
A
V V V ;
o
16 @ 95 . 2
s
in

Now, these same velocity vectors (
C
B
C
A
B
A
V V V , , )can be expressed in {B} and
{C} by: (velocity is a free vector, just coordinate rotation, no translation)

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

( )
=
0
0
2
B
A
B
V ( )
=
0
83 . 2
83 . 2
C
A
B
V ( )
=
0
83 . 2
83 . 0
C
B
B
V

( )
=
0
41 . 1
41 . 1
B
A
C
V ( )
=
0
0
4
C
A
C
V ( )
=
0
43 . 1
58 . 2
C
B
C
V

82

There are 27 permutations for A, B, C. So far, we have given 9 combinations. 2
more types:
1) ( ) ( )
j
i
k
i
j
k
V V = e.g. ( )
=
0
2
0
A
B
A
V (9 of these)

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

Transport Theorem:
P
dt
dP
dt
dP
B
A
B A
+ =

( ) P
dt
dP
dt
dP
B
A
k
B
k
A
k
+
|
|
.
|
\
|
=
|
|
.
|
\
|

General three-part velocity equation:

P T P
P R P P
B A
B
A
B A
B B
A A
=
+ =

P R P R P V
B A
B
B A
B B
A A
& & &
+ + =

P V V V
P
+ + =
0

Adding descriptive indices:

( ) ( ) ( ) ( ) P R V R V V
B A
B B
A
A
P
B A
B B
A
A
P
A
A
+ + =

A rotating rigid body has different translational velocities at different points; however, it
has the same rotational velocity over the whole rigid body.

83

Rotational Velocity

Angular Velocity Vector:
( )
B
A
A
is the angular velocity of Cartesian coordinate frame {B} with respect to
{A}, expressed in {A} coordinates. ( )
B
A
A
is a unique vector description.
Nonholonomic: cannot integrate ( )
B
A
A
to get an orientation vector; in fact,
there is no such thing as an orientation vector.

{ } { }
T
z y x
T
z y x

& & &
= , but there is a relationship.

Kinematic Differential Equations in terms of Orientation Angles

e.g. Z-Y-X ( , , ) Euler angle convention:

(
(
(
+ +
+ +
=

c c s c s
R
A
B

From Spacecraft Dynamics by Kane, Likins, and Levinson:

( )
(
(
(
&
&
&
0
0
1 0
s c c
c s c
s
z
y
x
B
A
A

where { }
T
&
&
& are the Euler angle rates.
Somewhat artificial, 23 other descriptions (Euler and Fixed). Inverse relationship:
(
(
(
(
(
z
y
x
t c t s
s c
c
c
c
s
1
0
0
&
&
&

Artificial singularities:
o
90 = .

A rotating rigid body has the same rotational velocity at different points.

84

Angular velocity vector equation for multiple frames:

C
B
B
A
C
A
+ =

Put in same frame:

( ) ( ) ( ) ( ) ( )
C
B A
B B
A
A
C
B
A
B
A
A
C
A
A
R + = + =

or, if it is more convenient to express in the local frames (as in robot joints):

( ) ( ) ( )
C
B
C
A
C B
A
B
A
B C
A
A
R R + =

Combined Translational and Rotational Velocity

where: both (3x1) vectors

85

Jacobian Matrices

Jacobian matrix is a linear transformation mapping joint to Cartesian velocities:

m = dimension of Cartesian (task) space
n = dimension of joint space

Express resulting Cartesian velocities in any frame {k};
&
are relative joint angle rates
and hence are expressed about different local Z axes. Velocity relationship:

Jacobian matrix is a multi-dimensional form of the derivative:

= X
k
&
=
&

3 ways to calculate Jacobian matrix:

1) Partial derivatives definition.

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!

86

2) Column as velocity due to joint i.
Physical interpretation of Jacobian matrix: each column i is the Cartesian
velocity vector of the end-effector frame w.r.t. the base frame, due to joint i only, and
with
i
&
factored out.

( ) ( ) ( )
(
(
(
=
| | |
| | |
0
2
0
1
0
L
&
L
& &
L
N
N N N
k
k
X X X J

( )
( )
( )
)
=
i
N
k
i
N
k
i
N
k V
X
0
0
0
&

A) Revolute Joint Columns

where
i
i k
i i
k
Z R Z

= is the 3
rd
column of R
k
i

and ( ) ( )
N
i
i
k
i N
i
k
P R P = where ( )
N
i
i
P is the translational part of T
i
N

Column i of the Jacobian matrix, for a revolute joint:

Jacobian matrix, for all-revolute manipulator:
( ) ( ) ( )
(
(

=
N
k
N
N
k
N
k
i
k
N
i
k
i
k
k
N
k
k
k
k
Z
P Z
Z
P Z
Z
P Z
J
1
1
1
L L

87

B) Prismatic Joint Columns

where
i
i k
i i
k
Z R Z

= is the 3
rd
column of R
k
i
.

Column i, for prismatic joint:

3) Velocity recursion a'la Craig.

Add translational and rotational velocities serially from base to 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
P v R v
Z R

&

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
Z d P v R v
R
&

Start with 0
0
0
0
0
= = v (if base is fixed).

Use recursion equations, 1 , , 2 , 1 , 0 = N i L .

Then, factor out { }
&
from
N
N
N
N
v , to get J
N
.

88

Jacobian Matrix Examples

X
Y
Z
0
0
1
X
1
X
2
X
3
X
H
Z
0
Z
2
Z
3
Z
H
Y
H
L
2
L
3
L
1
1
2

=
& &
J X
0 0
where:
=
Z
y
x
V
X
3
3
3
0
3
0
0
0
&
&
&
(m=n=3)

We will get the symbolically simplest velocity result by using the velocity of frame
{3} w.r.t frame {0}; velocities of the {H} frame can then be found from:

( ) ( ) ( )
( ) ( )
3
0
0
0
0
3
0
3 3
0
0
3
0
0
0
0
=
+ =
H
H
X L V V

89

Derive J
0
3 ways:

1) Partial derivatives definition J =

( ) =
)
`
= =
y
x
p
p
P f
3
0
0

j
j
j
i
j
j
j
i i
p
dt
d
p
t
p
&

= =
= =
3
1
3
1
i = x, y

=
1
x
p
=
2
x
p
=
3
x
p

=
1
y
p
=
2
y
p
=
3
y
p

( ) ( ) ( ) ( )
( ) ( )Z
3 2 1 3
0
0
3
2
0
2
1
0
1
0
0
3
0
0

& & &
+ + =
+ + =

= J
0

Resulting Velocity relationship (with details):

90

( ) ( ) ( )
(
(

=
3
0
3
3
0
3
0
2
0
3
2
0
2
0
1
0
3
1
0
1
0
0
Z
P Z
Z
P Z
Z
P Z
J

)
`
+
=
2 2
2 2 1
3
1
s L
c L L
P ( )
)
`
+
+
= =
12 2 1 1
12 2 1 1
3
1 0
1 3
1
0
s L s L
c L c L
P R P

)
`
=
0
2
3
2
L
P ( )
)
`
= =
12 2
12 2
3
2 0
2 3
2
0
s L
c L
P R P

)
`
=
0
0
3
3
P ( )
)
`
= =
0
0
3
3 0
3 3
3
0
P R P

( )
+

=
0
12 2 1 1
12 2 1 1
3
1
0
1
0
c L c L
s L s L
P Z ( )
=
0
12 2
12 2
3
2
0
2
0
c L
s L
P Z ( )
=
0
0
0
3
3
0
3
0
P Z

= = =
1
0
0

3
0
2
0
1
0
Z Z Z

(
(
(
(
(
(
(
+

=
3
2
1
12 2 12 2 1 1
12 2 12 2 1 1
1 1 1
0 0 0
0 0 0
0 0 0
0
0
&
&
&
&
&
&
c L c L c L
s L s L s L
z
y
x
z
y
x

Eliminate needless rows:
(
(
(
+

=
3
2
1
12 2 12 2 1 1
12 2 12 2 1 1
1 1 1
0
0
&
&
&
&
&
c L c L c L
s L s L s L
y
x
z

(
(
(
+

=
1 1 1
0
0
12 2 12 2 1 1
12 2 12 2 1 1
0
c L c L c L
s L s L s L
J

91

Revolute joints:

( )
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
P v R v
Z R

&

=
0
0
0
0
0

=
1
1
1
0
0
&

+
=
2 1
2
2
0
0

& &

+ +
=
3 2 1
3
3
0
0

& & &

=
0
0
0
0
0
v
( )
= + =
0
0
0
1
0
0
0
0
0 1
0 1
1
P v R v

( )
=
|
|
|
.
|
\
|
(
(
(
= + =
0 0
0
0
0
0
1 0 0
0
0
1 2 1
1 2 1
1 1 2 2
2 2
2
1
1
1
1
1 2
1 2
2

&
&
&
c L
s L
L c s
s c
P v R v

( ) ( )
( )
( )
+ +
+ +
=
|
|
|
|
.
|
\
|
+ +
(
(
(
= + =
0 0
0
0 1 0 0
0
0
2 3 2 1 3 2 23 1
2 3 2 1 3 2 23 1
2 1 2 1 2 1
1 2 1
3 3
3 3
3
2
2
2
2
2 3
2
3
3

& &
& &
& & &
&
c L c L c L
s L s L s L
L c L
s L
c s
s c
P v R v

(
(
(
+
+
=
1 1 1
0
0
3 2 3 2 23 1
3 2 3 2 23 1
3
c L c L c L
s L s L s L
J

92

2) Spatial 3P Cartesian manipulator L J X
& &
0 0
= { }
T
c b a L =

Derive
0
J 3 ways:
1) Partial derivatives definition
(
(
=
j
i
L
f
J

( )
= =
a
c
b
p
p
p
P f
z
y
x
3
0
0

0 =
a
p
x
1 =
b
p
x
0 =
c
p
x

0 =
a
p
y
0 =
b
p
y
1 =
c
p
y

1 =
a
p
z
0 =
b
p
z
0 =
c
p
z

( ) { } 0
3
0
0
=

(
(
(
=
0 0 1
1 0 0
0 1 0
0
J
c
b
a
J
z
y
x
&
&
&
&
&
&
0
0

93


(
(
=
0
3
0
2
0
1
0
0
Z Z Z
J

(
(
(
(
(
(
(
c
b
a
z
y
x
z
y
x
&
&
&
&
&
&
0 0 0
0 0 0
0 0 0
0 0 1
1 0 0
0 1 0

Eliminate needless rows:
(
(
(
c
b
a
z
y
x
&
&
&
&
&
&
0 0 1
1 0 0
0 1 0

(
(
(
=
0 0 1
1 0 0
0 1 0
0
J (same result)

94

Prismatic joints:
( )
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
Z d P v R v
R
&
= = = =
0
0
0
3
3
2
2
1
1
0
0

=
0
0
0
0
0
v
( )
= + =
a
Z d v R v
&
&
0
0
1
1
1 0
0 1
0 1
1

( )
(
(
(
= + =
b
a
b a
Z d v R v
&
&
&
&
&
0
0
0
0
0
0 1 0
1 0 0
0 0 1
2
2
2 1
1 2
1 2
2

( )
(
(
(
= + =
c
a
b
c b
a Z d v R v
&
&
&
&
&
&
&
0
0 0
0 0 1
0 1 0
1 0 0
3
3
3 2
2 3
2 3
3

(
(
(
=
1 0 0
0 0 1
0 1 0
3
J
c
b
a
J
z
y
x
&
&
&
&
&
&
3
3

95

Jacobian matrix expressed in another frame:
Still relating end-effector frame velocity w.r.t. base frame, just different
coordinates for velocity vector basis. Simpler expressions possible.
)
`
(
(
=
)
`

v
R
R
v
k
k
k 0
0
0
0
0

=
)
`
&
J
v
k
k
=
)
`
&
J
v
0
0
(
(
=
& &
J
R
R
J
k
k
k 0
0
0
0
0

&
not dependent on frames (relative joint rates)
J
R
R
J
k
k
k 0
0
0
0
0
(
(
=


Jacobian was calculated in frame {0} (methods 1 and 2).

(
(
(
+

=
1 1 1
0
0
12 2 12 2 1 1
12 2 12 2 1 1
0
c L c L c L
s L s L s L
J

(
(
(
+

=
1 1 1
0
0
2 2 2 2 1
2 2 2 2
1
c L c L L
s L s L
J
(
(
(
=
1 0 0
0
0
1 1
1 1
1
0
c s
s c
R

(
(
(
+ =
1 1 1
0
0 0
2 2 2 1
2 1
2
L L c L
s L
J
(
(
(
=
1 0 0
0
0
12 12
12 12
2
0
c s
s c
R

(
(
(
+
+
=
1 1 1
0
0
3 2 3 2 23 1
3 2 3 2 23 1
3
c L c L c L
s L s L s L
J
(
(
(
=
1 0 0
0
0
123 123
123 123
3
0
c s
s c
R
J
3
agrees with that derived in the 3
rd
method above.

96

2) Spatial 3P Cartesian manipulator

Jacobian was calculated in frame {0} (methods 1 and 2).

The complexity amongst all frames is equivalent, so transformations are not too
useful, but to complete the examples, here they are:

(
(
(
=
0 0 1
1 0 0
0 1 0
0
J

(
(
(
=
0 0 1
0 1 0
1 0 0
1
J
(
(
(
=
1 0 0
0 0 1
0 1 0
1
0
R

(
(
(
=
0 1 0
0 0 1
1 0 0
2
J
(
(
(
=
0 0 1
1 0 0
0 1 0
2
0
R

(
(
(
=
1 0 0
0 0 1
0 1 0
3
J
(
(
(
=
0 1 0
1 0 0
0 0 1
3
0
R

J
3
agrees with that derived in the 3
rd
method above.

97

Jacobian Matrix Applications

Forward Velocity Transformation:

Inverse Velocity Solution: Used in resolved motion rate control scheme (next page)

for n m = , square matrix, assuming full rank

for n m < , underconstrained, kinematically-redundant robots

Moore-Penrose pseudoinverse
*
J
( )
1
*

=
T T
JJ J J ; order n x m, again assuming full rank;
if not, use singular value decomposition (SVD).

Solution: numerical Gaussian elimination b Ax = ( X J
& &
= )
symbolical
1
J

Partitioned Inverse Velocity Solution:

For a manipulator with a spherical wrist (all three wrist joint coordinate frames
share the same origin), the velocity problem partitions as follows:

)
`
=
)
`
W
A
LR LL
UL
J J
J v
&
&
0

W LR A LL
A UL
J J
J v

& &
&
+ =
=

( )
A LL LR W
UL A
J J
v J

& &
&
=
=
1
1

For the m=n case, the partitioned solution yields identical results to the full solution.
Two 3x3 problems require significantly less computation than one 6x6 problem. Easier
to find symbolic solutions also.

98

Resolved-Rate Control Algorithm

Basic equation:

Block Diagram:

Algorithm Flowchart:

99

Singularities:

Singularities occur when the Jacobian matrix loses full rank.

At singular configurations, freedom to move in one or more Cartesian directions
(translation and/or rotation) has been lost.

Singular configurations also present a singular solution in the inverse pose domain.

Singularities are classified into 2 types: 1) Workspace boundary singularities,
such as when the elbow is straight out (or folded on itself). 2) Workspace interior
singularities, such as when two axes align.

There is a velocity / force duality for singularities. At a singularity, a freedom to
move has been lost in a certain direction, but in the same direction, a force (or moment)
may be resisted with zero actuator torque.

Another way to put it: At a singularity, infinite joint rates are theoretically
required to produce finite Cartesian motion of the end-effector. Also, zero joint torque is
required to resist a force or moment (the manipulator structure resists it, not the actuator).

Singular configurations arise when:

0 = J for square Jacobian matrices; and

0 =
T
JJ for underconstrained kinematically redundant manipulators

As the determinant approaches zero, the Jacobian matrix becomes ill-conditioned.

One way to calculate the inverse of a matrix:

( )
J
J Adj
J =
1

So we see
1
J does not exist at 0 = J , and is unreliable near 0 = J .

100

Jacobians and Static Forces

Derive the relationship between static forces / moments applied by the end-effector
and the required joint torques.

Principle of virtual work: If a body is in equilibrium, the total virtual work of the
forces acting on the body is zero for any virtual displacement of the body.

Work: Virtual Work:

In particular, we can equate virtual work expressions (since they are zero) in the
joint space with the Cartesian space:

where: 1 n vector of joint torques
1 n vector of infinitesimal joint displacements
1 m F vector of Cartesian wrench (forces and moments)
1 m X vector of infinitesimal Cartesian displacements

n = dimension of joint space
m = dimension of Cartesian space

Express dot product as:

Using definition of Jacobian matrix:

We get:

Which must hold for all and so:

101

Finally, transposing both sides yields:

Jacobian matrix and Cartesian wrench must be expressed w.r.t. some frame {k}.
Just like the joint angles, has no dependence on frame, but these are relative joint
torques.

This is a mapping from Cartesian space to joint space which does not require an inverse!
That is rare and beautiful!!

Joint Torque direction convention

= J F
T
calculates the joint torque directions to apply F, not resist F. That is,
= J F
T
causes the end-effector to push on the environment with F. This can be seen
with a simple one-link manipulator example.
)
`
sin
cos
L
L
P ;
)
`
=

cos
sin
&
&
L
L
V ; so
(
(
(
=
1
cos
sin
0
L
L
J

[ ]
(
(
(
=
z
y
x
m
f
f
L L 1 cos sin

Assume
o
30 , 1 = = L .
[ ] 5 . 0
0
0
1
1 866 . 0 5 . 0 =
(
(
(
= [ ] 866 . 0
0
1
0
1 866 . 0 5 . 0 =
(
(
(
=
[ ] 1
1
0
0
1 866 . 0 5 . 0 =
(
(
(
=

Interesting side note: canonical (simplest) Jacobian matrix:
(
(
(
=
(
(
(
(
(
(
= =
1
0
1 1 0 0
0
0
0 1
0
1
L Lc
Ls
c s
s c
J R J

102

Velocity Kinematics Examples


Given 1 , 2 , 3
3 2 1
= = = L L L ,
o o o
35 , 25 , 15
3 2 1
= = = , and 3 , 2 , 1
3 2 1
= = =
& & &
:

1) Forward Velocity Kinematics

(
(
(

= =
6
494 . 7
634 . 4
3
2
1
1 1 1
0 532 . 1 430 . 4
0 286 . 1 062 . 2
0 0
& &
J X

( ) ( ) ( )
)
`
=
)
`
+
)
`
= + =
047 . 9
430 . 10
553 . 1
796 . 5
494 . 7
634 . 4
3
0
3 3
0
0
3
0
0
0
0
X L V V
H

( ) ( ) 6
3
0
0
0
0
= =
H

2) Inverse Velocity Kinematics

a) k = 0
(
(
(
(
(
(
(
1
0
0
2 2
1
2 2
1
2 2 1
12 2 1 1
2 2 1
12 2 1 1
2 1
12
2 1
12
1 0
s L
s
s L
c
s L L
s L s L
s L L
c L c L
s L
s
s L
c
J

( )
(
(
(
= =

3
2
1
6
494 . 7
634 . 4
1 306 . 0 143 . 1
0 813 . 0 747 . 1
0 507 . 0 604 . 0
0 1 0
X J
& &

103

b) k = 2
(
(
(
= =
6
719 . 8
267 . 1
6
494 . 7
634 . 4
1 0 0
0 766 . 0 643 . 0
0 643 . 0 766 . 0
0 2
0
2
X R X
& &

(
(
(
(
(
(
(
+
=
1
1
0
1
0 0
1
2 2 2
2
2 2 2 1
2 2 1
2 1
1 2
L s L
c
L s L L
L c L
s L
J

( )
(
(
(
= =

3
2
1
6
719 . 8
267 . 1
1 5 . 0 072 . 1
0 5 . 0 861 . 1
0 0 789 . 0
2 1 2
X J
& &
(same result!)

3) Singularity Analysis

Determinant invariant under coordinate transformations:

2 2 1
3 2 1 0
s L L J J J J = = = =

Singularity conditions:

0
0
2
1
=
=
L
L
(not possible)

0
2
= s ; , 0
2
=

Example: ( ) 54 . 2 25 sin 2 3
0
= =
o
J

More meaningful to look at plot of determinant:

Two interesting cases:

104

min - 0 = J ; 0
2
=

max - 6 = J ;
o
90
2
=

4) Static Force Analysis

Given 1 , 2 , 3
3 2 1
= = = L L L ,
o o o
35 , 25 , 15
3 2 1
= = = , and F
0
, calculate .

F J
T 0 0
=

z
y
x
T
m
f
f
J
0
0
3
2
1

a) Force
(
(
(
=
0
246 . 0
368 . 2
0
1
1
1 0 0
1 532 . 1 286 . 1
1 430 . 4 062 . 2

b) Moment
(
(
(
=
1
1
1
1
0
0
1 0 0
1 532 . 1 286 . 1
1 430 . 4 062 . 2

Why are all three joint torques 1 Nm? All joint torques must resist against the previous
neighbor link. Free-body diagram:

c) Force and moment
(
(
(
=
1
246 . 1
368 . 3
1
1
1
1 0 0
1 532 . 1 286 . 1
1 430 . 4 062 . 2

105

Velocity Kinematics Examples (cont.)

2) Spatial 3P Cartesian manipulator

1) Forward Velocity Kinematics

(
(
(
= =
a
c
b
c
b
a
L J X
&
&
&
&
&
&
& &
0 0 1
1 0 0
0 1 0
0 0

2) Inverse Velocity Kinematics

( )
(
(
(
= =

c
b
a
a
c
b
X J L
&
&
&
&
&
&
& &
0 1 0
0 0 1
1 0 0
0 1 0

3) Singularity Analysis

1
3 2 1 0
= = = = J J J J no possible singularities!

4) Static Force Analysis

F J
T 0 0
=

(
(
(
y
x
z
z
y
x
c
b
a
F
F
F
F
F
F
f
f
f
0
0
0 1 0
0 0 1
1 0 0

106

Cartesian Transformation of Velocities and Static Wrenches
Move velocity and wrench vectors (both translational and rotational) from one
point to another on a rotating rigid body. Replace one vector with an equivalent vector
acting at a different point. e.g., calculate velocity (wrench) at wrist to produce a desired
velocity (wrench) at hand.

1) Velocity transformation
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
B
A
A A B
P v v

=
+ =
more properly, ( )
B
k
V
0

B A
B B
A
B B
A
B B A
P v P v v

=
+ = =

All vectors must be expressed in same frame: choose {A}.

( )
( )
( )
( )
)
(
(
B
B
B
A
B
A
B B
A A
B
A
A
A
V
R
R P R V

0
0
0
0
0

X T X
B
V
A
B
A
& &
=

(
(

=
R
R P R
T
A
B
A
B B
A A
B
V
A
B
0

where:
(
(
(
=
0
0
0
x y
x z
y z
B
A
p p
p p
p p
P

107

2) Wrench transformation
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 and {A} would
then be the last wrist frame {N} for which the Jacobian matrix is derived.

Basic equations:
B B
A
B A
B A
F P M M
F F
+ =
=
more properly,
B
k
F

All vectors must be expressed in same frame: choose {A}.

)
`
(
(
=
)
`
B
B
B
A
B
A
B B
A
A
B
A
A
A
M
F
R R P
R
M
F
0

W T W
B
W
A
B
A
=

(
(
=
R R P
R
T
A
B
A
B B
A
A
B
W
A
B
0

Note:
W
A
B
T is a block-transpose of
V
A
B
T ; i.e. R P
A
B B
A
is switched with 0.
The book says
T
W
A
B W
A
B
T T = , but this is not true - only block-transpose, not matrix
transpose.

Duality: in velocity, rotational term unchanged, while in wrench, translational
term unchanged.

108

Example: Cartesian Transformation of Velocities and Static Forces

1) Velocity transformation
Find the equivalent velocity at A corresponding to a given motion of B.
Given: ( )
=
0
1
1
0
B
B
V ; ( )
=
2
0
0
0
B
B

=
0
5 . 1
6 . 2
B
A
P ;
(
(
(

=
1 0 0
0 866 . 0 5 . 0
0 5 . 0 866 . 0
R
A
B

(
(
(
=
(
(
(

(
(
(
=
0 3 0
6 . 2 0 0
5 . 1 0 0
1 0 0
0 866 . 0 5 . 0
0 5 . 0 866 . 0
0 6 . 2 5 . 1
6 . 2 0 0
5 . 1 0 0
R P
A
B B
A

X T X
B
V
A
B
A
& &
=

( )
( )
(
(
(
(
(
(
(
2
0
0
0
834 . 3
366 . 3
2
0
0
0
1
1
1 0 0 0 0 0
0 866 . 0 5 . 0 0 0 0
0 5 . 0 866 . 0 0 0 0
0 3 0 1 0 0
6 . 2 0 0 0 866 . 0 5 . 0
5 . 1 0 0 0 5 . 0 866 . 0
0
0
A
A
A
V

Check:
B A
= r V V
B A
=

( )
=
0
5
1
0
6
0
0
1
1
0 0 3
2 0 0

0
1
1
0
k j i
V
A
B

109

2) Wrench transformation
Find the equivalent wrench at A corresponding to a given wrench at B.
Given:
=
0
1
1
B
B
F ;
=
0
0
0
B
B
M

R P R P
A
B B
A A
B B
A
, , same as 1) above.

W T W
B
W
A
B
A
=

(
(
(
(
(
(
(
=
)
`
3
0
0
0
366 . 1
366 . 0
0
0
0
0
1
1
1 0 0 0 3 0
0 866 . 0 5 . 0 6 . 2 0 0
0 5 . 0 866 . 0 5 . 1 0 0
0 0 0 1 0 0
0 0 0 0 866 . 0 5 . 0
0 0 0 0 5 . 0 866 . 0
A
A
A
M
F

Check:
B A
F F =
B A
F r M =

110

Acceleration Kinematics
Acceleration vector is first time derivative of velocity vector (second time
derivative of position vector). Linear in acceleration terms, non-linear in rate
components. Both Cartesian acceleration and velocity are vectors: translational and
rotational.
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
( )
j
i
k
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: given by Craig in Eq. (6.6):
( ) Q R V R Q R
dt
d
B A
B B
A
Q
B A
B
B A
B
+ =

General five-part acceleration equation:
Position:
P T P
P R P P
B A
B
A
B A
B B
A A
=
+ =

Velocity: P R P R P V
B A
B
B A
B B
A A
& & &
+ + =
P V V V
P
+ + =
0

Acceleration:
( )
P
B A
B B
A
P
B A
B B
A A
P R V R V
dt
d
A + + =
( )
P
B A
B B
A
B
A
P
B A
B B
A
P
B A
B B
A
P
B A
B B
A
P
B A
B B
A A
P R V R P R V R A R A A + + + + + =

( )
P
B A
B B
A
B
A
P
B A
B B
A
P
B A
B B
A
P
B A
B B
A A
P R P R V R A R A A + + + + = 2

( ) P P V A A A
P
+ + + + = 2
0

111

Rotational Acceleration
( )
j
i
k
is the rotational acceleration of frame {j} with respect to reference frame
{i}, expressed in the basis (coordinates) of {k}.

Velocity:
C
B A
B B
A
C
A
R + =

Acceleration:
( ) ( )
C
B A
B B
A
C
B A
B B
A
C
A
R
dt
d
R
dt
d
+ = + =

C
B A
B B
A
C
B A
B B
A
C
A
R R + + =

(we can modify this for angular velocity vectors expressed in the local frame also)

Combined Translational and Rotational Acceleration

{ }
)
`
a
X
& &

where:

{ } ( )
{ } ( )
N
N
A a

0
0
0
0
=
=
both are (3x1) vectors

112

Acceleration Example

Planar 2-link manipulator - acceleration of end-effector frame w.r.t. {0}, also expressed
in {0}.

1) Craig acceleration recursion

=
0
0
1
2
1
L
P
=
0
0
2
3
2
L
P
(
(
(
=
1 0 0
0
0
2 2
2 2
2
1
c s
s c
R I R =
3
2

=
0
0
0
0
0

=
0
0
0
0
0

=
0
0
0
0
0
a

=
1
1
1
0
0
&

=
1
1
1
0
0
& &

=
0
0
0
1
1
a

+
=
2 1
2
2
0
0

& &

+
=
2 1
2
2
0
0

& & & &

( ) ( )
1
1
2
1
1
1
1
1
2
1
1
1 2
1 2
2
a P P R a + + =

+
+
=
(
(
(
=
0 0 1 0 0
0
0
1 2 1
2
1 2 1
1 2 1
2
1 2 1
1 1
2
1 1
2 2
2 2
2
2

& & &

& & &
& &
&
c L s L
s L c L
L
L
c s
s c
a

+
= =
2 1
2
2
3
3
0
0

& & & &

( ) ( )
2
2
3
2
2
2
2
2
3
2
2
2 3
2 3
3
a P P R a + + =

113

( )
( )
+ + +
+ +
=
0
1 2 1
2
1 2 1 2 1 2
1 2 1
2
1 2 1
2
2 1 2
3
3

& & & & & & &
& & & & &
c L s L L
s L c L L
a

3
3 0
3 3
0
a R a =

( ) ( ) ( )
( ) ( ) ( )
|
.
|
\
|
+ + +
|
.
|
\
|
+ + + +
=
0
2
2 1 12 2 1 12 2
2
1 1 1 1 1
2
2 1 12 2 1 12 2
2
1 1 1 1 1
3
0

& & & & & & & & &
& & & & & & & & &
s c L s c L
c s L c s L
a

Rewrite for comparison to next section:

( )
( ) ( )
)
`
+
(
+
+
+
)
`
+

=
2 1
1
2 1 12 2 1 1 1
2 1 12 2 1 1 1
2
1
12 2 12 2 1 1
12 2 12 2 1 1
3
0

& &
&
& & &
& & &
& &
& &
s L s L
c L c L
c L c L c L
s L s L s L
a

The second term above can be written as:

( ) ( )
( ) ( )
)
`
+ +
+ +
2
1
2 1 12 2 2 1 12 2 1 1 1
2 1 12 2 2 1 12 2 1 1 1

&
&
& & & & &
& & & & &
s L s L s L
c L c L c L

114

Acceleration Example (cont.)
Planar 2-link manipulator - acceleration of end-effector frame w.r.t. {0}, also expressed
in {0}. Alternative method:

2) Differentiation of rate equation
& &
J X =
& & & & & &
J J X + =

Do in frame {0} - if in frame {2} or {3}, must account for r somehow.

(
(
(
+

=
1 1
12 2 12 2 1 1
12 2 12 2 1 1
0
c L c L c L
s L s L s L
J

(
= =
dt
dJ
dt
dJ
J
ij
&

dt
d
J
dt
d
J
dt
d
J
dt
dJ
N
N
ij ij ij ij

+ + + = L
2
2
1
1

=
=
N
k
k
k
ij ij
J
dt
dJ
1
&

( )
( )
(
(
(
+
+
=
0 0
2 12 2 1 12 2 2 12 2 1 12 2 1 1
2 12 2 1 12 2 2 12 2 1 12 2 1 1
0

& & & &
& & & &
&
s L s L s L s L s L
c L c L c L c L c L
J

( ) ( )
( ) ( )
(
(
(
+ +
+ +
=
0 0
2 1 12 2 2 1 12 2 1 1 1
2 1 12 2 2 1 12 2 1 1 1
0

& & & & &
& & & & &
&
s L s L s L
c L c L c L
J

& & & & & &
J J X
0 0 0
+ =

( ) ( )
( ) ( )
)
`
(
(
(
+ +
+ +
+
)
`
(
(
(
+

=
2
1
2 1 12 2 2 1 12 2 1 1 1
2 1 12 2 2 1 12 2 1 1 1
2
1
12 2 12 2 1 1
12 2 12 2 1 1
0
0 0 1 1

&
&
& & & & &
& & & & &
& &
& &
& &
s L s L s L
c L c L c L
c L c L c L
s L s L s L
X

Same result!

115

Uses for general acceleration equation

( )

& & & & & &

& & & & & &
& &
J X J
J J X
J X
=
+ =
=
1

i. Forward Acceleration Kinematics Analysis
& & & & & &
J J X + = predicts the
Cartesian accelerations X
& &
given the joint rates and accelerations.

ii. Resolved Acceleration Control like resolved rate, but command
acceleration instead. Solve ( )
& & & & & &
J X J =
1
and double integrate to get
commanded joint angles.

iii. Dynamics equations
& &
is required for the Newton/Euler dynamics
recursion in Chapter 6. If calculated via numerical differentiation, numerical
instability can result, so the analytical approach ( )
& & & & & &
J X J =
1
is much
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:

& & & &
J J
1
=

How to find the time rate of change of the Jacobian matrix J
&
? See previous page for a
specific example.

116

Manipulator Dynamics

Kinematics - The study of motion without regard to forces.

Dynamics - The study of motion with regard to forces. The study of the relationship
between force (torque) and motion. Composed of kinematics and kinetics.

a) Forward Dynamics (simulation) - given the actuator forces and torques,
compute the resulting motion (requires solution of highly coupled, nonlinear ODEs):
Given , calculate
& & &
, , (all are N x 1 vectors).

b) Inverse Dynamics (control) - given the desired motion, calculate the actuator
forces and torques (solution more straight-forward than Forward Dynamics): Given

& & &
, , , calculate .

Both problems require the N dynamic equations of motion, one for each link. Highly
coupled and non-linear. Methods for determining the dynamic equations of motion:
-Newton-Euler recursion (force balance, including inertial (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,
i i Ci
a , , , must be with respect to an inertially-fixed
frame. (The frame of expression, {k}, needn't be inertial!)

Assumptions:
Serial robot, rigid links, ignore actuator dynamics, no friction, no joint or link
flexibility.

117

Mass Distribution: Inertia Tensor

Spatial generalization of planar scalar moment of inertia; units:
2
md (
2
kgm ).
Inertia tensor relative to frame {A}:

Mass moments of inertia:
( ) dv z y I
V
xx

+ =
2 2
( ) dv z x I
V
yy

+ =
2 2
( ) dv y x I
V
zz

+ =
2 2

Mass products of inertia:
dv xy I
V
xy

= dv xz I
V
xz

= dv yz I
V
yz

=

Principal moments of inertia:
A certain orientation of the reference frame {A}, the principal axes, yields zero
products of inertia. The eigenvalues of a general I
A
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 having index the 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:
Change tensor description by translation of reference frame from center of mass
{C} to any frame {A} (same orientation): { }
T
C C C C
z y x P = is the vector giving the
location of the center of mass from the origin of {A}.
e.g.:
( )
C C xy
C
xy
A
C C zz
C
zz
A
y mx I I
y x m I I
=
+ + =
2 2

Vector-matrix form:
[ ]
T
C C C
T
C
C A
P P I P P m I I + =
3

118

Newton-Euler Recursive Algorithm

Free-body diagram of link i:

i
f : Internal force exerted on link i by link i-1.

i
n : Internal moment exerted on link i by link i-1.

Inertial loads: Newton and Euler equations:

Force balance:

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

119

Newton-Euler Recursive Algorithm
Used to find the robot dynamic equations of motion. Can also be used to directly
solve the inverse dynamics problem. Assuming an all revolute-joint manipulator (for
prismatic joint dynamics see Craig text).

Outward iteration for kinematics: 1 0 : N i
(without regard for frames of expression - see Eqs. 6.45 - 6.50, p. 200, for frames)

Velocities and accelerations:
1 1 1
+ + +
+ =
i i i i
Z
&

1 1 1 1 1

+ + + + +
+ + =
i i i i i i i
Z Z
& & &

( )
1 1 1 + + +
+ + =
i
i
i i i
i
i i i
P P a a

( )
1
1
1 1 1
1
1 1 1 +
+
+ + +
+
+ + +
+ + =
Ci
i
i i Ci
i
i i Ci
P P a a

Inertial loading:
1 1 1 + + +
=
Ci i i
a m F

1 1 1 1 + + + +
+ =
i
C
i i
C
i
I I N

Inward iteration for kinetics: 1 : N i
(without regard for frames of expression - see Eqs. 6.51 - 6.53, p. 200, for frames)

Internal forces and moments:
i i i
F f f + =
+1

i i i
i
i Ci
i
i i
N f P F P n n + + + =
+ + + 1 1 1

Externally applied joint torques:
i i i
Z n =

Inclusion of gravity forces:
G a =
0
0

This is equivalent to a fictitious upward acceleration of 1G of the robot base; the
effect is to account for the downward acceleration due to gravity (weight of all links).

120

Lagrange-Euler Energy Method

Alternative method to find the robot dynamic equations of motion. Requires only
link velocities. The Lagrangian is formed from the Kinetic energy k and Potential
energy u of the robot system.

( ) ( ) u k u k L = =
&
,

i i
Ci T
i Ci
T
Ci i i
I V V m k
2
1
2
1
+ =

Ci i REF i
P g m u u
0 0
=

( )

=
=
N
i
i
k k
1
,
&
( )

=
=
N
i
i
u u
1

Note:

( ) ( ) =
& & &
M k
T
2
1
, ; where ( ) M is the manipulator mass matrix.

Dynamic equations of motion:
Found for each active DOF from the following expression involving the
Lagrangian, joint variable, and actuator torque. Perform N times, once for each joint
variable, to yield N independent dynamic equations of motion.

=
|
.
|
\
|
L L
dt
d
&

This expression may be rewritten using ( ) ( ) u k L =
&
, :

= +
|
.
|
\
|
u k k
dt
d
&

121

Simple Dynamics Example

Derive the dynamic equation of motion for a planar 1-link 1R mechanism by 3 methods:
1) Sophomore methods FBD (free-body diagram), force balance.
2) Newton-Euler recursion.
3) Lagrange-Euler formulation.

Y
L
X
,

1) Sophomore methods - FBD, force balance Free-Body Diagram:
mg
-F
X
-F
Y
F
Y
F
X

122

1) Sophomore methods - FBD, force balance (cont.)

a)
|
.
|
\
|
= = =
c
L
s
L
m F ma F
x Cx x
2 0
2 2

( ) c s
mL
F
x
2 0
2
& & &
+ =

b)
|
.
|
\
|
= = =
s
L
c
L
m mg F ma F
y Cy y
2 0
2 2

( ) s c
L
m mg F
y
2 0
2
& & &
+ =

c) gc
mL
I M
z
2
0
0
= =
& &

gc
mL
I
2
0
+ =
& &

where:
4
2
2 0
mL
I md I I
zz
C C
+ = + =

123

Simple Dynamics Example (cont.)

2) Newton-Euler recursion

Outward iteration: i=0 (thats the only one!)

=
0
0
0
1
0
P
=
0
0
2
1
1
L
P
C

(
(
(
=
1 0 0
0
0
1
0

c s
s c
R
(
(
(
=
zz
yy
xx
C
I
I
I
I
0 0
0 0
0 0
1
1

=
0
0
0
0
0

=
0
0
0
0
0

=
0
0
0
0
g a to account for gravity

( ) ( )
0
0
1
0
0
0
0
0
1
0
0
0 1
0 1
1
a P P R a + + =

(
(
(
=
0 0
0
1 0 0
0
0
1
1

gc
gs
g c s
s c
a
( )
= + + =
0
2
2
2
1
1
1
1
1
1
1
1
1
1
1
1
1
1

& &
&
L
gc
L
gs
a P P a
C C C

= =
0
2
2
2
1
1
1
1

& &
&
L
gc
L
gs
m a m F
C

= + =

& &
zz
C C
I
I I N 0
0
1
1
1 1
1
1
1
1 1
1
1 1

124

Inward iteration: i=1 (thats the only one!)

=
0
0
0
2
2
f
=
0
0
0
2
2
n

= + =
0
2
2
2
1
1
2
2 1
2 1
1

& &
&
L
gc
L
gs
m F f R f

1
1
2
2 1
2 2
1
1
1 1
2
2 1
2 1
1
1
N f R P F P n R n
C
+ + + =

|
.
|
\
|
+
=

& &
& &
zz
I
L
gc
mL
n 0
0
2 2
0
0
1
1

1
1
1
1
Z n =

gc
mL mL
I
zz
2 4
2
+
|
|
.
|
\
|
+ =
& &

Check with method 1) above:
+ +

=
(
(
(

= =
0
2 2
2 2
0
2
2
1 0 0
0
0
2
2 2
1
1 0
1 1
0
g c
L
s
L
s
L
c
L
m
L
gc
L
gs
m c s
s c
f R f

& & &
& & &
& &
&

125

Simple Dynamics Example (cont.)

3) Lagrange-Euler formulation

&
0
0
1
1

(
(
(
=
zz
yy
xx
C
I
I
I
I
0 0
0 0
0 0
1
1

Kinetic and potential energy are scalars - write k in {1} and u in {0}!!

=
0
2
0
1
1
&
L
v
C

=

& &
zz
C
I
I 0
0
0
0
1
1
1 1
1
1

2 2
2
2
1
4 2
1

& &
zz
I
mL
k + =

s
L
mg s
L
c
L
g m u
2 2
2
0
0
0 =
=

s
L
mg
mL
I u k L
zz
2 4 2
1
2
2
|
|
.
|
\
|
+ = =
&

=
|
.
|
\
|
L L
dt
d
&

|
|
.
|
\
|
+ =
4
2
mL
I
L
zz
&
&

|
|
.
|
\
|
+ =
|
.
|
\
|
4
2
mL
I
L
dt
d
zz
& &
&

c
L
mg
L
2
=

gc
mL mL
I
zz
2 4
2
+
|
|
.
|
\
|
+ =
& &

126

Structure of Manipulator Dynamics Equations

State Space Equation
( ) ( ) ( ) + + = G V M
& & &
,

( ) M : N x N mass matrix; symmetric and positive definite
( )
&
, V : N x 1 vector of Coriolis and centrifugal terms
( ) G : N x 1 vector of gravity terms

Configuration Space Equation
( ) ( )[ ] ( )[ ] ( ) + + + = G C B M
2
& & & & &

( ) M : N N mass matrix; symmetric and positive definite
( ) B :
( )
N
N N
1
2
Coriolis matrix
( ) C : N N Centrifugal matrix
( ) G : N x 1 vector of gravity terms
[ ]
& &
:
( ) N N
1
2
1: { }
T
N N

& &
L
& & & &
1 3 1 2 1

[ ]
2
&
: N 1: { }
T
N
2 2
2
2
1

&
L
& &

Cartesian State Space Equation
( ) ( ) ( ) + + =
x x x
G V X M F
& & &
,

( )
x
M : N x N Cartesian mass matrix; symmetric and positive definite
( )
&
,
x
V : N x 1 vector of Cartesian Coriolis and centrifugal terms
( )
x
G : N x 1 vector of gravity terms in Cartesian space

where: Note:

( ) ( )
( ) ( ) ( ) ( )
( ) ( ) =
=
=

G J G
J J M V J V
J M J M
T
x
T
x
T
x
& & & &

1
1
, ,
+ =
=
& & & & & &
& &
J J X
J X

127

Dynamics Example

Determine the dynamic equations of motion for the two-link planar 2R
manipulator 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:
X
Y
0
0
L
2
L
1
1
2

g
c L m s L L m L m
I
L m c L L m
I
zz zz
|
.
|
\
|
+ |
.
|
\
|
+
|
|
.
|
\
|
+ +
|
|
.
|
\
|
+ + =
2 2 4 4 2
12 2 2 2 2 2 1 2
2
2
2 2
2 1
2
2 2 2 2 1 2
2 2
1

& & & & &

( ) g
c L m
c L m
c L m
s L L m
s L L m
L m c L L m
I
L m
c L L m L m
L m
I I
zz zz zz
|
.
|
\
|
+ + + +
|
.
|
\
|
+
|
|
.
|
\
|
+ + +
|
|
.
|
\
|
+ + + + + =
2 2 2
4 2 4 4
12 2 2
1 1 2
1 1 1
2 1 2 2 1 2
2
2
2 2 1 2
2
2
2 2 2 2 1 2
2 1
2
2 2
2 2 1 2
2
1 2
2
1 1
2 1 1

& & &
& & & &

where ( )
2 2
12
i i
i
zzi
h L
m
I + = .

128

State Space Representation

( ) ( ) ( ) + + = G V M
& & &
,
)
`
=
2
1

)
`
=
2
1
& &
& &
& &

( )
(
(
(
(
+ + +
+ + + + + + +
=
4 4 2
4 2 4 4
2
2 2
2
2
2 2 2 2 1 2
2
2
2 2 2 2 1 2
2
2
2 2
2 2 1 2
2
1 2
2
1 1
2 1
L m
I
L m c L L m
I
L m c L L m
I
L m
c L L m L m
L m
I I
M
zz zz
zz zz zz

( )
( )
|
.
|
\
|
+
|
.
|
\
|
=
2
1
2 2 1 2
2 1 2 2 1 2
2
2
2 2 1 2
2
2
,

&
& & &
&
s L L m
s L L m
s L L m
V

( ) g
c L m
c L m
c L m
c L m
G
+ +
=
2
2 2
12 2 2
12 2 2
1 1 2
1 1 1

Configuration Space Representation

( ) ( )[ ] ( )[ ] ( ) + + + = G C B M
2
& & & & &
( ) ( ) G M , same

( )[ ]
2 1
2 2 1 2
0

& & & &
)
`
=
s L L m
B

( )[ ]
(
(
(

=
2
2
2
1
2 2 1 2
2 2 1 2
2
0
2
2
0
&
&
&
s L L m
s L L m
C

129

Numerical Dynamics Example
For the planar 2R robot whose dynamic equations of motion were derived
analytically above, 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 should be 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 7806 =
3
m
kg
; 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).

The initial robot configuration is:
=
)
`
=
o
o
90
10
2
1

The (constant) commanded Cartesian velocity is:
)
`
=
)
`
=
5 . 0
0
0 0
0
y
x
X
&
&
&
(m/s)

The dynamics equations require the relative joint accelerations
& &
; how do we find these? (See
the last page of the Acceleration Kinematics notes: ( )
& & & & & &
J X J =
1
. 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 three pages.
In the last plot, note that the robot travels 0.5 m in the Y
0
direction in 1 sec (agrees with constant
commanded rate of 0.5 m/s). The robot does not move in X; must move to compensate for the pure Y
motion, but we cannot control it independently with only 2 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 inverse dynamics algorithm; these are the torques necessary to move
this robots inertia in the commanded manner. Notice from the joint angles, joint rates, joint
accelerations, and joint torques plots that the robot is approaching the
2
= 0 singularity towards the end
of this motion.

130

Numerical Dynamics Example: Plots

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
)
,

(
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)
1

(
r
)
,

2

(
g
)

(
d
e
g
)

131

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
)

132

0 0.2 0.4 0.6 0.8 1
0
10
20
30
40
50
60
70
80
Joint Torques
time (sec)

(
1
-
r
e
d
;

2
-
g
r
e
e
n
)

(
N
m
)

No gravity (above) and with gravity in Y direction (below).

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

(
1
-
r
e
d
;

2
-
g
r
e
e
n
)

(
N
m
)

133

Single Joint Control
Manipulator dynamics is extremely complex in terms of number of terms. Last
week we saw how bad it was for a planar 2R 2-dof robot can you imagine what the six
dynamic equations of motion look like for the spatial 6R PUMA robot?? We can easily
use symbolic Matlab to crank out the terms but they go on for pages and their structure is
very difficult to understand. In this case, numerical Newton-Euler recursion ala Craig is
very 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 develop this electromechanical system model and implement it in
the Matlab Simulink environment to test the model. We will focus on a common system,
the armature-controlled DC servomotor driving a single robot joint / gear train / link
combination. While we ignored actuator dynamics in the dynamics section we now
include it.
Open-Loop System Diagram:

V
r
(t) reference voltage
M
(t) generated motor torque
L
(t) load torque
V
A
(t) armature voltage
M
(t) motor shaft angle
L
(t) load shaft angle
L armature inductance
M
(t) motor shaft velocity
L
(t) load shaft velocity
R armature resistance J
M
lumped motor inertia J
L
(t) total load inertia
i
A
(t) armature current C
M
motor viscous damping C
L
load viscous damping
v
B
(t) back emf voltage n gear ratio

134

Closed-Loop Feedback Control Diagram: High Level

High-level Computer Control Diagram

Simplifying Assumptions
rigid motor, load shafts
n >> 1 (reduce speed, increase torque)

Real World Our Model (Simplified)
Non-linear Linearized
MIMO SISO
Coupled Decoupled
Time-varying load inertia Disturbance; gear ratio diminishes
Hysteresis, backlash, stiction, Coulomb friction Ignore
Discrete and continuous Continuous for control design

135

Single Joint/Link System Modeling

Derive all linear ordinary differential equations (ODEs) describing dynamics of armature-
controlled DC servomotor driving a single robot joint / link. Electrical, mechanical
(including gear train). Modeling, simulation, control. System diagram: See earlier.

Armature Circuit Diagram:

Electrical Model Kirchoffs voltage law

(1)

Electromechanical coupling:
Generated motor torque proportional to armature current

(2)
Back emf voltage proportional to motor shaft angular velocity

(3)

(K
T
= K
B
can be shown)

136

Mechanical Model
Eulers Eq. (Rotational form of Newtons 2
nd
law):

Free-body diagrams:

Load
(4)

Motor
(5)

where ( ) t
LR
is the load torque reflected to the motor shaft.

Gear ratio n

(6)

Substituting (6) into (5) yields (7):

(7)

137

Reflect load inertia and damping to motor shaft

Substitute into (4):

(8)

Substitute (8) into (7) to eliminate
L
:

(9)

Combine terms:

(10)

Define:
2
n
J
J J
L
M E
+ = Effective inertia: Total reflected to motor shaft
2
n
C
C C
L
M E
+ = Effective damping: Total reflected to motor shaft

138

Final Mechanical Model (ODE):

(11)

This is a 2
nd
order ODE in
M
. Can also be written as 1
st
order ODE in
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.

For example, NASA AAI 8-axis arm downstairs has n = 200 with harmonic
gearing (this is not a common industrial robot, but an extraordinary space robot; still n is
large for most industrial robots). This is why we can ignore the time-varying robot load
inertia and design decoupled independent linear joint controllers. Load inertia variation
treated as disturbance.

Alternative: Reflect motor inertia and damping to load shaft
(we will use the other case above)

( ) ( )
L L M L L M L
C n C J n J = + + +
& & &
2 2

139

Open-Loop Block Diagram

Use Laplace transforms on different pieces of the model connect together with blocks. Transfer
function goes inside each block; variables are the arrow labels. No need to simplify just use Simulink!

Closed-Loop Feedback Control Diagram
Assume perfect encoder sensor for angular position feedback. Include block for PID controller.

140

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

Trial & Error PID Design

Start with P gain value start low and work up to get desired time nature of response.
Add D gain value term for stability. Add I gain value to eliminate steady-state error.
Always use Simulink to simulate control & dynamic response of the single joint/link
system for different PID choices; compare and select best controller.

Better: analytical design for PID controllers (classical control, ME 401).

Performance Criteria (come to ME 601 for more details)
Stability
Rise Time
Peak Time
Percent Overshoot
Settling Time
Steady-state error

These performance criteria provide a rational basis for choosing best controller, at least
in theory and simulation. The real world always provides some additional challenges
(noise, modeling errors, non-linearities, calibration, backlash, etc.).

141

Include Gravity as a Disturbance Torque

Unmodeled, unexpected disturbances are modeled as disturbances in control
systems. Convenient to do so at the motor torque level in the block diagram:

Gravity is known and expected; however, we can include its effect as a disturbance. First
let us model the effect for a single joint. Lump all outboard links, joints, and motors as a
rigid body:

Test with original PID gains
Redesign new PID gains with gravity disturbance considered

142
142

Kinematically-Redundant Manipulators
Redundancy extra, more than is required

3 types of electromechanical redundancy
Fail-safe: two motors at a joint; if one fails, just use the second (e.g. NASA FTS)
Actuation: more motors than dof; antagonistic loading to increase stiffness
Kinematic: more dof than required for the task; optimization possible

Kinematic Redundancy
Dexterity, autonomy - next generation of robots with increased capability

Wider application - industrial cells set up carefully to avoid obstacles, joint limits,
manipulator singularities, etc. This is costly, time-consuming, and limiting. More
rigorous industrial applications, space, undersea, nuclear power plants, service industry
(robot maid, helper, delivery, hospital, etc.)

Performance Optimization - can optimize performance in addition to satisfying
the required motion trajectories with the extra freedom (partial list):
avoid obstacles
avoid joint limits
avoid singularities
minimize manipulator energy
minimize joint torques
minimize trajectory time
minimize base shaking force (space manipulators)

local - optimization for neighborhood of current configuration
global - minimum (or maximum) over entire motion range

Redundant Robot System Examples (some pictures on following page)
redundant arm - 7dof (human, LTM, FTS, RRC), 8dof (ARMII)
humanoid - Special Purpose Dexterous Manipulator (SPDM) Canada; 2-7dof
arms on 5-dof trunk including waist and neck joints = 19 dof!
compound manipulator - SSRMS with arm (or SPDM!)
arm on track - PUMA on linear track
arm with moving workcell - 5 dof arm (axisymmetric) with 2 dof table
cooperating serial robots forming closed-chain with a workpiece

143
143

SSRMS with SPDM on International Space Station

8-axis AAI ARMII (NASA LaRC) 8-cable CDDHI

RoboNaut (NASA JSC) Human Arm with PMAs

144
144

Kinematically-Redundant Manipulator - has more joint freedoms than required for the
Cartesian task. The dimension of the joint space (n) is greater than the dimension of
Cartesian task space (m). General Velocity relationship:

(
mxn
J )

m < n

degree of redundancy = n m

The inverse velocity problem is Underconstrained, infinite valid solutions exist.
(Actually, n m infinities of solutions exist.)

Self-motion (like human arm) - optimization takes place in joint rate combinations such
that 0 = X
&
.

Primary task: required trajectory
Secondary task: optimization of some performance criteria in null space (self-motion)

145
145

Inverse Velocity (Resolved Rate) Solution

1 Pseudoinverse-based

1.1 Particular Solution - satisfies primary task (satisfies Cartesian trajectory)

Minimize cost function:

Subject to:

1.1.1 Least-norm Joint Rate Solution

W = I

where (Moore-Penrose pseudoinverse):

1.1.2 Minimum Manipulator Kinetic Energy

( ) = M W ; the manipulator inertia tensor

X J
P
& &

=

where:
( )
1
1 1

=
T T
J JM J M J

Both Moore-Penrose pseudoinverse forms above are subject to singularities.
Singular Value Decomposition (SVD) would ameliorate this problem.

146
146

1.2 Homogeneous Solution - satisfies secondary task (optimization)
Project arbitrary vector into the null-space of the Jacobian matrix:

doesnt affect the Cartesian trajectory:
H
&
causes 0 = X
&
.

For optimization, choose:

where ( ) H is an objective function of joint angles to be minimized
or maximized. k > 0 for maximization and k < 0 for minimization.

1.2.1 Joint Limit Avoidance

1.2.2 Manipulability Maximization (Singularity Avoidance)

1.3 Total Solution

Sum of particular and homogeneous solutions (linear superposition):

2 Klein & Huangs Algorithm

Same result as pseudoinverse with gradient projection into the null-space, more
efficient (less computations). Accomplishes particular and homogeneous at same time.

( ) ( ) = H kJ X w JJ
T
&
;

solve w using Gaussian elimination, then:

( ) + = H k w J
T
&
.

147
147

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

3 Singular Value Decomposition

Same result as pseudoinverse with gradient projection into the null-space, but with
singularity robustness. If J is less than full rank, solution cannot track arbitrary Cartesian
trajectories, but results are bounded so motion can drive through singularities.

T
UWV J =

J mxn
U mxn column orthogonal
W nxn positive semi-definite diagonal
V nxn orthogonal

T
j
U
w
diag V J
(
(
|
|
.
|
\
|
=
1

j
w are the singular values of J. For underconstrained system 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,
j
w
1
is set to zero for 0 =
j
w ! AINT MATH FUN!!?!

Both U and V are column-orthonormal (ignoring last n m columns of U). V is
also row-orthonormal, i.e.:
n
T T
I V V VV = =
m
T
I UU = ; however,
n
T
I U U (see 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
j
w are an orthonormal basis for the
null-space of J.
4 Kinematically-Redundant Manipulator Singularities

148
148

singular when:

.

(due to ( )
1
=
T T
JJ J J ).
Use of SVD avoids this problem in the neighborhood of manipulator singularities.

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:

G AGA=

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 hold for the Moore-Penrose
pseudoinverse:

( )
( ) AG AG
GA GA
A GAG
=
=
=
*
*

where ( )
*
indicates the complex conjugate transpose.

149
149

Kinematically-Redundant Manipulator Example

Resolved-rate algorithm; solve
&
from
& &
J X = .

3-link planar robot required to satisfy only x,y (not x,y, ). The Jacobian matrix for this
case is of order 2x3 (giving translational velocity of hand frame {H} origin with respect
to the base frame {0}, expressed in {0} coordinates):

(
+ + +

=
123 3 123 3 12 2 123 3 12 2 1 1
123 3 123 3 12 2 123 3 12 2 1 1 0
c L c L c L c L c L c L
s L s L s L s L s L s L
J

For a numerical example, let L
i
= 1 and
o o o
30 , 60 , 60
3 2 1
= = = . The required
trajectory (instantaneous Cartesian rates for this snapshot example) is { }
T
X 1 1 =
&
.
(

=
866 . 0 866 . 1 366 . 2
500 . 0 500 . 0 366 . 1
0
J

1) Particular Solution ( ) X JJ J X J
T T
P
& & &
1
= =

(
=
830 . 9 598 . 4
598 . 4 366 . 2
T
JJ ( )
(
118 . 1 173 . 2
173 . 2 646 . 4 1
T
JJ both symmetric

(
(
(

=
118 . 0 441 . 0
000 . 1 732 . 1
323 . 0 205 . 1
*
J
=
)
`
(
(
(

=
559 . 0
732 . 2
527 . 1
1
1
118 . 0 441 . 0
000 . 1 732 . 1
323 . 0 205 . 1
P
&

This
P
&
is the least-norm solution for joint rates. Lets check: find another solution and
see if the norm is greater, as it should be. The simplest way to find another solution (out
of the one-infinity of solutions) is to lock a joint and solve for the other two. I chose
locking joint 2 arbitrarily, but this yielded a singular reduced Jacobian matrix! (This is
the planar equivalent of the elbow constraint I discovered for the 8-dof ARMII.) So then
I chose to lock joint 1:

0
1
=
&
, remove column 1 from Jacobian; this reduced problem is no longer redundant:

150
150

)
`

=
)
`
3
2
866 . 0 866 . 1
500 . 0 500 . 0
1
1
&
&
;
)
`

=
)
`
1
1
000 . 1 732 . 3
000 . 1 732 . 1
3
2
&
&
;
=
732 . 4
732 . 2
0
P
&

This particular solution has a larger Euclidean norm than that associated with the
pseudoinverse solution: 5.464 > 3.179. Q.E.D.

2) Homogeneous Solution ( )z J J I
H

=
&

The Moore-Penrose pseudoinverse is a right-sided inverse, that is:

m
I JJ =
*
, but
n
I J J
*

(
(
(
=
118 . 0 0 323 . 0
0 1 0
323 . 0 0 882 . 0
*
J J for this example.

The null-space-projection matrix is:
( )
(
(
(
=

882 . 0 0 323 . 0
0 0 0
323 . 0 0 118 . 0
J J I .

This matrix projects an arbitrary vector z into the null-space of the Jacobian matrix,
meaning the resulting homogeneous solution
H
&
maps through J to the zero vector.
Using an arbitrary z (not attempting to optimize anything):

(
(
(
=
559 . 0
0
205 . 0
1
1
1
882 . 0 0 323 . 0
0 0 0
323 . 0 0 118 . 0
H
&

Homogeneous term for elbow joint will always be zero! Primary solution cannot be
added to because only elbow controls distance from shoulder to wrist points!

Check:
)
`
=
0
0
H
J
&
.

The physical interpretation of the null space for this simple example is like a planar 4-bar
mechanism: If we fix the position x,y of the hand, this is just like having a second fixed
pivot. But of the hand is not constrained, so the self-motion of the 3-link planar robot

151
151

is just the motion of the 4-bar mechanism through its range of motion. Different
combinations of the joint angles
3 2 1
, , yield the same x,y.

3) Total Solution ( )z J J I X J
H P

+ = + =
& & & &

=
0
732 . 2
732 . 1
&
(3
rd
component is not always 0!)

check:
)
`

=
1
1
0
732 . 2
732 . 1
866 . 0 866 . 1 366 . 2
500 . 0 500 . 0 366 . 1
&
J

152
152

Same Example - Singular Value Decomposition (SVD)

(

=
866 . 0 866 . 1 366 . 2
500 . 0 500 . 0 366 . 1
J

T
UWV J =

T
J
(
(
(

(
(
(
=
939 . 0 188 . 0 288 . 0
0 836 . 0 548 . 0
344 . 0 514 . 0 786 . 0
0 0 0
0 420 . 0 0
0 0 467 . 3
0 430 . 0 903 . 0
0 903 . 0 430 . 0

T
j
U
w
diag V J
(
(
|
|
.
|
\
|
=
1

(
(
(

=
(
(
(
(
(
(
(
(
(

=
118 . 0 441 . 0
000 . 1 732 . 1
323 . 0 205 . 1
0 0
430 . 0 903 . 0
903 . 0 430 . 0
0 0 0
0 381 . 2 0
0 0 288 . 0
939 . 0 188 . 0 288 . 0
0 836 . 0 548 . 0
344 . 0 514 . 0 786 . 0
*
J

In the above, the third singular value is zero, so
j
w
1
is set to zero (n m singular values
will always be zero). Agrees with
*
J from Moore-Penrose formula.

Note:
2
I UU
T
=
(
(
(
=
0 0 0
0 1 0
0 0 1
U U
T

3
I V V VV
T T
= =

153
153

More General Redundant Robot: Add Rotational Rate

Figure:
X
Y
Z
0
0
1
X
1
X
2
X
3
X
H
Z
0
Z
2
Z
3
Z
H
Y
H
L
2
L
3
L
1
1
2
4
Z
4
L
4

4-link planar robot for general planar Cartesian trajectories x,y, . The Jacobian matrix
for this case is of order 3x4 (giving translational and rotational velocity of hand frame
{H} origin with respect to the base frame {0}, expressed in {0} coordinates):

Actuation redundancy n m = 4-3 = 1 for translational and rotational rates.

Actuation redundancy n m = 4-2 = 2 for translational rates only. (cut last Jacobian row)

154
Parallel Manipulators

Serial vs. Parallel robot demonstrate with wooden model kit.

Examples: Stewart Platform, Variable Geometry Trusses, NASA Hyperredundant
Serpentine Arm, Backhoe, etc.

Serial Robot: joints and links extend from
the base in a serial fashion to the end-
effector. Overall structure is cantilevered
w.r.t. end-effector loads. All joints must be
active.
Parallel Robot: joints and links arranged in
parallel from the base to the end-effector.
Multiple load paths from the end-effector to
the base. Combination of active and
passive joints.

Comparison of parallel with serial manipulators
Advantages of parallel manipulators
Higher loads
Higher accelerations
Lower mass
Higher stiffness
Better accuracy, repeatability
Ground-mounted actuators
Direct-drive
Open structure for cabling

Disadvantages of parallel manipulators
SMALLER WORKSPACE
Generally analysis is more difficult

Mobility the Grubler (planar) and Kutzbach (spatial) mobility equations (see earlier in
notes) 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. Infinite variations and possible designs.

155
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:
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.

There exists an interesting duality between serial and parallel manipulators: for parallel
manipulators, the inverse pose and velocity solutions are generally straight-forward and
the forward pose and velocity problems are harder!

We could also do Dynamics but this is of less concern for a parallel robot than a serial
robot just due to the parallel nature and inherent advantages. See Tsais textbook.

Serial robot kinematics DH parameters, homogeneous transformation matrices
Parallel robot kinematics Vector Loop-Closure Equations; phasors; Eulers identity

Phasors powerful, attractive, Re-Im manner to represent a vector:

Eulers Identity:

Compact notation; VERY convenient for time derivatives (velocity analysis):

Example Vector Loop-Closure Equation; Crawdad robot:

156
Planar 3-RPR Manipulator Kinematics

Planar 3-RPR Manipulator Diagram:

Inverse Pose Kinematics (for pose control)

Given: Find:

Vector Loop Closure Equations:

V.L.C.E. rewritten:

Inverse pose solution:

Where:

What if we need the intermediate passive joint variables
3 2 1
, , ?:

157
Forward Pose Kinematics (for simulation and sensor-based control): 3-RPR robot

Given: Find:

This is a coupled, nonlinear problem to solve difficult to solve and multiple solutions
exist; just like Inverse Pose Kinematics for serial robots!

Use same Vector Loop Closure Equations:

V.L.C.E. details expanded for one RPR leg:

Considering all three legs (the problem is coupled), these represent six scalar equations in
the six unknowns
3 2 1
, , , , , y x .

We will use the Newton-Raphson numerical iteration technique to solve this Forward Pose Kinematics
Problem. We can just directly solve the above six equations for the six unknowns; however, we dont
necessarily need the intermediate variables
3 2 1
, , (we can calculate them later, using Inverse Pose
Kinematics, if required for dynamics or 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
3 2 1
, , ; then we will have three equations to solve for the primary unknowns , , y x :

Bring
2
i
L to the other side and solve via Newton-Raphson.

158
Newton-Raphson Method

Numerical iteration to solve coupled sets of n nonlinear equations
(algebraic/transcendental not ODEs) in n unknowns. Requires a good initial guess of
the solution to get started and only yields one of the multiple solutions. Extension of
Newtons single function/single variable root-finding technique to n functions and n
variables. Form of given functions to solve:

( ) 0 X F =

where the n functions are: ( ) ( ) ( ) ( ) { }
T
n
F F F X X X X F L
2 1
=

and the n variables are: { }
T
n
x x x L
2 1
= X

Taylor Series Expansion of F about X:

( ) ( ) ( )
2
1
X X X X O x
x
F
F F
j
n
j
j
i
i i
+
+ = +

=
n i , , 2 , 1 L =

Where ( )
(
(
= =
j
i
NR NR
x
F
J J X is the Newton-Raphson Jacobian Matrix, a multi-
dimensional form of the derivative and a function of X. If X is small, the higher-order
terms ( )
2
X O from the expansion are negligible. For solution, we require:
( ) 0 = + X X
i
F n i , , 2 , 1 L =

Now with ( ) 0
2
X O we have:
( ) ( ) ( ) 0
1
= + =
+ = +

=
X X X X X
NR i j
n
j
j
i
i i
J F x
x
F
F F n i , , 2 , 1 L =

So to calculate the required correction factor X at each solution iteration step, we must
solve ( ) 0 = + X X F
NR
J :
( ) X F X
1
=
NR
J

(actually, Gaussian elimination on ( ) X F X =
NR
J is preferable numerically).

159
Newton-Raphson Algorithm Summary

0) Establish functions and variables to solve for: ( ) 0 X F =

1) Make an initial guess to the solution:
0
X

2) Solve ( ) ( )
k k k NR
J X F X X = for
k
X , where k is the iteration counter

3) Update the current best guess for the solution:
k k k
X X X + =
+1

4) Iterate until <
k
X , where we use the Euclidean norm and 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 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 to far
from this known initial guess!). Also, except in the case of singularities where the multiple solution
branches converge, the one solution resulting is the one you want, closest to the initial guess, most likely
the actual configuration the real robot will be taking.
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
resolved-rate control.

3-RPR manipulator forward pose kinematics solution
Use the Newton-Raphson numerical iteration method for solution, with:
{ }
T
y x = 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.

160
Derive the required Newton-Raphson Jacobian Matrix:

( )
(
(
=
j
i
NR
x
F
J X

=
x
F
i
3 , 2 , 1 = i

=
y
F
i
3 , 2 , 1 = i

=
i
F
3 , 2 , 1 = i

where:
)
`
=
y
x
i
B
A
A
A and
)
`
=
y
x
i
H
P
P
C

The index i was dropped for convenience; use 3 , 2 , 1 = 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
3 2 1
, , as in inverse pose kinematics above.

ALTERNATE 3-RPR manipulator forward pose NEWTON-RAPHSON solution

As we said, we could have solved the original 6 equations in the 6 unknowns including the
intermediate variables
3 2 1
, , . The functions are simpler (no squaring & adding) but the size of the
problem is doubled to 6 , , 2 , 1 L = 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
{ }
T
y x
3 2 1
= X .

161
( )
(
(
(
(
(
(
(
(

=
3 3 3 3
3 3 3 3
2 2 2 2
2 2 2 2
1 1 1 1
1 1 1 1
0 0 1 0
0 0 0 1
0 0 1 0
0 0 0 1
0 0 1 0
0 0 0 1

c L s P c P
s L c P s P
c L s P c P
s L c P s P
c L s P c P
s L c P s P
J
y x
y x
y x
y x
y x
y x
NR
X

ALTERNATE 3-RPR manipulator forward pose solution: ANALYTICAL

Figure:

Branch
2 2 1 1
A C C A is a 4-bar mechanism with input angle
1
and output angle
2
. With given
lengths L
1
and L
2
, this 4-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
C A (a circle of given radius L
3
, centered at
3
A ) 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.

162
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 (same for serial robots): 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
o
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 values (end-effector orientation) within the
desired limits. Then stack these up and intersect to find the dexterous workspace, defined for a desired
range
LIMIT
.

163
Velocity Kinematics: 3-RPR robot

First the pose configuration variables must all be known; then we can define and solve two problems:

Forward velocity kinematics (simulation):

Given: Find:

Inverse velocity kinematics (resolved-rate control):

Given: Find:

In both cases we will have intermediate passive joint rate unknowns
3 2 1
, ,
& & &
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:

Velocity equations for one RPR leg (1
st
time derivative of XY and angle equations):

164
Written in Matrix-vector form (RPR leg Jacobian matrix):

Compact notation:

{ }
T
z
y x X & &
&
= is the same for all three legs (Cartesian rates of end-effector).
i
& includes one
active and two passive joint rates for each of the three RPR legs, 3 , 2 , 1 = 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 required, the Inverse
Velocity problem is never singular!! Compact notation:

165
Forward Velocity Problem

Compact notation (solve numerically from 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:

o
o
270
120
3 3
2 2 2
1 1 1
+ = +
+ + = +
+ = +

The second two relationships are tailored for
o
30
3 2 1
= = = (equilateral end-effector triangle).

166
Example Symbolic Matlab Code

As a final small treat, here is the code that was used to symbolically invert the RPR leg Jacobian
matrix a few pages back. Symbolic computing has a lot of power in robotics kinematics, dynamics, and
control!

%
% Symbolic Matlab code to invert the RPR leg Jacobian symbolically
%

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);

Advanced Robotics DR Bob

Загружено:

Сведения о документе

Оригинальное название

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Advanced Robotics DR Bob

Загружено:

Авторское право:

Доступные форматы

ME 604

for this first rotation only).

are the rows of R

axes are NOT identical

; can get 1 and 3.

; get 2, with 1 and 3 known.

; isolate and solve 4, 5, and 6.

& & &

& & & &

& & &

& & & & & &

& & & &

Вам также может понравиться