Advanced Robotics Dr Bob

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


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


3/166

3

Introduction to Robotics

For a snazzier Intro to Robotics, please see:

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


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.


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.


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.


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/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?!?


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.


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


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

&

&&


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


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


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)


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


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


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


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.


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


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');


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:


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:


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!!!


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:


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


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


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.


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.


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.


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:


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:


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.


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.


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:


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:


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:


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:


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:


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.


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.


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


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?


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


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+


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.


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


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


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


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 .


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.


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.


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:


53/166

53

Forward Pose Kinematics Examples


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


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.


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!


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.


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 .


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


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


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.


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.


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.


63/166

63

Inverse Pose Kinematics Examples


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.


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:


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


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


67/166


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.


69/166

69

Inverse Pose Kinematics Examples (cont.)


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


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.


71/166

71



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


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=


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.


74/166

74



Inverse Pose Kinematics General Statement

Given: , calculate:

Since m=6 (Cartesian dof) and n=8 (joint dof) we have the kinematically-redundant

underconstrained case. There are infinite solutions (multiple as well!). There are some

great ways to make use of this redundancy for performance optimization in addition to

following commanded Cartesian translational and rotational velocity trajectories. For

inverse pose purposes we will here simplify instead and lock out the redundancy so that


75/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.


76/166


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


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


79/166

79



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 .


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.


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


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.


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.


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


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!


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


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

.


88/166


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

Documents

Advanced Robotics Dr Bob