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
DENAVITHARTENBERG (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
KINEMATICALLYREDUNDANT 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 degreesoffreedom
(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) DegreesofFreedom  the number of independently controllable motions in a
mechanical device. The number of motors in a serial manipulator.
5) Mechanism  a 1DOF 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
degreesoffreedom 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 endeffector;
natural for humans.
18) Endeffector  tool or hand at the end of a robot.
19) Workspace  The volume in space that a robots endeffector 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 endeffectors 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 stateof
theart in robotics still largely focuses robot arms. There is much current research work
aimed at creating humanlike 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 highaccuracy, highrepeatability, highprecision 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!  nonlinear).
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 lefthand matrix must equal the number of rows in
the righthand matrix; if not, the multiplication is undefined and cannot be done!
Multiplication proceeds by multiplying and adding terms along the rows of the lefthand
matrix and down the columns of the righthand matrix: (use your index fingers from the
left and right hands):
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 righthand 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 nonzero; 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 Clike, 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.
Doubleclick on Matlab icon to get started. Type
>>demo
to get a comprehensive overview of Matlab including builtin 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: MFiles. Put your sequence of MATLAB statements in
an ASCII file name.m (can create with the beautiful Matlab Editor/Debugger  this is
colorcoordinated, tabfriendly, 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 MFile name name, without the
.m. Matlab language is interpretive and executes linebyline. 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 MFile inside the Editor/Debugger to see the values! Online help is
generally great:
>>help
Example MFiles (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 righthand 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 degreesoffreedom which a robot or mechanism possesses.
For serial robots, simply count up the number of (single degreeoffreedom) 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 onedegreeoffreedom joints
J
2
is the number of twodegreeoffreedom joints
Onedegreeoffreedom joints: Revolute, Prismatic
Twodegreeoffreedom joints: Cam joint (rolling and sliding), Gear joint
General planar nlink serial robot has n+1 links (including fixed ground link), connected
by n active 1dof 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 onedegreeoffreedom joints
J
2
is the number of twodegreeoffreedom joints
J
3
is the number of threedegreeoffreedom joints
J
4
is the number of fourdegreeoffreedom joints
J
5
is the number of fivedegreeoffreedom joints
Onedegreeoffreedom joints: Revolute, Prismatic, Screw
Twodegreeoffreedom joints: Cylindrical, Gear joint
Threedegreeoffreedom joints: Spherical
Fourdegreeoffreedom joints: Spherical in a slot
Fivedegreeoffreedom joints: Spatial cam
General spatial nlink serial robot has n+1 links (including fixed ground link), connected
by n active 1dof 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 righthanded 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 rowwise:
(
(
(
=
(
(
(
=
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):
XYX
XYZ
XZX
XZY
YXY
YXZ
YZX
YZY
ZXY
ZXZ
ZYX
ZYZ
Note: XYY, 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:
ZYX () 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
(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 ZYX () Euler angles in Chapter 2, in the XYZ
() 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
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 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. rollpitchyaw, 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 XYZ
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 matrixvector 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}. Righthand 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
=
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 ZYX 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
DenavitHartenberg (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. {i1}. Joints {i1} and {i} connected by link {i1}.
Cartesian coordinate frame {i1} rigidly attached to link {i1}; moves with joint i1.
J. Denavit and R.S. Hartenberg, 1955, "A Kinematic Notation for LowerPair
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 {i1} to {i} (or, give pose of {i} w.r.t. {i1}).
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:
DH 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. 3axis 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. 3axis 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. 6axis 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
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 ZYX () 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 endeffector 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
1. 3axis Planar 3R Articulated Robot
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 SumofAngles 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
1. 3axis Planar 3R Articulated Robot
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
2. 3axis Spatial PRP Cylindrical Robot
Forward Pose Kinematics Symbolic Derivations
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
2. 3axis Spatial PRP Cylindrical Robot
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. 6axis Spatial 6R Unimation PUMA Robot
Forward Pose Kinematics Symbolic Derivations
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 sumofangles 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 sumofangles 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 endeffector
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:
Nonlinear, 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 5DOF (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 axissymmetric Cartesian tasks not requiring the robot
roll freedom.
Solution Methods
Numerical e.g. NewtonRaphson. Requires a good initial guess and only finds
one solution, closest to the initial guess.
Analytical (closedform)
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 nonlinear, while the inverse kinematics
solution is generally more straightforward. This is reversed for serial robots. (See the
notes section on Parallel Manipulators.)
Tanhalf 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 closedform (analytical); maybe the solution has to be numerical.
63
Inverse Pose Kinematics Examples
1. 3axis 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
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 6dof 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 HalfAngle 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 HalfAngle 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 quadrantspecific 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
Inverse Pose Kinematics Examples
1. 3axis Planar 3R Articulated Robot
Inverse Pose Kinematics Examples
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.)
2. 3axis 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
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 6dof
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
Inverse Pose Kinematics Examples (cont.)
2. 3axis Spatial PRP Cylindrical Robot
Inverse Pose Kinematics Examples
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 6dof robot has any three consecutive joint axes
intersecting, there exists a closedform (analytical) solution to the inverse position
kinematics. The majority of industrial robots are in this category.
In particular, many robots have a spherical wrist  three wrist actuators rotate about
a common point. In this case, the position and orientation subproblems 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.
Peeloff 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 =
+ +
+
+
=
=
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 sumof
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
(casesensitive!)
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 HalfAngle 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 HalfAngle 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 quadrantspecific 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 Examples (cont.)
3. 8axis Spatial 8R NASA AAI ARMII Robot
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. 321 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. 321 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 threepart 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. ZYX ( , , ) Euler angle convention:
(
(
(
+ +
+ +
=
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
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 multidimensional 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 endeffector 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 allrevolute 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 endeffector link.
Revolute:
( )
1
1
1
1
1
1
1
1
1
1
+
+
+
+
+
+
+
+
+
+
+ =
+ =
i
i
i
i
i
i i
i i
i
i
i
i i
i i
i i
i
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
1. 3axis 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
=
& &
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
2) Column as velocity due to joint i.
( ) ( ) ( )
(
(
=
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
3) Velocity recursion a'la Craig.
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
2) Column as velocity due to joint i.
(
(
=
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
3) Velocity recursion a'la Craig.
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 endeffector 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
(
(
=
1. 3axis Planar 3R Articulated Robot
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, kinematicallyredundant robots
MoorePenrose 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
ResolvedRate 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 endeffector. 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 illconditioned.
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 endeffector
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 endeffector to push on the environment with F. This can be seen
with a simple onelink 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
1. 3axis Planar 3R Articulated Robot
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. Freebody 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 blocktranspose 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 blocktranspose, 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, nonlinear 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 fivepart 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 2link manipulator  acceleration of endeffector 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
+
+
=
(
(
(
=
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
+
= =
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 2link manipulator  acceleration of endeffector 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
( )
+ =
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.
Parallelaxis 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
Vectormatrix form:
[ ]
T
C C C
T
C
C A
P P I P P m I I + =
3
118
NewtonEuler Recursive Algorithm
Freebody diagram of link i:
i
f : Internal force exerted on link i by link i1.
i
n : Internal moment exerted on link i by link i1.
Inertial loads: Newton and Euler equations:
Force balance:
Moment balance (about CG
i
): (using D'Alemberts principle: inertial force is ma ).
119
NewtonEuler 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 revolutejoint 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
LagrangeEuler 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 1link 1R mechanism by 3 methods:
1) Sophomore methods FBD (freebody diagram), force balance.
2) NewtonEuler recursion.
3) LagrangeEuler formulation.
Y
L
X
,
1) Sophomore methods  FBD, force balance FreeBody 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) NewtonEuler 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) LagrangeEuler 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
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 resolvedrate 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 resolvedrate 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 2dof 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 NewtonEuler 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 multiaxis industrial robots are controlled via
linearized, independent single joint control. So, robot control in industry is generally
accomplished by using N independent (but simultaneous) linearized joint controllers,
where N is the number of joint space freedoms of the robot.
We now will 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 armaturecontrolled DC servomotor driving a single robot joint / gear train / link
combination. While we ignored actuator dynamics in the dynamics section we now
include it.
OpenLoop 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
ClosedLoop Feedback Control Diagram: High Level
Highlevel Computer Control Diagram
Simplifying Assumptions
rigid motor, load shafts
n >> 1 (reduce speed, increase torque)
Real World Our Model (Simplified)
Nonlinear Linearized
MIMO SISO
Coupled Decoupled
Timevarying 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):
Freebody 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 8axis 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 timevarying 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
OpenLoop 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!
ClosedLoop 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 steadystate 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
Steadystate 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, nonlinearities, 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
KinematicallyRedundant Manipulators
Redundancy extra, more than is required
3 types of electromechanical redundancy
Failsafe: 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, timeconsuming, 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; 27dof
arms on 5dof 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 closedchain with a workpiece
143
143
SSRMS with SPDM on International Space Station
8axis AAI ARMII (NASA LaRC) 8cable CDDHI
RoboNaut (NASA JSC) Human Arm with PMAs
144
144
KinematicallyRedundant 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.)
Selfmotion (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 (selfmotion)
145
145
Inverse Velocity (Resolved Rate) Solution
1 Pseudoinversebased
1.1 Particular Solution  satisfies primary task (satisfies Cartesian trajectory)
Minimize cost function:
Subject to:
1.1.1 Leastnorm Joint Rate Solution
W = I
where (MoorePenrose 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 MoorePenrose 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 nullspace 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 nullspace, 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 kinematicallyredundant 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 nullspace, 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 semidefinite 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 columnorthonormal (ignoring last n m columns of U). V is
also roworthonormal, 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 nonzero
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
nullspace of J.
4 KinematicallyRedundant 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 rowrank
deficient). Mathematically, G is a generalized inverse of matrix A if:
G AGA=
The MoorePenrose pseudoinverse is just one possible generalized inverse of a
matrix. It is the one applied most often to redundancy resolution of manipulators. In
addition to the above relationship, the following hold for the MoorePenrose
pseudoinverse:
( )
( ) AG AG
GA GA
A GAG
=
=
=
*
*
where ( )
*
indicates the complex conjugate transpose.
149
149
KinematicallyRedundant Manipulator Example
Resolvedrate algorithm; solve
&
from
& &
J X = .
3link 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 leastnorm 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 oneinfinity 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 8dof 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 MoorePenrose pseudoinverse is a rightsided 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 nullspaceprojection 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 nullspace 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 4bar
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 selfmotion of the 3link planar robot
151
151
is just the motion of the 4bar 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 MoorePenrose 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
4link 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 = 43 = 1 for translational and rotational rates.
Actuation redundancy n m = 42 = 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. endeffector loads. All joints must be
active.
Parallel Robot: joints and links arranged in
parallel from the base to the endeffector.
Multiple load paths from the endeffector 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
Groundmounted actuators
Directdrive
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 3DOF 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/DAC3851, Sacramento, CA,
September 1417, 1997.
There exists an interesting duality between serial and parallel manipulators: for parallel
manipulators, the inverse pose and velocity solutions are generally straightforward 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 LoopClosure Equations; phasors; Eulers identity
Phasors powerful, attractive, ReIm manner to represent a vector:
Eulers Identity:
Compact notation; VERY convenient for time derivatives (velocity analysis):
Example Vector LoopClosure Equation; Crawdad robot:
156
Planar 3RPR Manipulator Kinematics
Planar 3RPR 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 sensorbased control): 3RPR 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 NewtonRaphson 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 NewtonRaphson.
158
NewtonRaphson 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 rootfinding 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 NewtonRaphson Jacobian Matrix, a multi
dimensional form of the derivative and a function of X. If X is small, the higherorder
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
NewtonRaphson 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,
userdefined solution tolerance. Also halt the iteration if the number of steps
becomes too high (which means the solution is diverging): generally less than
10 iterations is required for even very tight solution tolerances.
If the initial guess to the solution
0
X is sufficiently close to an actual solution, the NewtonRaphson
technique guarantees quadratic convergence.
Now, for manipulator forward pose problems, the NewtonRaphson technique requires a good
initial guess to ensure convergence and yields only one of the multiple solutions. However, this does
not present 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 NewtonRaphson Jacobian Matrix is nearly identical to the
Inverse Velocity Jacobian Matrix for parallel robots. (In the planar case it is identical, in the spatial it is
related very closely.) This reduces computation if you need both forward pose computation and
resolvedrate control.
3RPR manipulator forward pose kinematics solution
Use the NewtonRaphson 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 NewtonRaphson 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 NewtonRaphson 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 3RPR manipulator forward pose NEWTONRAPHSON 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 3RPR manipulator forward pose solution: ANALYTICAL
Figure:
Branch
2 2 1 1
A C C A is a 4bar mechanism with input angle
1
and output angle
2
. With given
lengths L
1
and L
2
, this 4bar mechanism has 1DOF, and it can trace out a coupler curve for point C
3
in
the plane. In general, this coupler curve is a tricircular sextic. The forward pose kinematics solution
may be found by intersecting leg
3 3
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 3RPR parallel robot forward pose kinematics problem. There are
always six solutions, but 0,2,4, or 6 of these will be real, depending on the commanded configuration
and robot geometry.
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 endeffector in any orientation; 2) dexterous workspace is a
subset of the reachable workspace because it is the volume in 3D space reachable by the endeffector in
all orientations. For parallel robots, the dexterous workspace is almost always null since the rotation
capability is never full for all three Euler angles; therefore we usually define a reduced dexterous
workspace wherein all Euler angles can reach
o
30 or some other userdefinable range.
3RPR Example
For planar parallel robots we can generally find the reachable workspace using a geometric method,
figuring out what the endeffector can reach, guided by each leg on its own, and then intersecting the
results:
For determination of the dexterous workspace, it is most convenient to numerically or geometrically
generate in Matlab the reachable workspace for different values (endeffector 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: 3RPR 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 (resolvedrate 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 endeffector). Figure:
Vector LoopClosure 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 Matrixvector form (RPR leg Jacobian matrix):
Compact notation:
{ }
T
z
y x X & &
&
= is the same for all three legs (Cartesian rates of endeffector).
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 endeffector 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*s1LH*sp;
j21 = L*c1+LH*cp;
j13 = LH*sp;
j23 = LH*cp;
% Jacobian matrix
J = sym([j11 c1 j13;j21 s1 j23;1 0 1]);
% Invert
Jinv = inv(J);
% Simplify
Jinvsimp = simple(Jinv);
% Check
Ident1 = simple(Jinvsimp * J);
Ident2 = simple(J * Jinvsimp);
Mult mai mult decât documente.
Descoperiți tot ce are Scribd de oferit, inclusiv cărți și cărți audio de la editori majori.
Anulați oricând.