8/11/2019 Advanced Robotics Dr Bob
1/166
ME 604
ADVANCED ROBOTICS I
CLASS NOTES
DR. BOB
Mechanical Engineering
Ohio University
Dr. Bob Productions
8/11/2019 Advanced Robotics Dr Bob
2/166
2
Table of Contents
INTRODUCTION TO ROBOTICS ............................................................................. 3
MATRIX INTRODUCTION ...................................................................................... 12
MATLAB INTRODUCTION....................................................................................... 17
MOBILITY AND MOTION DESCRIPTION ........................................................... 21
ORTHONORMAL ROTATION MATRICES .......................................................... 24
HOMOGENEOUS TRANSFORMATION MATRICES.......................................... 33
DENAVIT-HARTENBERG (DH) PARAMETERS.................................................. 40
FORWARD POSE KINEMATICS ............................................................................ 49
INVERSE POSE KINEMATICS................................................................................ 60
VELOCITY ANALYSIS AND JACOBIAN MATRICESS ...................................... 80
ACCELERATION KINEMATICS ..........................................................................110
MANIPULATOR DYNAMICS .................................................................................116
SINGLE JOINT CONTROL .....................................................................................133
KINEMATICALLY-REDUNDANT MANIPULATORS ........................................142
PARALLEL MANIPULATORS ...............................................................................154
8/11/2019 Advanced Robotics Dr Bob
3/166
3
Introduction to Robotics
For a snazzier Intro to Robotics, please see:
http://www.ent.ohiou.edu/~bobw/PDF/IntroRob.pdf
8/11/2019 Advanced Robotics Dr Bob
4/166
4
Some Definitions:
1) Robot An electromechanical machine with more than one degrees-of-freedom
(DOF) which is programmable to perform a variety of tasks. From R.U.R. by the
Czechoslovakian playwright Karl Kapek; robot - worker or serf.
2) Anthropomorphic: Similar to Humans.
3) Manipulator - mechanical arm, with several DOF.
4) Degrees-of-Freedom - the number of independently controllable motions in a
mechanical device. The number of motors in a serial manipulator.
5) Mechanism - a 1-DOF machine element.
6) Fixed Automation - designed to perform a single repetitive task.
7) Flexible Automation - can be programmed to perform a variety of tasks.
8) Robot system - manipulator(s), sensors, actuators, communication, computers,
interface, hand controllers to accomplish a programmable task.
9) Actuator - motor that drives a joint; generally rotary (revolute) or linear (prismatic);
electric, hydraulic, pneumatic, piezoelectric.
10) Cartesian Coordinate frame - dextral, orthogonal,XYZ, kji ,, .
11) Kinematics - the study of motion without regard to forces. Cartesian Pose: position
and orientation of a coordinate frame.
a) Forward Kinematics - given the joint variables, calculate the Cartesian pose.
b) Inverse Kinematics - given the Cartesian pose, calculate the joint variables.
12) Position (Translation) - measure of location of a body in a reference frame.
13) Orientation (Rotation) - measure of attitude of a body (e.g. Roll, Pitch, Yaw) in a
reference frame.
14) Singularity - a configuration where the manipulator momentarily loses one or more
degrees-of-freedom due to its geometry.
8/11/2019 Advanced Robotics Dr Bob
5/166
5
15) Actuator Space - vector of actuator commands, connected to joint through gear train
or other drive.
16) Joint Space - vector of joint variables; basic control parameters.
17) Cartesian Space - Position vector and orientation representation of end-effector;
natural for humans.
18) End-effector - tool or hand at the end of a robot.
19) Workspace - The volume in space that a robots end-effector can reach, both in
position and orientation.
20) Dynamics - the study of motion with regard to forces (the study of the relationship
between forces/torques and motion). Composed of kinematics and kinetics.
a) Forward Dynamics (simulation) - given the actuator forces and torques,
compute the motion.
b) Inverse Dynamics (control) - given the desired motion, calculate the actuator
forces and torques.
21) Control - causing the robot system to perform the desired task. Different levels.
a) Teleoperation - human moves master, slave manipulator follows.
b) Automation - computer controlled (using sensors).
c) Telerobotics - combination of the b) and c)
22) Haptics - From the Greek, meaning to touch. Haptic interfaces give human
operators the sense of touch and forces from the computer, either in virtual or real,
remote environments. Also called force reflection.
8/11/2019 Advanced Robotics Dr Bob
6/166
6
Robot Applications
Traditionally, robots are applied anywhere one or more of the 3Ds exist: in any job which
is too:Dirty,Dangerous, and/orDullfor a human to perform.
IndustryManufacturing, assembly, part handling, palletizing, maintenance, inspection, welding,
spray painting, deburring, machining.
Remote operationsUndersea, nuclear environment, bomb disposal, law enforcement, outer space, other
hazardous environments.
Service
Hospital helpmates, handicapped assistance, retail, household servants, lawnmowers,security guards.
8/11/2019 Advanced Robotics Dr Bob
7/166
7
Common Robot Configurations
CartesianRobots 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.
CylindricalThe 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.
SphericalSpherical robots have two rotational R axes and one translational P (radius) axis. The
robots end-effectors can trace out concentric spheres in 3D space.
ArticulatedThe 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/11/2019 Advanced Robotics Dr Bob
8/166
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 robotsMobile 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 robotsMany students (and U.S. Senators) expect to see C3PO (from Star Wars) walking around
when visiting a robotics laboratory. Often they are disappointed to learn that the state-of-
the-art in robotics still largely focuses robot arms. There is much current research work
aimed at creating human-like robots that can walk, talk, think, see, touch, etc. Generally
Hollywood and science fiction lead real technology by at least 20 or 30 years.
Discussion: will robots ever replace human beings?!?
8/11/2019 Advanced Robotics Dr Bob
9/166
9
Honda Humanoid Robot
Parallel robotsMost 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 greateraccuracy, higher speeds, and lighter robot weight; however, a major drawback is that the
workspace of parallel robots is severely restricted compared to equivalent serial robots.
Parallel robots are used in expensive flight simulators, as machining tools, and can be
used for high-accuracy, high-repeatability, high-precision robotic surgery.
8/11/2019 Advanced Robotics Dr Bob
10/166
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.:
PA is a position vector Pexpressed in Cartesian coordinate frame {A}.
BAP is a position vector from the origin of frame {A} to the origin of frame {B}
expressed in the basis of frame {A}.
RAB is an orthonormal rotation matrix giving the orientation of frame {B} relative to
frame {A}.
TAB 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: 1R or TR .
4) Trailing subscripts indicate vector components (x,y,z) or are descriptive.
5) Trigonometric functions are often abbreviated:
ii
ii
ii
t
s
c
=
=
=
tan
sin
cos
Required Mathematics
Vectors, matrices, linear algebra
Trigonometry, geometry
Calculus
Ordinary differential equations
8/11/2019 Advanced Robotics Dr Bob
11/166
11
Brief Review of Trigonometry & Calculus
IV Quadrants - degrees and radians
Sine, Cosine, Tangent functions
Inverse Functions - include atan2
Some trig identities:
( ) ( )
( ) ( )aa
aa
coscos
sinsin
=
=
( )
( ) casbsacbba
sasbcacbba
=
=
sin
cos m
122 =+ ii sc
Law of Cosines: cABBAC cos2222 +=
Law of Sines:C
c
B
b
A
a sinsinsin==
Some Relations from Calculus
derivatives (including chain rule), integrationdt
dx
dx
df
dt
df=
)(
)(
)(
2
2
txdt
sd
dt
dva
txdt
dsv
txs
&&
&
===
==
=
===
==
=
)(
)(
)(
txavs
txav
txa
&
&&
8/11/2019 Advanced Robotics Dr Bob
12/166
12
Matrix Introduction
Matrix: m x n array of numbers, where mis the number of rows and nin the number of
columns.
[ ]
=
mnmm
n
n
aaa
aaa
aaa
A
L
MOMM
L
L
21
22221
11211
Used to simplify and standardize solution of n linear equations in nunknowns (where
m=n). Used in velocity, acceleration, and dynamics analysis linear equations (not
position! - non-linear).
Special Matrices
Square (m=n=3) [ ]
=
333231
232221
131211
aaa
aaa
aaa
A Diagonal [ ]
=
33
22
11
00
00
00
a
a
a
A
Identity [ ]
=
100
010
001
I Transpose [ ]
=
332313
322212
312111
aaa
aaa
aaa
AT
Symmetric [ ] [ ]
==
332313
232212
131211
aaa
aaaaaa
AA T
Column Vector (3x1 matrix) { }
=
3
2
1
x
x
x
X
Row Vector (1x3 matrix) { } { }321
xxxX T
=
Matrix Addition Just add up like terms
++
++=
+
hdgc
fbea
hg
fe
dc
ba
8/11/2019 Advanced Robotics Dr Bob
13/166
13
Matrix Multiplication with Scalar Just multiply each term
=
kdkc
kbka
dc
bak
Matrix Multiplication [ ] [ ][ ] [ ][ ]ABBAC =
Row, Column indices have to line up as follows:
[ ] [ ][ ]
( ) ( )( )pxnmxpmxn
BAC
=
That is, the number of columns in the left-hand matrix must equal the number of rows in
the right-hand matrix; if not, the multiplication is undefined and cannot be done!
Multiplication proceeds by multiplying and adding terms along the rows of the left-hand
matrix and down the columns of the right-hand matrix: (use your index fingers from theleft and right hands):
Example:[ ]
( ) ( )( )133212 xxx
fiehdg
cibhag
i
h
g
fed
cbaC
++
++=
=
note the inner indices (p=3) must match, as stated above.
Matrix Multiplication Examples
[ ]
=
654
321A [ ]
=
67
89
87
B
[ ] [ ][ ]
=
++++
++++=
==
108115
4246
364032424528
1816821187
67
89
87
654
321BAC
( ) ( )( )233222 xxx
8/11/2019 Advanced Robotics Dr Bob
14/166
14
[ ] [ ][ ]
=
+++
+++
+++
=
==
574431
755841
695439
36213014247
48274018329
48214014327
654
321
67
89
87
ABD
( ) ( )( )322333 xxx
Matrix Inversion Matrix division [ ] [ ][ ]BAC = solve for [B]
[ ] [ ][ ] [ ] [ ] [ ] [ ][ ] [ ][ ] [ ] [ ] [ ] [ ]CABBBIBAACABAC 111 =====
Matrix [A] must be square.
[ ][ ] [ ] [ ] [ ]IAAAA == 11 (identity matrix, the matrix 1).
[ ] ( )[ ]
A
AAdjoA
int1=
A Determinant of [A]
( )[ ] ( )[ ]TACofactorAAdjo =int
Cofactor of ( ) ijji
ijij Ma +
= 1
Minor ijM of ija is determinant of submatrix with row iand columnjremoved.
System of Linear Equations nequations in nunknowns.
For n=3:
3333232131
2323222121
1313212111
bxaxaxa
bxaxaxa
bxaxaxa
=++
=++
=++
Using matrix multiplication (backwards), this is written as:
[ ]{ } { }bxA = where:
[ ]
=
333231
232221
131211
aaa
aaa
aaa
A (known coefficients)
8/11/2019 Advanced Robotics Dr Bob
15/166
15
{ }
=
3
2
1
x
x
x
x (unknowns to be solved) { }
=
3
2
1
b
b
b
b (known right-hand sides)
Unique solution { } [ ] { }bAx1
=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.
1446
52
=+
=+
yx
yx
=
14
5
46
21
2
1
x
x
[ ]
=
46
21A { }
=2
1
x
xx { }
=14
5b
{ } [ ] { }bAx 1=
( ) ( ) 86241 ==A Determinant non-zero; unique solution!
[ ]
=
=
8/14/3
4/12/1
16
2411
AA
check: [ ][ ] [ ] [ ] [ ]
===
10
012
11IAAAA
=
=
2
1
14
5
8/14/3
4/12/1
2
1
x
x Answer.
check: Plug answer into original equations and compare {b}.
8/11/2019 Advanced Robotics Dr Bob
16/166
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 Ax = invA*b; % Solve linear equations
x1 = x(1); % Extract answers
x2 = x(2);
AT = A; % Matrix transpose
8/11/2019 Advanced Robotics Dr Bob
17/166
17
MatlabIntroduction
MATrixLABoratory
Control systems simulation and design software. Very widespread in other fields.
Introduction to basics, programming, plots, animation, matrices, vectors. Based on Clanguage, programming is vaguely C-like, but much simpler to use. Sold by Mathworks
(http://www.mathworks.com).
Can buy student version software and manual for about the price of one textbook (can use
it for many classes!). ENT college has a Matlab license; it is installed in most computer
labs.
Double-click on Matlab icon to get started. Type
>>demo
to get a comprehensive overview of Matlab including built-in functions. Try all the
categories under Matlab first; you can ignore Toolboxes, Simulink, and Stateflow for
now. (Exception: there is Symbolic Math under Toolboxes for the adventurous student!).
Type in commands (such as the Vector/Matrix examples given earlier) at the Matlab
prompt >>. Press to see result or ; to suppress result.
Recommended operation mode: M-Files. Put your sequence of MATLAB statements inan ASCII file name.m (can create with the beautiful Matlab Editor/Debugger - this is
color-coordinated, tab-friendly, with parentheses alignment help and debugging
capabilities). Make sure the file exists where the Matlab search path can find it. If you
use the 1.4M disk drive, at the >> prompt, type:
>>cd a:
This instructs Matlab that your default directory is on the a:disk; you are free to specify
a directory structure under a:. At the >> prompt type the M-File namename, without the.m. Matlab language is interpretive and executes line-by-line. Use the ; at the end of
statements to suppress intermediate results. If you use this suppression, the variable
name still holds the resulting value(s) - just type the variable name at the prompt after the
program runs to see the value(s). If there is a syntax or programming logic error, it will
give a message at the bad line and then quit. Type:
>>who
8/11/2019 Advanced Robotics Dr Bob
18/166
18
to show you what variables you have defined;
>>whos
will show the variables, plus their matrix dimensions (scalar, vector array, or matrix),
very useful for debugging. Plus, after running a file, place the cursor over different
variables in the M-File inside the Editor/Debugger to see the values! On-line help is
generally great:
>>help
Example M-Files (given on the following two pages)
1) Matlb1.m: Input, programming, plots, animation.
2) Matlb2.m: Matrix and vector definition, multiplication, transpose, and solution of
linear equations.
8/11/2019 Advanced Robotics Dr Bob
19/166
19
%
% Matlab Example Code 1: Matlb1.m
% Matrix, Vector examples
% Dr. Bob, ME 604
%
clc; clear; % Clear the cursor and clear all previously defined variables
%
% Matrix and Vector definition, multiplication, and transpose
%
A = [1,2, 3; ... % Define 2x3 matrix [A] (... is continuation line)
1,1,-1];
x = [1;2;3]; % Define 3x1 vector {x}
v = A*x; % 2x1 vector {v} is the product of [A] times {x}
AT = A'; % Transpose of matrix [A]
vT = v'; % Transpose of vector {v}
%
% Solution of linear equations Ax=b
%
A = [1,2, 3; ...% Redefine matrix [A] to be 3x3: coefficient matrix
1,1,-1; ...
8,2,10];
b = [3;2;1]; % Define right-hand side vector of knowns {b}
detA = det(A); % First check to see if det(A) is near zero
x = inv(A)*b; % Redefine {x} to be the solution of Ax=b by inversion
check = A*x; % Check results;
z = b - check; % Better be zero!
%
% Display the created variables (who), with dimensions (whos)
%
who
whos
8/11/2019 Advanced Robotics Dr Bob
20/166
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 windowif 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');
8/11/2019 Advanced Robotics Dr Bob
21/166
21
Mobility and Motion Description
Mobility
Mobility - the number of degrees-of-freedom which a robot or mechanism possesses.
For serial robots, simply count up the number of (single degree-of-freedom) joints.
Grubler's Criterion: Planar robots
( ) 21 1213 JJNM =
Where:
Mis the mobility
Nis the total # of links, including groundJ1is the number of one-degree-of-freedom joints
J2is the number of two-degree-of-freedom joints
One-degree-of-freedom joints: Revolute, Prismatic
Two-degree-of-freedom joints: Cam joint (rolling and sliding), Gear joint
General planar n-link serial robot has n+1 links (including fixed ground link), connected
by nactive 1-dof joints:
( ) ( ) nnnnnM ==+= 23012113 dof
(just count number of active joints)
Planar Examples:
8/11/2019 Advanced Robotics Dr Bob
22/166
22
Kutzbach's Criterion: Spatial robots
( ) 54321 1234516 JJJJJNM =
Where:M
is the mobilityNis the total # of links, including ground
J1is the number of one-degree-of-freedom joints
J2is the number of two-degree-of-freedom joints
J3is the number of three-degree-of-freedom joints
J4is the number of four-degree-of-freedom joints
J5is the number of five-degree-of-freedom joints
One-degree-of-freedom joints: Revolute, Prismatic, Screw
Two-degree-of-freedom joints: Cylindrical, Gear joint
Three-degree-of-freedom joints: SphericalFour-degree-of-freedom joints: Spherical in a slot
Five-degree-of-freedom joints: Spatial cam
General spatial n-link serial robot has n+1 links (including fixed ground link), connected
by nactive 1-dof joints:
( ) ( ) ( ) ( ) ( ) nnnnnM ==+= 56010203045116 dof
(just count number of active joints)
Spatial Examples:
8/11/2019 Advanced Robotics Dr Bob
23/166
23
Motion Description
We need to describe the spatial position & orientation (pose) of links, tools, end-
effectors, sensors, workpieces, etc.
We attach a separate, independent right-handed Cartesian coordinate frame to each
moving body of interest. These frames are fixed in the moving body (e.g. link) and are
moved relative to each other via active joints.
Drawing (moving frame {B} relative to reference fame {A}):
BAP and RAB are required to describe the position (translations) and orientation
(rotations) of {B} with respect to {A} together this is called the pose.
Position
{ }TrollpitchyawzyxX= Note: position is vector but rotation IS NOT A VECTOR!!
Velocity
{ }TzyxzyxX &&&& =
Acceleration
{ }TzyxzyxX &&&&&&&& =
Velocity and acceleration are vector representations, both position and orientation
The 3D representation of orientation (rotations) CANNOT BE EXPRESSED AS A
VECTOR!!!
8/11/2019 Advanced Robotics Dr Bob
24/166
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. 1221 PPPP +=+
OrientationFor 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 uniqueOrthonormal 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:
8/11/2019 Advanced Robotics Dr Bob
25/166
25
2. Form of Orthonormal Rotation Matrices:
RAB 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. BAX :
What vector operation is used for projecting one vector onto another?
=
ABABAB
ABABAB
ABABABAB
ZZZYZX
YZYYYX
XZXYXX
R
So the Orthonormal Rotation Matrix is also called the Direction Cosine Matrix. Note the
dot product is commutative, so another way to define it is row-wise:
=
=
AB
AB
AB
BABABA
BABABA
BABABAAB
Z
Y
X
ZZYZXZ
ZYYYXY
ZXYXXX
R
8/11/2019 Advanced Robotics Dr Bob
26/166
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
= RR
A
B
B
A how do we calculate this?
=
=
|||
|||
AB
AB
AB
BABABA
BABABA
BABABABA ZYX
ZZZYZX
YZYYYX
XZXYXX
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 propertyand only holds for this very special type of matrix!!
4. Simple example (for the following sketch, find RA
B ; also find1
= RR A
B
B
A ):
8/11/2019 Advanced Robotics Dr Bob
27/166
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, andZ.
We will now derive the rotation matrix representing the orientation of {B} w.r.t. {A}
when {B} is rotated about theZaxis of {A} by angle . Figure:
From the geometry of this situation, the formula is:
The student is left to derive in a similar manner ( )YR and ( )XR 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 RAB . There are twelve distinct ways to describe
3 general rotations about fixed axes and also twelve distinct ways to describe 3 general
rotations about moving axes (called Euler angles):
X-Y-XX-Y-Z
X-Z-X
X-Z-Y
Y-X-YY-X-Z
Y-Z-X
Y-Z-Y
Z-X-YZ-X-Z
Z-Y-X
Z-Y-Z
Note: X-Y-Y, etc., is not allowed since a repeated Yrotation is not independent.
8/11/2019 Advanced Robotics Dr Bob
28/166
28
7. Euler Angles. Since there are so many artificial paths to reach the same result, let us
choose one convention and stick with it always:
Z-Y-X() Euler angles (about moving axes) demonstrate using frames {B}and {A}:
Start with frames {B} and {A} aligned (what is RAB 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 BZ (which is
identical to AZ for this first rotation only).
b. Next rotate moving frame {B} by an angleabout the axis BY (which was
moved away from AY
in the a. rotation).
c. Last rotate moving frame {B} by an angle about the axis BX (which has
been twice rotated away from AX ).
Craig derives the overall orientation description for this Euler rotation convention:
( ) ( ) ( )
=
=
cs
sc
cs
sccssc
R
RRRR
AB
XYZAB
0
0001
0
0100
100
00
++
++
=
ccscs
cssscssscccs
cscssssccscc
RAB
Given , it is easy to find RAB - just substitute and evaluate.
8/11/2019 Advanced Robotics Dr Bob
29/166
29
8. The inverse problem is not so easy: Given a valid RAB , find .
Craig gives this solution for Z-Y-X () Euler angles in Chapter 2, in the X-Y-Z() Fixed angles section (read the book turns out these two conventions aremathematically identical in symbolic formula results). The inverse problem is solved by
forming three independent equations or combinations of equations from the symbolicexpression of RAB :
++
++
=
ccscs
cssscssscccs
cscssssccscc
rrr
rrr
rrr
333231
232221
131211
ijr are nine given, consistent (see constraint discussion below) numbers of RAB .
are the 3 unknowns.
Using the first column and the facts that 122 =+ sc and cos/sintan = :
+= 221
21131,2 rrratan
Using the 21 and 11 terms:
=
c
r
c
ratan 1121 ,2
Using the 32 and 33 terms:
=
c
r
c
ratan 3332 ,2
Note that dividing by c on the numerator & denominator of the last two solutions may
seem silly, but thesignof 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 RAB when substituted into the forward
rotation matrix expression.
8/11/2019 Advanced Robotics Dr Bob
30/166
30
9. Orthonormal Rotation Matrix Constraints
Remember spatial rotations have 3 dof (e.g. roll-pitch-yaw, or ), but theOrthonormal 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 Orthogonalconstraints: all three columns (and rows) are mutually
perpendicular to each other for all possible orientations. This makes sense sinceX-Y-Z
of frame {B} are permanently mutually perpendicular for all motion. We can use one
vector cross product or three vector dot products to express these three constraints:
There are three Normalized constraints: all three columns (and rows) are unit vectors.
We express these three constraints with three (scalar) vector length equations:
8/11/2019 Advanced Robotics Dr Bob
31/166
31
10. Position vector rotational transformations
We now introduce a very useful matrix-vector equation for transforming the
coordinates of a given position vector PB known in the {B} frame to the same vector
PA described in the {A} frame coordinates (changing the basis of the vector from {B} to
{A}).The XYZcomponents of a vector are the projections of that vector onto the XYZ
axes of the frame of interest. Let's project a vector Ponto the {A} frame using the dot
product.
PZp
PYp
PXp
AZA
AYA
AXA
=
=
=
Now, let's change the basis (Cartesian coordinate frame wherein the coordinates ofa vector are expressed) of vector Pfrom {B} to {A}. Right-hand side vectors of above
equations can be expressed in {B} coordinates:
PZp
PYp
PXp
BA
BZ
A
BA
BY
A
BA
BX
A
=
=
=
Now, AB
AB
AB ZYX ,, are the rows of RAB , so:
PRP BABA
=
Note the two vectors are identical in direction and length, just the basis for expression of
the XYZcomponents are different. So, we can use the Orthonormal Rotation Matrix to
rotate coordinates of a vector from one Cartesian coordinate frame to another.
Example:
8/11/2019 Advanced Robotics Dr Bob
32/166
32
11. Euler Angles Forward and Inverse Examples
We conclude this section on Orthonormal Rotation Matrices by presenting
examples for the forward and inverse problems for the Z-Y-X () Euler anglesconvention.
a.
Forward calculation: Given Z-Y-X Euler angles o50= , o40= , ando30= , find RAB .
Answer:
=
663.0383.0643.0
105.0803.0587.0
741.0457.0492.0
RAB
Be sure to checkthe six constraints to ensure this is a valid Orthonormal Rotation Matrix
result.
b. Inverse solution:
Given
=
663.0383.0643.0
105.0803.0587.0
741.0457.0492.0
RAB , solve for the Z-Y-X Euler angles
,, (both solution sets).
Answer:
Solution
1 o50 o40 o30
2 o230 o140 o210
Be sure to checkthe second solution set.
8/11/2019 Advanced Robotics Dr Bob
33/166
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 PRP BABA
= from earlier), plus a translation since
now BAP is considered.
General form of Homogenous Transformation Matrix (4 x 4):
Perspective, scaling when last row is not [ ]1000 - used in computer graphics.For robotics purposes, this last rowneverchanges - 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.
8/11/2019 Advanced Robotics Dr Bob
34/166
34
3 Interpretations for Homogeneous Transformation Matrices
Common Example for all 3 interpretations: Given:
1) Description of a frame
TAB 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}.
BAP 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 aretheXYZunit vectors of {B} projected onto theXYZ{A} unit directions.
[ ]
=
|||
|||
BA
BA
BAA
B ZYXR
Figure for first interpretation:
8/11/2019 Advanced Robotics Dr Bob
35/166
35
2) Transform Mapping:
Matrix TAB maps PP AB . 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: PB , find PA
Note: PRPP BABBAA +=
Figure for second interpretation:
8/11/2019 Advanced Robotics Dr Bob
36/166
36
3) Transform Operator:
Toperates on 1PA to yield 2P
A . Same Cartesian coordinate frame {A}, there is no
{B} for this interpretation. The original vector 1PA has been translated and rotated to
new vector2
PA . Order of translation and rotation doesn't matter if we assume rotations
always occur about the tail of vectors. Equation:
Given: 1PA , find 2P
A
Figure for third interpretation:
8/11/2019 Advanced Robotics Dr Bob
37/166
37
Invert Homogeneous Transformation Matrices
Given TAB , find the opposite Homogeneous Transformation Matrix, i.e. the pose
of Cartesian coordinate frame {A} w.r.t. {B}:1
= TT AB
BA
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.
[ ] [ ]== TT BAAB1
Beautiful Propertyfrom Orthonormal Rotation Matrices:
Translation part: Without regard for frame of expression:
With regard,
[ ] =1TAB
Example: Given: find 1= TT ABBA
Figure for example:
8/11/2019 Advanced Robotics Dr Bob
38/166
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 Ti
i
1
.
A vector Pi is mapped back to Pi 1
by Ti i1 :
For a general robot, there are many such frames.
Example: Frames {A}, {B}, {C}, {D} lie along a serial robot chain.
Given PD (e.g. hand location in local frame), find PA (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:
8/11/2019 Advanced Robotics Dr Bob
39/166
39
Transform Equations: Robotics Example
Frames: Figure:
{WO} World
{B} Base
{WR} Wrist
{H} Hand
{G} Goal
Given: Fixed matrices: TTT WOGWRH
WOB ,,
1) Given THG from machine vision algorithm. Calculate TB
WR (Can compare with TB
WR
calculated by forward pose kinematics and joint angle encoders.)
Transform loop closure equation:
TTTTT HGWRH
BWR
WOB
WOG =
TTTTT WRHB
WRHG
WOG
WOB =
11
111 = TTTTT WRH
HG
WOG
WOB
BWR
or,11
= TTTT WR
GWO
GWO
BB
WR
where ( ) 1111 == TTTTT WRHHGHGWRHWRG
2) Want {H}to be same as {G} - to grasp the goal object. Calculate TBWR to achieve that
pose.
ITHG =
11 = TTTT WRH
WOG
WOB
BWR
Use transform graphs to check results for both cases.
8/11/2019 Advanced Robotics Dr Bob
40/166
40
Denavit-Hartenberg (DH) Parameters
Standard description of the geometric configuration of joints and links in a serial
robot. Paul (1983) interpretation of DH Parameters different from Craig's (1989).
4 parameters to describe completely, in general, the pose relationship betweenconsecutive frames {i} w.r.t. {i-1}. Joints {i-1} and {i} connected by link {i-1}.
Cartesian coordinate frame {i-1} rigidly attached to link {i-1}; moves with joint i-1.
J. Denavit and R.S. Hartenberg, 1955, "A Kinematic Notation for Lower-Pair
Mechanisms Based on Matrices", Journal of Applied Mechanics, pp. 215 - 221.
4 parameters (two rotations, two translations). One control variable out of the 4. 4
transform operators to change from {i-1} to {i} (or, give pose of {i} w.r.t. {i-1}).
Frame Attachment Conventions:
1) Zaxis 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 definingX.
3) Yaxis formed by right hand rule (ZandXknown).
Showing onlyZandXis sufficient, drawing is made clearer byNOTshowing Y.
First Link:
Choose 0Z 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.
8/11/2019 Advanced Robotics Dr Bob
41/166
41
Drawing of General Link Connecting 2 Joints:
D-H Parameters:
1i : Angle between 1
iZ to iZ measured about 1
iX
1ia : Distance from 1 iZ to iZ measured along 1 iX
id : Distance from 1
iX to iX measured along iZ
i : Angle between 1
iX to iX measured about iZ
Screw Pairs:
1i , 1ia alignZaxes about and along 1
iX
id , i alignXaxes about and along iZ
General Result: Table of DH ParametersColumns are joint/link index iand the 4 DH parameters.
Rows are the 4 DH parameters for each active joint/link in the serial chain: total of
nactive joints/moving links.
i 1i 1ia id 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 . . .)
8/11/2019 Advanced Robotics Dr Bob
42/166
42
DH Parameters Examples
Complete Examples
1. 3-axis Planar 3R Articulated Robot
X
Y
Z
0
0
1
X1
X2
X3
XH
Z0
Z2
Z3
ZH
YH
L 2
L 3
L 1
1
2
3
i 1i 1ia id i
1 0 0 0 1
2 0 1L 0 2
3 0 2L 0 3
Issues:
Links in different planes, but shift allXaxes to align (di=0)
What happened toL3?
8/11/2019 Advanced Robotics Dr Bob
43/166
43
2. 3-axis Spatial PRP Cylindrical Robot
X0
Z0
1XZ Z1 2
L 2
X2
Z3
1L
X3
21Z Z
Top
Front
i 1i 1ia id i
1 0 0 1L 0
2 0 0 0 o90
3 o90 0 2L 0
8/11/2019 Advanced Robotics Dr Bob
44/166
44
3. 6-axis Spatial 6R Unimation PUMA Robot
i 1i 1ia id i
1 0 0 0 1 2 o90 0 0L o902
3 0 1L 0o
903+
4 o90 0 2L 4
5 o90 0 0 5
6 o90 0 0o906+
8/11/2019 Advanced Robotics Dr Bob
45/166
45
Issue: Joint angle offsets
The active joint angle variable for a revolute joint is defined as:
i : Angle between 1
iX to iX measured about iZ
The kinematic diagrams generally show the zero value definition for each joint angle
parameter. In the zero angle configuration, if the 1
iX and iX axes are NOT identical
when 0=i , then a (constant) joint angle offset is required to account for this. Examples
of this may be seen in the 2nd
row for the Cylindrical Robot and the 2nd
, 3rd
, and 6th
rows
for the PUMA Robot above.
8/11/2019 Advanced Robotics Dr Bob
46/166
46
Incomplete Examples
(only coordinate frames are shown - DH Parameters intentionally left blank)
4. 7-axis Spatial 7R Fanuc S10 Robot
i 1i 1ia id i
12
3
4
5
6
7
8/11/2019 Advanced Robotics Dr Bob
47/166
47
5. 8-axis Spatial 8R NASA AAI ARMII Robot
i 1i 1ia id i
1
2
3
4
5
6
7
8
8/11/2019 Advanced Robotics Dr Bob
48/166
48
Very Incomplete Examples for practice!
(not even coordinate frames are shown - DH Parameters intentionally left blank)
6. 7-axis Spatial 7R NASA Flight Telerobotic Servicer (FTS) Robot
i 1i 1ia id i
1
2
3
4
5
6
7
8/11/2019 Advanced Robotics Dr Bob
49/166
49
Forward Pose Kinematics
Given numerical values for all the joint variables, find the pose (position and
orientation) of the end-effector Cartesian coordinate frame (or other frame of interest)
with respect to the base Cartesian coordinate frame.
Given ; Find (4x4 Homogeneous Transformation Matrix)
Above assumes all revolute joints with given joint angles if there are prismatic joints, a
joint lengthvalue must be given for those joints.
Non-linear (transcendental) expressions, but solution is straight-forward all terms
(joint angles) inside sinesand cosinesare given. We will use concatenation of matrices
(consecutive link transformations) matrix multiplication to find result. Result can beevaluated numerically or symbolically. Preferred: symbolic expressions of the Forward
Pose Kinematics result that can be implemented numerically in Matlab.
Forward Pose Kinematics is useful for robot simulation and sensor-based control.
The problem is made easy by isolating the problem to the pose of one frame w.r.t. its
previous neighbor along the serial chain use one row of the DH parameters table to
determine this. Then simply repeat for all moving links/joints w.r.t their previous
neighbor and multiply the whole thing together.
Derivation of Consecutive Link Transformations:Define frame {i} with respect to frame {i-1}: Ti i
1 . Attach 3 intermediate frames
{P}, {Q}, and {R}. Figure:
From {i-1} to {i}: Rotate 1i from 1 iZ to PZ about 1 iX .Translate 1ia from PZ
to QZ along PX
.
Translate id from QX to RX
measured along QZ .
Rotate i from QX to RX
about RZ .
8/11/2019 Advanced Robotics Dr Bob
50/166
50
=
1000
0
1111
1111
1
1
iiiiiii
iiiiiii
iii
ii
cdcscss
sdscccsasc
T
Physical interpretation: ii i
iR P 1 1
,
(Craig Equation 3.6)
( ) ( )iiZiiXi
i dScrewaScrewT ,, 111
=
( ) ( ) ( ) ( ) ( )
===
1000
00
00
001
,11
11
1
111111ii
ii
i
iXiXiXiXiiXcs
sc
a
RaDaDRaScrew
( ) ( ) ( ) ( ) ( )
===
1000
100
00
00
,i
ii
ii
iZiZiZiZiiZd
cs
sc
RdDdDRdScrew
Caution:screws commute (order of translation and rotation doesn't matter), but matrices
don't in general. Also, cannot commute theXand Zscrews, they must appear as in the
equations above.
8/11/2019 Advanced Robotics Dr Bob
51/166
51
Forward Pose Kinematics Result:
Concatenate Consecutive Homogeneous Transformation Matrices:
TN0 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 allNjoint variables:
There are 2 different representations: NN XT
00
, where{ }TN zyxX =
0 , and we useZ-Y-X() Euler angles convention.
Sum of Angles Simplification
If any two (or more) consecutive Z axes are parallel (i.e. consecutive i rotate
about parallel axes), we can simplify the resulting symbolic Forward Pose Kinematics
expressions by using sum of angle formulas:
( )
( ) casbsacbba
sasbcacbba
=
=
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 NOTcommute 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.
8/11/2019 Advanced Robotics Dr Bob
52/166
52
Additional, fixed transforms
The above TN0 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 Kinematicsexpressions will be simplest if {N} is located at the last active wrist joint; if a tool,
gripper, or other end-effector is attached we need another frame of interest (say {H} for
hand) attached; {H} is rigidly connected to {N} (i.e. no more joints in between) but offset
some distance.
The overall Forward Pose Kinematics Homogeneous Transformation Matrix is
given in generic form below. Note that the fixed matrices TB0 and TNH 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:
8/11/2019 Advanced Robotics Dr Bob
53/166
53
Forward Pose Kinematics Examples
1. 3-axis Planar 3R Articulated Robot
Forward Pose Kinematics Symbolic Derivations
Given 321 ,, , calculate T03 and TH
0 . Plug each row of DH table into Eq. 3.6.
i 1i 1ia id i
1 0 0 0 1
2 0 1L 0 2
3 0 2L 0 3
=
1000
0100
00
00
11
11
0
1
cs
sc
T
=
1000
0100
00
0
22
122
1
2
cs
Lsc
T
=
1000
0100
00
0
33
233
2
3
cs
Lsc
T
( ) ( ) ( )
+
+
==
1000
0100
0
0
12211123123
12211123123
3232
121
01
03
sLsLcs
cLcLsc
TTTT (interpret geometrically)
Simplification of rotation part using sum of angles formulae (all 3 active Z axes arealways parallel): e.g. [1,1] term:
Original multiplication: ( ) ( ) 3212132121 scssccsscc +
First simplification: 312312 sscc
Second simplification: 123c
( )( )321123
321123
sin
cos
++=
++=
s
c
( )( )2112
2112
sin
cos
+=
+=
s
c
Trigonometric Sum-of-Angles Formulae:
( )
( ) casbsacbba
sasbcacbba
=
=
sin
cos m
8/11/2019 Advanced Robotics Dr Bob
54/166
54
What happened toL3?: Additional, fixed transform
[ ]
=
1000
0100
0010
001 3
3
L
TH (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. L3is a constant since the third link is rigid.
( ) ( )33
32103
0 ,, LTTT HH =
[ ]
+
+
=
1000
0100
0010
001
1000
0100
0
03
12211123123
12211123123
0
L
sLsLcs
cLcLsc
TH
[ ]
++
++
=
1000
0100
0
0
123312211123123
123312211123123
0 sLsLsLcs
cLcLcLsc
TH
Position vector significantly more complicated; orientation identical.
8/11/2019 Advanced Robotics Dr Bob
55/166
55
1. 3-axis Planar 3R Articulated RobotForward Pose Kinematics Example
1) Given 1,2,3 321 === LLL andooo 35,25,15 321 === , calculate TH
0 .
TTT HH 3030 =
=
1000
0100
062.20259.0966.0
430.40966.0259.0
03T ;
[ ]
=
1000
0
0
1
3 ITH
=
1000
0100
028.30259.0966.0
689.40966.0259.0
0TH
2) Given 1,2,3 321 === LLL andoo 0,90 321 === , calculate TH
0 .
=
1000
0100
5001
0010
03T sameTH =
3
=
1000
0100
6001
0010
0TH
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 RH0 in addition to the position
vector HP0 . Actually, since the robot is planar, you should check the first example too
using a sketch!
8/11/2019 Advanced Robotics Dr Bob
56/166
56
2. 3-axis Spatial PRP Cylindrical RobotForward Pose Kinematics Symbolic Derivations
Given 21 ,, LL , calculate T03 . Plug each row of DH table into Eq. 3.6.
i 1i 1ia id i
1 0 0 1L 02 0 0 0 o90 3 o90 0 2L 0
=
1000
100
0010
0001
1
01
LT
=
1000
0100
00
00
12
sc
cs
T
=
1000
0010
100
0001
223
LT
=
1000
010
0
0
1
2
2
03
L
sLsc
cLcs
T
(interpret geometrically)
In this case the point of interest is the origin of {3} so no {H} frame is required.
8/11/2019 Advanced Robotics Dr Bob
57/166
57
2. 3-axis Spatial PRP Cylindrical RobotForward Pose Kinematics Example
1) Given 2,30,3 21 === LL o , calculate T03 .
=
1000
3010
15.00866.0732.1866.005.0
03T
2) Given 2,90,3 21 === LL o
, calculate T0
3 .
=
1000
3010
2100
0001
03T
Check both results with sketches (top and front views) be sure to include the {3} and{0} axes to check the orientation R03 in addition to the position vector 3
0P .
8/11/2019 Advanced Robotics Dr Bob
58/166
58
3. 6-axis Spatial 6R Unimation PUMA RobotForward Pose Kinematics Symbolic Derivations
Given 654321 ,,,,, , calculate T06 and T
BH . Plug each row of DH table into
Eq. 3.6.
i 1i 1ia id i 1 0 0 0 1
2 o90 0 0L o902
3 0 1L 0o
903+
4 o90 0 2L 4
5 o90 0 0 5
6 o90 0 0o
906+
=
1000
0100
00
00
11
11
01
cs
sc
T
=
1000
00
100
00
22
0
22
12
sc
L
cs
T
=
1000
0100
00
0
33
133
23
sc
Lcs
T
=
100000
100
00
44
2
44
34
cs
L
sc
T
=
100000
0100
00
55
55
45
cs
sc
T
=
100000
0100
00
66
66
56
sc
cs
T
( ) ( ) ( ) ( ) ( ) ( )6565
454
343
232
121
01
06 TTTTTTT =
( ) ( )65436321
03
06 ,,,, TTT =
( ) ( ) ( )32131
01321
03 ,,, TTT =
(take advantage of parallelZaxes, 2 and 3 sum-of-angles formula)
( )[ ]
=
1000
0
100
0
1000
0100
00
00
,,212323
0
212323
11
11
32103
cLcs
L
sLsc
cs
sc
T
8/11/2019 Advanced Robotics Dr Bob
59/166
59
( )[ ]
+
+
=
1000
0,,
212323
211101231231
211101231231
32103
cLcs
ssLcLcsscs
scLsLssccc
T
( ) ( ) ( ) ( )6565
454
34654
36 ,, TTTT =
(no parallelZaxes 4, 5, and 6 NOsum-of-angles formula)
( )[ ]
=
1000
0
0
,,546546465464
256565
546546465464
65436
ssccsscscscc
Lccsss
sccccssscccs
T
It is left to the reader to perform the symbolic multiplication TTT 3603
06 = ; 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 T06 . For this robot we need to control the {H}
frame. There is also a separate base frame {B}different from {0}. The overall Forward PoseKinematics Homogeneous Transformation Matrix is given conceptually below; the symbolic terms
would be worse in complexity than T06 . The below overall transform can be evaluated numerically.
LBandLHare constants. The orientation of {B} is identical to {0} and the orientation of {H} is identicalto {6} for all motion.
( ) ( ) ( )HHBBB
H LTTLTT6
654321060 ,,,,, =
( )[ ]
=
1000
1000010
0001
0B
BB
LLT ( )[ ]
=
1000
1000010
0001
6
HHH
LLT
8/11/2019 Advanced Robotics Dr Bob
60/166
60
Inverse Pose Kinematics
Given numerical values for the pose (position and orientation) of the end-effector
Cartesian coordinate frame (or other frame of interest) with respect to the base Cartesian
coordinate frame, find the required joint variables to achieve this pose.
Given: Find:
Non-linear, coupled equations to solve in trancendentals of the unknowns; solution
is generally difficult. Multiple solutions exist. Analytical solutions do not exist for some
manipulator structures. Inverse Pose Kinematics is required for robot pose control. The
required equations to solve are from Forward Pose Kinematics.
Forward Position Kinematics of Serial RobotsJust 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 alimited 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 solutionsFor 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.
8/11/2019 Advanced Robotics Dr Bob
61/166
61
Existence of solutions
For m = nor 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 IFthe
commanded pose is within the limited task space spanned by the manipulator. For
example, a 5-DOF (n=5) robot cannot turn a ratchet in general, but is sufficient for axis-
symmetric applications (such as spray painting). Actually this is still the m = n case
because m is reduced to m=5for axis-symmetric Cartesian tasks not requiring the robot
roll freedom.
Solution Methods
Numerical e.g. Newton-Raphson. Requires a good initial guess and only findsone solution, closest to the initial guess.
Analytical(closed-form)
Algebraic vs. Geometric, combination of the two
Pose Kinematics of Parallel Robots
There is an interesting duality with serial robots: For parallel robots, the forward
kinematics solution is generally coupled and non-linear, while the inverse kinematics
solution is generally more straight-forward. This is reversed for serial robots. (See the
notes section on Parallel Manipulators.)
Tan-half angle substitution useful in solving inverse pose kinematics (stay tuned)
Pieper's solution if three consecutive axes are intersecting in a serial robot, the inverse
pose solution is guaranteed to exist analytically.
8/11/2019 Advanced Robotics Dr Bob
62/166
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 TN0 is known (commanded) and the joint variables
{ }TN L,,, 321 are unknown:
Spatial Serial Robots16 equations, 4 trivial (row 4) = 12 equations.
3 translational equations (column 4) all 3 are independent.
9 rotational equations - only 3 are independent.
Planar16 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.
Ifm=n(the number of Cartesian and joint freedoms match) we can solve the problem, in
principle. Maybe not in closed-form (analytical); maybe the solution has to be numerical.
8/11/2019 Advanced Robotics Dr Bob
63/166
63
Inverse Pose Kinematics Examples
1. 3-axis Planar 3R Articulated Robot
X
Y
Z
0
0
1
X1
X2
X3
XH
Z0
Z2
Z3
ZH
YH
L 2
L 3
L 1
1
2
3
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.
8/11/2019 Advanced Robotics Dr Bob
64/166
64
First, Simplification take advantage of fixedL3transform
Inverse Pose Equations to Solve
Subspace of general 3D pose space:
Note that the Forward Pose Kinematics result can be represented by T03 or by
{ }TyxX 33= . The given LHS matrix cannot be a general 3D pose since this is aplanar problem. The expression of the subspace of general 6-dof poses spanned by the
XYplane and described by T03 is given below:
Equations to Solve:
Write 2 independent translational equations:
Write 2 rotational equations, only 1 is independent:
Rotational equations:
8/11/2019 Advanced Robotics Dr Bob
65/166
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 21, (fully coupled in theseunknowns); the rotational equation involves all three unknowns. Therefore, let us solvefor 21, first from the translational equations and find 3 last.
Rearrange translational equations:
Square and add these rearranged translational equations to eliminate2
:
The result is one equation in one unknown 1 :
We can solve this equation for 1 by using the Tangent Half-Angle Substitution.
2tan 1
=t
2
2
11
1cos
t
t
+
=
21 1
2sin
t
t
+=
Substitute into theEFGequation:
01
2
1
122
2
=+
++
+
G
t
tF
t
tE ( ) 0121 22 =+++ tGtFtE
( ) ( ) ( ) 022 =+++ EGtFtEG quadratic formula:EG
GFEFt
+=
222
2,1
8/11/2019 Advanced Robotics Dr Bob
66/166
66
Solve for 1 by inverting the original Tangent Half-Angle definition:
Two 1 solutions result from the in the quadratic formula; both are correct
(multiple solutions elbow up and elbow down).
To find 2 , return to original arranged translational equations:
Find the unique 2 for each 1 value (use quadrant-specific atan2function):
Now we have two solutions for ( )21, ; 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. ( )1321
,, and ( )2321 ,, .
8/11/2019 Advanced Robotics Dr Bob
67/166
8/11/2019 Advanced Robotics Dr Bob
68/166
68
2) Given TH0 and 1,2,3 321 === LLL , calculate ( ) 2,1,,, 321 =ii .
=
10000100
6001
0010
0TH ;
=
10000100
5001
0010
03T
2) Answers(deg):
i 1 2 3
1 90 0 0
There is only one solution (why?).
3) Given TH0 and 1,2,3 321 === LLL , calculate ( ) 2,1,,, 321 =ii .
=
1000
0100
928.60866.05.0
0.405.0866.0
0TH ;
=
1000
0100
428.60866.05.0
134.305.0866.0
03T
3) Answers(deg):
Impossible! (why? complex result) Out of manipulator workspace.
8/11/2019 Advanced Robotics Dr Bob
69/166
69
Inverse Pose Kinematics Examples (cont.)
2. 3-axis Spatial PRP Cylindrical Robot
X0
Z0
1XZ Z1 2
L 2
X2
Z3
1L
X3
21Z Z
Top
Front
Inverse Pose Kinematics Symbolic Solution
Given T03 , calculate 21 ,, LL .
Forward Kinematics Solution:
=
1000010
0
0
1
2
2
03
L
sLsc
cLcs
T
Inverse pose equations come from here forward pose expressions:
=
1000
010
0
0
1000
1
2
2
333231
232221
131211
L
sLsc
cLcs
prrr
prrr
prrr
z
y
x
8/11/2019 Advanced Robotics Dr Bob
70/166
70
This robot has spatial motion, but rotation is limited to planar. Subspace of general 6-dof
pose (where the pose is represented by T03 or by { }T
zyxX 333= :
=
1000
010
0
0
1000
010
0
0
1
2
2
3
3
3
L
sLsc
cLcs
z
ysc
xcs
We have a problem there are only 3 joints (n=3) but there are m=4Cartesian values.
This is an overconstrained problem and no solution exists in general a dependency
exists among { }TzyxX 333= and all four cannot be commanded independently.
Therefore, let us command only 3 Cartesian values { }Tred zyxPX 33330
== ; we will
treat this robot as a translational freedom robot. is not independent but is related to x3andy3. The three equations to solve for the three unknowns are then taken only from the
translational equations:
13
23
23
Lz
sLy
cLx
=
=
=
Inverse Pose Kinematics Solution:
1) 31 zL =
2)3
3
2
2 tancos
sin
x
y
L
L==
( )33 ,xyatan2=
3) 2323
222
222 sincos yxLL +=+
23
232 yxL +=
Mathematically, there are two solution sets; the 2L solution is not a practical choice.(If 2L is allowed, then + is the angle solution).
This is not a general spatial manipulator, i.e. 30P and R03 cannot be specified
independently.
8/11/2019 Advanced Robotics Dr Bob
71/166
71
Inverse Pose Kinematics Examples (cont.)
2. 3-axis Spatial PRP Cylindrical Robot
Inverse Pose Kinematics Examples
1) Given 30
P , calculate 21 ,, LL .
=
=
3
1
732.1
3
3
3
30
z
y
x
P
1) Answers(mand deg):
i 1L 2L
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 30P back). Also, for both cases
you can calculate R03 - they will be different (why?).
2) Given 30P , calculate 21 ,, LL .
=
=
3
2
0
3
3
3
30
z
y
x
P
2) Answers(mand deg):
i 1L 2L
1 3 -90 2
2 3 90 -2
8/11/2019 Advanced Robotics Dr Bob
72/166
72
Decoupled Inverse Pose Kinematics Solution - Pieper's Solution
Pieper proved that if a 6-dof robot has any three consecutive joint axes
intersecting, there exists a closed-form (analytical) solution to the inverse position
kinematics. The majority of industrial robots are in this category.
In particular, many robots have a spherical wrist - three wrist actuators rotate about
a common point. In this case, the position and orientation sub-problems may be
decoupled. Solve the position first (1st 3 joints) and then solve the orientation second
(2nd 3 joints, the wrist) based on the orientation of the first 3 joints. Coordinate frame of
resolution must be located at the wrist point - this is general, because can make
transformation from any other frame rigidly connected to the last joint.
PUMA Example (no details):
Given T06 , calculate 654321 ,,,,, .
( )65432106 ,,,,, fT=
( ) ( )
=
1000
,,,,,,, 32160
654321060
6
PRT
Joints 4, 5, and 6 cannot affect translation of wrist origin.
1) Translational equations: Given 60P , calculate 321 ,, values (4 sets).
3 independent equations, 3 unknowns.
2) Rotational equations: Given R0
6 , and now knowing 321 ,, , calculate654 ,, values (2 sets).
9 dependent equations, 3 unknowns.
( ) ( ) ( )65432106321
03654
36 ,,,,,,,,, RRR
T=
8/11/2019 Advanced Robotics Dr Bob
73/166
73
4 sets of 321 ,, ; 2 sets of 654 ,, for each. Therefore, there are 8 overall
solutions to the inverse position problem for the PUMA. Some may lie outside joint
ranges. Generally choose closest solution to previous position.
Peel-off homogeneous transformations matrices with unknowns to separate
variables.
( ) ( ) ( ) ( ) ( ) ( )6565
454
343
232
121
01
06 TTTTTTT= ; LHS is numbers.
( )[ ] ( ) ( ) ( ) ( ) ( )656545434323212061
101 TTTTTTT =
; can get 1 and 3.
( )[ ] ( ) ( ) ( )65
65
4
54
3
4
0
6
1
2
0
3 TTTTT =
; get 2, with 1 and 3 known.
( )[ ] ( ) ( )656545061
404 TTTT =
; isolate and solve 4, 5, and 6.
8/11/2019 Advanced Robotics Dr Bob
74/166
74
Inverse Pose Kinematics Examples (cont.)
3. 8-axis Spatial 8R NASA AAI ARMII Robot
Inverse Pose Kinematics General Statement
Given: , calculate:
Since m=6 (Cartesian dof) and n=8 (joint dof) we have the kinematically-redundant
underconstrained case. There are infinite solutions (multiple as well!). There are some
great ways to make use of this redundancy for performance optimization in addition to
following commanded Cartesian translational and rotational velocity trajectories. For
inverse pose purposes we will here simplify instead and lock out the redundancy so that
8/11/2019 Advanced Robotics Dr Bob
75/166
75
m=n=6; let us choose 053 == for all motion to accomplish this. Then we have a
determined Inverse Pose Kinematics problem, still with multiple solutions (but finite).
The Forward Pose Kinematics relationship is:
So, the first step should be to simplify the equations as much as possible by calculating
the required T08 to achieve the commanded TBH :
The problem can be decoupled between the arm joints 1-4 and the wrist joints 5-8 since
the ARMII has a spherical wrist (all 4 wrist joint Cartesian coordinate frames share the
same origin). See the previous section that explained the Pieper results for the 6-axis
PUMA robot.
Now, we will further simplify by ignoring the wrist joints 6-8 (5 is already locked to zero) and solve theInverse Pose Kinematics problem only for the arm joints 1,2, and 4. The full inverse solution is given
in:
R.L. Williams II, "Kinematic Equations for Control of the Redundant Eight-
Degree-of-Freedom Advanced Research Manipulator II", NASA Technical
Memorandum 4377, NASA Langley Research Center, Hampton, VA, July 1992.
Inverse Pose Kinematics Symbolic Solution for Arm Joints Only, with 03=
Reduced problem statement:
Given { }TB zyxP 5555 = , calculate ( )i421 ,, , for all possible
solution sets i.
That is, with only three active joints, we can only specify three Cartesian objectives, inthis case the XYZ location of the origin of {5} with respect to origin of {B} (and
expressed in the basis of {B}). Note that 85 PP BB = due to the spherical wrist.
8/11/2019 Advanced Robotics Dr Bob
76/166
8/11/2019 Advanced Robotics Dr Bob
77/166
77
The result is one equation in one unknown 2 :
0sincos 22 =++ GFE
where:
25
23
22
3
3
2
2
ddZYG
YdF
ZdE
+=
=
=
We can solve this equation for 2 by using the Tangent Half-Angle Substitution. We
presented this back in the Inverse Pose Solution of the planar 3R robot; we solve for q2
(in that section, it was for q1).
Solve for 2 by inverting the original Tangent Half-Angle definition:
Two 2 solutions result from the in the quadratic formula; both are correct
(multiple solutions elbow up and elbow down).
To find 4 , return to original (arranged) translational equations:
23245
23245
cdZcd
sdYsd
=
=
Find the unique 4 for each 2 value (use quadrant-specific atan2function):
8/11/2019 Advanced Robotics Dr Bob
78/166
78
Solutions Summary
The solution is now complete for the ARMII robot reduced inverse pose problem
(translational joints only, plus 03= ).
There are multiple solutions: two values for 1 ; for each 1 , there are two valuesfor 2 ; for each valid ( )21, , there is a unique 4 . So there are a total of 4 ( )421 ,, 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 12 0 4
2 1 22 0 4
3 +1 22 0 4
4 +1 12 0 4
Dont take my word for it in all numerical examples, you can check the results;
plug all solution sets ( )421 ,, one at a time into the Forward Pose expressions and
verify that all sets yield the same, commanded 5PB . You can also calculate the
associated RB4 - all should be different (why?).
8/11/2019 Advanced Robotics Dr Bob
79/166
79
Inverse Pose Kinematics Examples (cont.)
3. 8-axis Spatial 8R NASA AAI ARMII Robot
Inverse Pose Kinematics Example
Given 5PB , calculate ( ) 4,3,2,1,,, 421 =ii .
=
=
6952.1
1159.0
6572.0
5
5
5
5
z
y
x
PB
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 5PB .
8/11/2019 Advanced Robotics Dr Bob
80/166
80
Velocity Analysis and Jacobian Matricess
Up to now, our robotics mathematics has been just in the pose domain. Velocity is
the first time derivative of position (translational and rotational velocity are both 3x1
vectors in the 3D world). Velocity requires linearized analysis it is more straight-
forward than pose solutions.
Velocity is useful for many topics in robotics:
Resolved Rate Control
Velocity of any point on manipulator
Moving objects in workspace
Dynamics
Pose
Translational: Rotational, e.g. 3-2-1 Euler angles ,,
Position is a vector Orientation is not a vector!
VelocityVector time rate of change of position and orientation.
Translational Orientation, e.g. 3-2-1 Euler &&& ,, : nonholonomic
Translational Velocity
Three coordinate frames involved (2 points and a frame): ( )jik
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:
ji P is the position vector from the origin of {i} to the origin of {j}, expressed in
{i} coordinates. So, this is more properly { }jik P , where {k} is implicitly assumed to be{i}, unless otherwise stated.
8/11/2019 Advanced Robotics Dr Bob
81/166
81
Translational velocity example
To demonstrate the various indices in ( )jik
V . Ais fixed reference frame.
Given: Absolute velocity ofB o90@2
s
in Absolute velocity of C o45@4
s
in
Figure:
From the given information:
( )
=
0
2
0
BAA V ; ( )
=
0
83.2
83.2
CAA V
From relative velocity equation:
( ) ( ) ( )CBA
BAA
CAA VVV += ( ) ( ) ( )
==
0
83.0
83.2
BAA
CAA
CBA VVV ; [email protected]
s
in
Now, these same velocity vectors ( CB
CA
BA VVV ,, )can be expressed in {B} and
{C} by: (velocity is a free vector, just coordinate rotation, no translation)
( ) ( )jiAk
Ajik
VRV = (or just look @ components)
( )
=00
2
B
AB
V ( )
=0
83.2
83.2
C
AB
V ( )
=0
83.2
83.0
C
BB
V
( )
=
0
41.1
41.1
BAC V ( )
=
0
0
4
CAC V ( )
=
0
43.1
58.2
CBC V
8/11/2019 Advanced Robotics Dr Bob
82/166
82
There are 27 permutations for A, B, C. So far, we have given 9 combinations. 2
more types:
1) ( ) ( )jik
ijk
VV = e.g. ( )
=
0
2
0
ABA V (9 of these)
2) ( ) { }0=iik
V e.g. ( )
=
0
0
0
BBA V (9 of these)
Transport Theorem:
Pdt
dP
dt
dPB
ABA
+=
( )Pdt
dP
dt
dPB
AkB
kA
k
+
=
General three-part velocity equation:
PTP
PRPP
BABA
BABB
AA
=
+=
PRPRPV BABBA
BBAA &&& ++=
PVVV P ++= 0
Adding descriptive indices:
( ) ( ) ( ) ( ) PRVRVV BABBAA
PBA
BBAA
PAA
++=
A rotating rigid body has different translational velocities at different points; however, it
has the same rotational velocity over the whole rigid body.
8/11/2019 Advanced Robotics Dr Bob
83/166
83
Rotational Velocity
Angular Velocity Vector:
( )BAA
is the angular velocity of Cartesian coordinate frame {B} with respect to
{A}, expressed in {A} coordinates. ( )BAA
is a unique vector description.
Nonholonomic: cannot integrate ( )BAA
to get an orientation vector; in fact,
there is no such thing as an orientation vector.
{ } { }TzyxT
zyx &&&= , but there is a relationship.
Kinematic Differential Equations in terms of Orientation Angles
e.g.Z-Y-X ( ,, ) Euler angle convention:
++
++
=
ccscs
cssscssscccs
cscssssccscc
RAB
From Spacecraft Dynamicsby Kane, Likins, and Levinson:
( )
=
=
&
&
&
0
010
scc
cscs
z
y
x
BAA
where { }T &&& are the Euler angle rates.Somewhat artificial, 23 other descriptions (Euler and Fixed). Inverse relationship:
=
z
y
x
tctssc
c
c
c
s
10
0
&&
&
Artificial singularities: o90= .
A rotating rigid body has the same rotational velocity at different points.
8/11/2019 Advanced Robotics Dr Bob
84/166
84
Angular velocity vector equation for multiple frames:
CB
BA
CA +=
Put in same frame:
( ) ( ) ( ) ( ) ( )CBABBAA
CBA
BAA
CAA R +=+=
or, if it is more convenient to express in the local frames (as in robot joints):
( ) ( ) ( )CBCA
CBABA
BCAA
RR +=
Combined Translational and Rotational Velocity
where: both (3x1) vectors
8/11/2019 Advanced Robotics Dr Bob
85/166
85
Jacobian Matrices
Jacobian matrix is a linear transformation mapping joint to Cartesian velocities:
m= dimension of Cartesian (task) space
n= dimension of joint space
Express resulting Cartesian velocities in any frame {k}; & are relative joint angle rates
and hence are expressed about different local Z axes. Velocity relationship:
Jacobian matrix is a multi-dimensional form of the derivative:
=Xk & =&
3 ways to calculate Jacobian matrix:
1) Partial derivatives definition.
f- vector of mCartesian 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!
8/11/2019 Advanced Robotics Dr Bob
86/166
86
2) Column as velocity due to joint i.Physical interpretation of Jacobian matrix: each column i is the Cartesian
velocity vector of the end-effector frame w.r.t. the base frame, due to joint ionly, and
with i& factored out.
( ) ( ) ( )
=
|||
|||0
20
10
L
&L&&
L
NNNN
k
k XXXJ
( ) ( )( )
=
iN
kiN
k
iN
k VX
0
00 &
A) Revolute Joint Columns
where iik
iik ZRZ = is the 3
rdcolumn of Rki
and ( ) ( )NiikiNik PRP = where ( )Nii P is the translational part of TiN
Column iof the Jacobian matrix, for a revolute joint:
Jacobian matrix, for all-revolute manipulator:
( ) ( ) ( )
=
Nk
NNk
Nk
ik
Nik
ik
kN
kkk
k
Z
PZ
Z
PZ
Z
PZJ
1
11 LL
8/11/2019 Advanced Robotics Dr Bob
87/166
87
B) Prismatic Joint Columns
where iik
iik ZRZ = is the 3
rdcolumn of Rki .
Column i, for prismatic joint:
3) Velocity recursion a'la Craig.
Add translational and rotational velocities serially from base to end-effector link.
Revolute:
( )11111
11
11
1
++
++
++
++
++
+=
+=
ii
ii
iii
iii
ii
iiii
iii
PvRv
ZR
&
Prismatic:
( ) 11111111
11
+
+++
++
+
++
+
++=
=
ii
iii
ii
iii
iii
iii
iii
ZdPvRv
R
&
Start with 000
00
== v (if base is fixed).
Use recursion equations, 1,,2,1,0 = Ni L .
Then, factor out { }&
from N
N
N
N
v , to get J
N
.
8/11/2019 Advanced Robotics Dr Bob
88/166
8/11/2019 Advanced Robotics Dr Bob
89/166
89
Derive J0 3 ways:
1) Partial derivatives definition J=
( ) =
==
y
x
p
p
Pf 300
jj j
ij
j j
ii p
dt
dp