Mechanics Control of Robotic Manipulators

8/17/2019 Mechanics Control of Robotic Manipulators

1/103

1

EE/ME 429/529

MECHANICS & CONTROL OF

ROBOTIC MANIPULATORS

CLASS NOTES

DR. BOB

Electrical/Mechanical Engineering

Ohio University

© Dr. Bob Productions


2/103

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

INVERSE POSE KINEMATICS.................................................................................56

TRAJECTORY GENERATION .................................................................................68

VELOCITY ANALYSIS AND JACOBIAN MATRICES..........................................78

ROBOT CONTROL ARCHITECTURES..................................................................96


3/103

3

Introduction to Robotics

For a snazzier Intro to Robotics, please see:

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


4/103

4

Some Definitions:

1) Robot – An electromechanical machine with more than one degrees-of-freedom

(DOF) which is programmable to perform a variety of tasks. From R.U.R. by the

Czechoslovakian playwright Karl Kapek; robot - worker or serf.

2) Anthropomorphic: Similar to Humans.

3) Manipulator - mechanical arm, with several DOF.

4) Degrees-of-Freedom - the number of independently controllable motions in a

mechanical device. The number of motors in a serial manipulator.

5) Mechanism - a 1-DOF machine element.

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

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

8) Robot system - manipulator(s), sensors, actuators, communication, computers,

interface, hand controllers to accomplish a programmable task.

9) Actuator - motor that drives a joint; generally rotary (revolute) or linear (prismatic);

electric, hydraulic, pneumatic, piezoelectric.

10) Cartesian Coordinate frame - dextral, orthogonal, XYZ , k ji ˆ,ˆ,ˆ .

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/103

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 robot’s 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/103

6

Robot Applications

Traditionally, robots are applied anywhere one or more of the 3Ds exist: in any job which

is too: Dirty, Dangerous, and/or Dull for a human to perform.

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/103

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/103

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 University’s 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/103

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/103

10

Craig Notation

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

2) Leading sub- and superscripts indicate which coordinate system the quantity is

expressed in; e.g.:

P A

is a position vector P expressed in Cartesian coordinate frame { A}.

B A

P is a position vector from the origin of frame { A} to the origin of frame { B}

expressed in the basis of frame { A}.

R A B is an orthonormal rotation matrix giving the orientation of frame { B} relative to

frame { A}.

T A B is a homogeneous transformation matrix giving the pose (position and orientation)

of frame { B} relative to frame { A}.

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

R .

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/103

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

122 ii sc

Law of Cosines: c AB B AC 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

t xdt

sd

dt

dva

t xdt

dsv

t xs

)(

)(

)(

t xavs

t xav

t xa


12/103

12

12

Matrix Introduction

Matrix: m x n array of numbers, where m is the number of rows and n in the number of

columns.

mnmm

n

n

aaa

aaa

aaa

A

21

22221

11211

Used to simplify and standardize solution of n linear equations in n unknowns (where

m=n). Used in velocity, acceleration, and dynamics analysis linear equations (not

position! - non-linear).

Special Matrices

Square (m=n=3)

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

A T

Symmetric

332313

232212

131211

aaa

aaaaaa

A A T

Column Vector (3x1 matrix)

3

2

1

x

x

x

X

Row Vector (1x3 matrix) 321

x x x X T

Matrix Addition Just add up like terms

hd gc

f bea

hg

f e

d c

ba


13/103

13

13

Matrix Multiplication with Scalar Just multiply each term

kd kc

kbka

d c

bak

Matrix Multiplication A B B AC

Row, Column indices have to line up as follows:

pxnmxpmxn

B AC

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

fiehdg

cibhag

i

h

g

f ed

cbaC

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

Matrix Multiplication Examples

654

321 A

67

89

87

B

108115

4246

364032424528

1816821187

67

89

87

654

321 B AC

233222 x x x


14/103

14

14

574431

755841

695439

36213014247

48274018329

48214014327

654

321

67

89

87

A B D

322333 x x x

Matrix Inversion Matrix “division” B AC solve for [ B]

C A B B B I B A AC A B AC 111

Matrix [ A] must be square.

I A A A A

11 (identity matrix, the matrix “1”).

A

A Adjo A

int1

A Determinant of [ A]

T ACofactor A Adjo int

Cofactor of ij ji

ijij M a

1

Minor ij M of ija is determinant of submatrix with row i and column j removed.

System of Linear Equations n equations in n unknowns.

For n=3:

3333232131

2323222121

1313212111

b xa xa xa

b xa xa xa

b xa xa xa

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

b x A where:

333231

232221

131211

aaa

aaa

aaa

A (known coefficients)


15/103

15

15

3

2

1

x

x

x

x (unknowns to be solved)

3

2

1

b

b

b

b (known right-hand sides)

Unique solution

b A x

1

only if [ A] has full rank. If not, 0

A and the inverse ofmatrix [ A] is undefined (dividing by zero).

Matrix Example

Solution of simultaneous linear equations.

1446

52

y x

y x

14

5

46

21

2

1

x

x

46

21 A

2

1

x

x x

14

5b

b A x1

86241 A Determinant non-zero; unique solution!

8 / 14 / 3

4 / 12 / 1

16

2411

A A

check:

10

012

11 I A A A A

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/103

16

16

Vector and Matrix Matlab Examples

P1 = [1;2;0]; % Define two vectors

P2 = [3;2;0];

sum1 = P1+P2; % Vector addition

sum2 = P2+P1;dot1 = dot(P1,P2); % Vector dot product

dot2 = dot(P2,P1);

cross1 = cross(P1,P2); % Vector cross product

cross2 = cross(P2,P1);

A = [1 2;6 4]; % Define given matrix and vector

b = [5;14];

dA = det(A); % Calculate the determinant of A

invA = inv(A); % Calculate the inverse of A x = invA*b; % Solve linear equations

x1 = x(1); % Extract answers

x2 = x(2);

AT = A’; % Matrix transpose


17/103

17

Matlab Introduction

MAT rix LABoratory

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 name name, without the .m. Matlab language is interpretive and executes line-by-line. Use the ; at the end of

statements to suppress intermediate results. If you use this suppression, the variable

name still holds the resulting value(s) - just type the variable name at the prompt after the

program runs to see the value(s). If there is a syntax or programming logic error, it will

give a message at the bad line and then quit. Type:

>>who


18/103

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/103

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 crated variables (who), with dimensions (whos)

%

who

whos


20/103

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/103

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 J J N M

Where:

M is the mobility

N is the total # of links, including ground J 1 is the number of one-degree-of-freedom joints

J 2 is the number of two-degree-of-freedom joints

One-degree-of-freedom joints: Revolute, Prismatic

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

General planar n-link serial robot has n+1 links (including fixed ground link), connected

by n active 1-dof joints:

nnnnn M 23012113 dof

(just count number of active joints)

Planar Examples:


22/103

22

Kutzbach's Criterion: Spatial robots

54321 1234516 J J J J J N M

Where:

M is the mobility

N is the total # of links, including ground

J 1 is the number of one-degree-of-freedom joints

J 2 is the number of two-degree-of-freedom joints

J 3 is the number of three-degree-of-freedom joints

J 4 is the number of four-degree-of-freedom joints

J 5 is the number of five-degree-of-freedom joints

One-degree-of-freedom joints: Revolute, Prismatic, Screw

Two-degree-of-freedom joints: Cylindrical, Gear joint

Three-degree-of-freedom joints: 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 n active 1-dof joints:

nnnnn M 56010203045116 dof

(just count number of active joints)

Spatial Examples:


23/103

23

Motion Description

We need to describe the spatial position & orientation (pose) of links, tools, end-

effectors, sensors, workpieces, etc.

We attach a separate, independent right-handed Cartesian coordinate frame to each

moving body of interest. These frames are fixed in the moving body (e.g. link) and are

moved relative to each other via active joints.

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

B A

P and R A B are required to describe the position (translations) and orientation

(rotations) of { B} with respect to { A} – together this is called the pose.

Position

T roll pitch yaw z y x X

Note: position is vector but rotation IS NOT A VECTOR!!

Velocity

T z y x z y x X

Acceleration

T z y x z y x X

Velocity and acceleration are vector representations, both position and orientation

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

VECTOR!!!


24/103

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/103

25

2. Form of Orthonormal Rotation Matrices:

R A B is a 3x3 matrix describing the orientation of frame { B} with respect to frame

{ A}. Remember 3D orientations have only 3 dof (e.g. roll, pitch, yaw); however, the

rotation matrix has 9 elements – why? (Let’s answer this later.)

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

X ˆ :

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

A B A B A B

A B A B A B

A B A B A B A B

Z Z Z Y Z X

Y Z Y Y Y X

X Z X Y X X

R

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

So the Orthonormal Rotation Matrix is also called the Direction Cosine Matrix. Note the

dot product is commutative, so another way to define it is row-wise:

A B

A B

A B

B A B A B A

B A B A B A

B A B A B A A B

Z

Y

X

Z Z Y Z X Z

Z Y Y Y X Y

Z X Y X X X

R

ˆ

ˆ

ˆ

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ


26/103

26

3. Inverse Orthonormal Rotation Matrix

The inverse matrix is simply shifting our reference – let us describe the orientation of { A}

w.r.t. { B}:

1

R R

A

B

B

A how do we calculate this?

|||

ˆˆˆ

|||

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

A B

A B

A B

B A B A B A

B A B A B A

B A B A B A B A Z Y X

Z Z Z Y Z X

Y Z Y Y Y X

X Z X Y X X

R

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

So, to find the inverse of an Orthonormal Rotation Matrix, we need only take the

transpose, which is cheap computationally and never subject to singularities!! This is a

beautiful property and only holds for this very special type of matrix!!

4. Simple example (for the following sketch, find R A

B ; also find1

R R A

B

B

A ):


27/103

27

5. Single axis, single angle rotations

To describe general 3D orientations we will use a series of three single axis, single

angle rotations. Therefore, we need to be able to derive and understand the rotation

matrix representing a single angle rotation about a single primary axis: X , Y , and Z .

We will now derive the rotation matrix representing the orientation of { B} w.r.t. { A}

when { B} is rotated about the Z axis of { A} by angle . Figure:

From the geometry of this situation, the formula is:

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

required.

6. Description of general (compound) spatial orientation

Remember description of orientation is rather artificial; many paths will lead to the

same numerical description for a general R A B . There are twelve distinct ways to describe

3 general rotations about fixed axes and also twelve distinct ways to describe 3 general

rotations about moving axes (called Euler angles):

X-Y-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 Y rotation is not independent.


28/103

28

7. Euler Angles. Since there are so many artificial paths to reach the same result, let us

choose one convention and stick with it always:

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

and { A}:

Start with frames { B} and { A} aligned (what is R A B for that special

configuration?). Assume { B} is the moving frame and { A} is the reference frame.

a. First rotate moving frame { B} by an angle about the axis B Z ˆ (which is

identical to A Z ˆ for this first rotation only).

b. Next rotate moving frame { B} by an angle about the axis BY ̂ (which was

moved away from AY ˆ

in the a. rotation).

c. Last rotate moving frame { B} by an angle about the axis B X ˆ (which has

been twice rotated away from A X ˆ ).

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

cs

sc

cs

sccssc

R

R R R R

A B

X Y Z A B

0

0001

0

0100

100

00

ccscs

cssscssscccs

cscssssccscc

R A B

Given

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


29/103

29

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

Craig gives this solution for Z-Y-X ( ) Euler angles in Chapter 2, in the X-Y-Z

( ) Fixed angles section (read the book – turns out these two conventions are

mathematically identical in symbolic formula results). The inverse problem is solved by

forming three independent equations or combinations of equations from the symbolicexpression of R

A B :

ccscs

cssscssscccs

cscssssccscc

r r r

r r r

r r r

333231

232221

131211

ijr are nine given, consistent (see constraint discussion below) numbers of R A B .

are the 3 unknowns.

Using the first column and the facts that 122

sc and cos / sintan :

221

21131,2 r r r atan

Using the 21 and 11 terms:

c

r

c

r atan

1121 ,2

Using the 32 and 33 terms:

c

r

c

r atan

3332 ,2

Note that dividing by c on the numerator & denominator of the last two solutions may

seem silly, but the sign of c makes a difference. Read Craig for singular solution when

0 c There are two overall inverse solutions, making use of the in the solution.

Both

solutions must yield back the original R A B when substituted into the forward

rotation matrix expression.


30/103

30

9. Orthonormal Rotation Matrix Constraints

Remember spatial rotations have 3 dof (e.g. roll-pitch-yaw, or

), but the

Orthonormal Rotation Matrix has 9 numbers. Therefore, there must be 6 (scalar)

constraints on these nine numbers. These six constraints come from the name

Orthonormal.

There are three Orthogonal constraints: all three columns (and rows) are mutually

perpendicular to each other for all possible orientations. This makes sense since X-Y-Z

of frame { B} are permanently mutually perpendicular for all motion. We can use one

vector cross product or three vector dot products to express these three constraints:

There are three Normalized constraints: all three columns (and rows) are unit vectors.

We express these three constraints with three (scalar) vector length equations:


31/103

31

10. Position vector rotational transformations

We now introduce a very useful matrix-vector equation for transforming the

coordinates of a given position vector P B known in the { B} frame to the same vector

P A described in the { A} frame coordinates (changing the basis of the vector from { B} to

{ A}).The XYZ components of a vector are the projections of that vector onto the XYZ

axes of the frame of interest. Let's project a vector P onto the { A} frame using the dot

product.

P Z p

PY p

P X p

A Z A

AY A

A X A

ˆ

ˆ

ˆ

Now, let's change the basis (Cartesian coordinate frame wherein the coordinates ofa vector are expressed) of vector P from { B} to { A}. Right-hand side vectors of above

equations can be expressed in {B} coordinates:

P Z p

PY p

P X p

B A

B Z

A

B A

BY

A

B A

B X

A

ˆ

ˆ

ˆ

Now, A B

A B

A B

Z Y X ˆ,ˆ,ˆ are the rows of R A B , so:

P RP B A

B A

Note the two vectors are identical in direction and length, just the basis for expression of

the XYZ components are different. So, we can use the Orthonormal Rotation Matrix to

rotate coordinates of a vector from one Cartesian coordinate frame to another.

Example:


32/103

32

11. Euler Angles Forward and Inverse Examples

We conclude this section on Orthonormal Rotation Matrices by presenting

examples for the forward and inverse problems for the Z-Y-X ( ) Euler angles

convention.

a. Forward calculation: Given Z-Y-X Euler angles 50 ,40

, and

30

, find R A B .

Answer:

663.0383.0643.0

105.0803.0587.0

741.0457.0492.0

R A B

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

result.

b. Inverse solution:

Given

663.0383.0643.0

105.0803.0587.0

741.0457.0492.0

R A B , solve for the Z-Y-X Euler angles

,, (both solution sets).

Answer:

Solution

1 50

40

30

2 230 140 210

Be sure to check the second solution set.


33/103

33

Homogeneous Transformation Matrices

One convenient 4x4 matrix representation to give pose (position and orientation) of

one moving Cartesian coordinate frame with respect to a reference frame. Figure:

Vector loop closure equation:

This is a basis shift (remember P RP B A

B A

from earlier), plus a translation since

now B A

P is considered.

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

Perspective, scaling when last row is not 1000 - used in computer graphics.

For robotics purposes, this last row never changes - we don't want to scale or skew rigid

links moved by active joints!

Must append a "1" to all (3 x 1) position vectors to use with Homogeneous

Transformation Matrices. This leads to a dummy equation of 1=1, but this is necessary

for matrix multiplication using Homogeneous Transformation Matrices.


34/103

34

3 Interpretations for Homogeneous Transformation Matrices

Common Example for all 3 interpretations: Given:

1) Description of a frame

T A B describes the pose (position and orientation) of moving Cartesian coordinate

frame { B}, w.r.t. to a reference frame, { A}. Cartesian coordinate frame { A} can be

moving too, but we station an observer on { A} to watch how { B} is translating and

rotating with respect to { A}.

B A

P is the position vector giving the location of the origin of { B} w.r.t. the origin

of { A}, expressed in the basis (coordinates) of { A}.

R

A

B is the rotation matrix giving the orientation of { B} w.r.t. { A}; columns arethe XYZ unit vectors of { B} projected onto the XYZ { A} unit directions.

|||

ˆˆˆ

|||

B A

B A

B A A

B Z Y X R

Figure for first interpretation:


35/103

35

2) Transform Mapping:

Matrix T A B maps PP

A B

. Describes a vector known in one Cartesian coordinate

frame { B} in another frame { A}. There is both position and orientation (basis) difference

in general. Equation:

Given: P B , find P

A

Note: P RPP B A

B B A A

Figure for second interpretation:


36/103

36

3) Transform Operator:

T operates on 1P A

to yield 2P A

. Same Cartesian coordinate frame { A}, there is no

{ B} for this interpretation. The original vector 1P A has been translated and rotated to

new vector2

P A . Order of translation and rotation doesn't matter if we assume rotations

always occur about the tail of vectors. Equation:

Given: 1P A , find 2P

A

Figure for third interpretation:


37/103

37

Invert Homogeneous Transformation Matrices

Given T A B , find the opposite Homogeneous Transformation Matrix, i.e. the pose

of Cartesian coordinate frame { A} w.r.t. { B}:1

T T A B

B A

Can just use matrix inversion (Matlab function inv), but this is computationally

expensive, may be numerically unstable, and doesn't take advantage of the structure of

Homogeneous Transformation Matrices. Alternatively, Gaussian elimination is less

computationally expensive and more robust numerically, but it still doesn't take

advantage of the structure of Homogeneous Transformation Matrices.

T T B

A A B

1

Beautiful Property from Orthonormal Rotation Matrices:

Translation part: Without regard for frame of expression:

With regard,

1T A B

Example: Given: find 1 T T A B

B A

Figure for example:


38/103

38

Transform Equations

We will represent a robot by a series of coordinate frames, one moving Cartesian

coordinate frame, rigidly attached to each moving rigid link at each active joint.

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

i

1

.

A vector Pi

is mapped back to Pi 1

by T i

i1

:

For a general robot, there are many such frames.

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

Given P D

(e.g. hand location in local frame), find P A

(e.g. hand location in base

frame):

Associated Transform Graph:

Now, given any three of these Homogeneous Transformation Matrices, we should be able

to calculate the fourth using linear algebra and matrix inversion; show one example:

Associated Transform Graph:


39/103

39

Transform Equations: Robotics Example

Frames: Figure:

{WO} World

{ B} Base

{WR} Wrist

{ H } Hand

{G} Goal

Given: Fixed matrices: T T T WO

GWR H

WO B ,,

1) Given T H G from machine vision algorithm. Calculate T

BWR (Can compare with T

BWR

calculated by forward pose kinematics and joint angle encoders.)

Transform loop closure equation:

T T T T T H

GWR H

BWR

WO B

WOG

T T T T T WR

H B

WR H G

WOG

WO B

11

111

T T T T T WR

H H G

WOG

WO B

BWR

or,11

T T T T WR

GWO

GWO

B B

WR

where 1111

T T T T T WR H H G

H G

WR H

WRG

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

WR to achieve that

pose.

I T H G

11

T T T T WR

H WO

GWO

B B

WR

Use transform graphs to check results for both cases.


40/103

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) Z axis is along the rotation direction for revolute joints, along the translation

direction for prismatic joints.

2) X axis is directed along the unique common normal between consecutive Z

axes. If consecutive Z axes intersect, X must be perpendicular to both, and there is

considerable freedom in defining X .

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

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

First Link:

Choose 0ˆ Z along 1

ˆ Z , and ensure frames {0} and {1} are identical when the first

variable is 0.

Simplify Kinematics:

Kinematic base vs. physical base of robot.

Hand frame vs. wrist frame for terminating basic kinematic equations.


41/103

41

Drawing of General Link Connecting 2 Joints:

D-H Parameters:

1

i : Angle between 1ˆ

i Z to i Z ˆ measured about 1

ˆ

i X

1

ia : Distance from 1ˆ

i Z to i Z ˆ measured along 1ˆ

i X

id : Distance from 1ˆ

i X to i X ˆ measured along i Z

ˆ


i X to i X ˆ measured about i Z

ˆ

Screw Pairs:

1

i , 1

ia align Z axes about and along 1ˆ

i X

id , i align X axes about and along i Z ˆ

General Result: Table of DH ParametersColumns are joint/link index i and the 4 DH parameters.

Rows are the 4 DH parameters for each active joint/link in the serial chain: total of

n active joints/moving links.

i 1

i 1

ia 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/103

42

DH Parameters Examples

Complete Examples

1. 3-axis Planar 3R Articulated Robot

X

Y

Z

0

0

1

X 1

X 2

X 3

X H

Z 0

Z 2

Z 3

Z H

Y H

L 2

L 3

L 1

1

2

3

i 1

i 1

ia id i

1 0 0 0 1

2 0 1 L 0 2

3 0 2 L 0 3

Issues:

Links in different planes, but shift all X axes to align (d i=0)

What happened to L3?


43/103

43

2. 3-axis Spatial PRP Cylindrical Robot

X 0

Z 0

1 X Z Z 1 2

L 2

X 2

Z 3

1 L

X 3

21 Z Z

Top

Front

i 1

i 1

ia id i

1 0 0 1 L 0

2 0 0 0 90

3 90 0 2 L 0


44/103

44

3. 6-axis Spatial 6R Unimation PUMA Robot

i 1

i 1

ia id i

1 0 0 0 1 2 90

0 0 L 902

3 0 1 L 0 903

4 90 0 2 L 4

5 90

0 0 5

6 90 0 0906


45/103

45

Issue: Joint angle offsets

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


i X to i X ˆ measured about i Z

ˆ

The kinematic diagrams generally show the zero value definition for each joint angle

parameter. In the zero angle configuration, if the 1ˆ

i X and i X ˆ axes are NOT identical

when 0 i , then a (constant) joint angle offset is required to account for this. Examples

of this may be seen in the 2nd

row for the Cylindrical Robot and the 2nd

, 3rd

, and 6th

rows

for the PUMA Robot above.


46/103

46

Incomplete Example

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

4. 7-axis Spatial 7R Fanuc S10 Robot

i 1

i 1

ia id i

1

23

4

5

6

7


47/103

47

Forward Pose Kinematics

Given numerical values for all the joint variables, find the pose (position and

orientation) of the end-effector Cartesian coordinate frame (or other frame of interest)

with respect to the base Cartesian coordinate frame.

Given ; Find (4x4 Homogeneous Transformation Matrix)

Above assumes all revolute joints with given joint angles – if there are prismatic joints, a

joint length value must be given for those joints.

Non-linear (transcendental) expressions, but solution is straight-forward – all terms

(joint angles) inside sines and cosines are given. We will use concatenation of matrices

(consecutive link transformations) – matrix multiplication to find result. Result can 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}: T

ii1

. Attach 3 intermediate frames

{P}, {Q}, and { R}. Figure:

From {i-1} to {i}: Rotate 1 i from 1ˆ i Z to P Z ˆ about 1ˆ i X .Translate 1

ia from P Z ˆ to Q Z

ˆ along P X ˆ .

Translate id from Q X ˆ to R X

ˆ measured along Q Z ˆ .

Rotate i from Q X ˆ to R X

ˆ about R Z ˆ .


48/103

48

1000

0

1111

1111

1

1

iiiiiii

iiiiiii

iii

ii

cd cscss

sd scccsasc

T

Physical interpretation: ii i

i R P 1 1

,

(Craig Equation 3.6)

ii Z ii X i

i d ScrewaScrewT ,, 111

1000

00

00

001

,11

11

1

111111ii

ii

i

i X i X i X i X ii X cs

sc

a

Ra Da D RaScrew

1000

100

00

00

,i

ii

ii

i Z i Z i Z i Z ii Z d

cs

sc

Rd Dd D Rd Screw

Caution: screws commute (order of translation and rotation doesn't matter), but matrices

don't in general. Also, cannot commute the X and Z screws, they must appear as in the

equations above.


49/103

49

Forward Pose Kinematics Result:

Concatenate Consecutive Homogeneous Transformation Matrices:

T N 0 gives the pose (position and orientation) of the last active joint/link Cartesian

coordinate frame { N } with respect to the kinematic base Cartesian coordinate frame,

attached to link {0}. It is a function of all N joint variables:

There are 2 different representations: N N X T

00

, where

T N z y x X

0 , and we use Z-Y-X ( ) Euler angles convention.

Sum of Angles Simplification

If any two (or more) consecutive Z axes are parallel (i.e. consecutive i rotate

about parallel axes), we can simplify the resulting symbolic Forward Pose Kinematics

expressions by using sum of angle formulas:

casbsacbba

sasbcacbba

sin

cos

Many common industrial robots have this parallel axes characteristic for at least one pair.

First multiply together any two individual Homogeneous Transformation Matrices that

represent consecutive parallel axes (take care to keep the proper matrix multiplication

order; i.e. use the associative property of matrix multiplication, DO NOT commute the

order of matrices!). Then use the above trig formulas to simplify before completing the

other matrix multiplications.

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


50/103

50

Additional, fixed transforms

The above T N 0 result is for the active joints only – often we need to expand this

result to include additional transformations that are constant. For example, the kinematic

base frame {0} may be located at the shoulder of the robot, while another base frame { B}

mounted on the floor may be convenient. Also, the Forward Pose 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 T B0 and T

N H are not

determined by DH parameters since there is no active joint represented by them. Simply

determine these matrices by inspection. Make the orientation identical if at all possible.Please see the examples for more details. Overall transform equation:


51/103

51

Forward Pose Kinematics Examples


Forward Pose Kinematics Symbolic Derivations

Given 321 ,, , calculate T 03 and T H

0 . Plug each row of DH table into Eq. 3.6.

i 1

i 1

ia id i

1 0 0 0 1

2 0 1 L 0 2

3 0 2 L 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

s Ls Lcs

c Lc Lsc

T T T T (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


52/103

52

What happened to L3?: Additional, fixed transform

1000

0100

0010

001 3

3

L

T H (by inspection from the planar 3R figure)

For this robot we need to control the { H } frame. However, there is no separate base

frame { B}, i.e. {0} is sufficient. The overall Forward Pose Kinematics Homogeneous

Transformation Matrix is given below. L3 is a constant since the third link is rigid.

33

32103

0 ,, LT T T H H

1000

0100

0010

001

1000

0100

0

03

12211123123

12211123123

0

L

s Ls Lcs

c Lc Lsc

T H

1000

0100

0

0

123312211123123

123312211123123

0 s Ls Ls Lcs

c Lc Lc Lsc

T H

Position vector significantly more complicated; orientation identical.


53/103

53


Forward Pose Kinematics Example

1) Given 1,2,3 321 L L L and 35,25,15 321 , calculate T H

0 .

T T T H H 3030

1000

0100

062.20259.0966.0

430.40966.0259.0

03T ;

1000

0

0

1

3 I T H

1000

0100

028.30259.0966.0

689.40966.0259.0

0T H

2) Given 1,2,3 321 L L L and 0,90 321 , calculate T H

0 .

1000

0100

5001

0010

03T sameT H

3

1000

0100

6001

0010

0T H

This second example is sufficiently simple to check your results by a sketch – be sure to

include the { H } and {0} axes to check the orientation R H 0 in addition to the position

vector H P0 . Actually, since the robot is planar, you should check the first example too

using a sketch!


54/103

54


Forward Pose Kinematics Symbolic Derivations

Given 21 ,, L L , calculate T 03 . Plug each row of DH table into Eq. 3.6.

i 1

i 1

ia id i

1 0 0 1 L 02 0 0 0 90

3 90

0 2 L 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

s Lsc

c Lcs

T

(interpret geometrically)

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


55/103

55


Forward Pose Kinematics Example

1) Given 2,30,3 21 L L

, calculate T

03 .

1000

3010

15.00866.0732.1866.005.0

03T

2) Given 2,90,3 21 L L

, calculate T

0

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 R

03 in addition to the position vector 3

0P .


56/103

56

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.


57/103

57

Existence of solutions

For m = n or m < n, no real solution exists when the given Cartesian pose is out of

the manipulator's workspace. The solutions are complex (imaginary) in this case.

For m > n, a solution exists if the pose in within the workspace, and ONLY IF the

commanded pose is within the limited task space spanned by the manipulator. For

example, a 5-DOF (n=5) robot cannot turn a ratchet in general, but is sufficient for axis-

symmetric applications (such as spray painting). Actually this is still the m = n case

because m is reduced to m=5 for axis-symmetric Cartesian tasks not requiring the robot

roll freedom.

Solution Methods

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

Analytical (closed-form)

Algebraic vs. Geometric, combination of the two

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


58/103

58

General Solution Procedure for Inverse Pose Kinematics of Serial Robots

Solution of the Inverse Pose Kinematics starts with the same expressions from

Forward Pose Kinematics. Write 16 equations (equate two Homogeneous

Transformation Matrices). The LHS matrix are known, given numbers. The RHS matrix

elements are symbols (from the Forward Pose Kinematics Homogeneous Transformation

Matrix). Now the Cartesian pose T N 0

is known (commanded) and the joint variables

T N ,,, 321 are unknown:

Spatial Serial Robots

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

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

9 rotational equations - only 3 are independent.

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

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

2 translational equations (column 4) – both are independent.

4 rotational equations - only 1 is independent.

If m=n (the number of Cartesian and joint freedoms match) we can solve the problem, in

principle. Maybe not in closed-form (analytical); maybe the solution has to be numerical.


59/103

59

Inverse Pose Kinematics Examples


X

Y

Z

0

0

1

X 1

X 2

X 3

X H

Z 0

Z 2

Z 3

Z H

Y H

L 2

L 3

L 1

1

2

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.


60/103

60

First, Simplification – take advantage of fixed L3 transform

Inverse Pose Equations to Solve

Subspace of general 3D pose space:

Note that the Forward Pose Kinematics result can be represented by T 03 or by

T y x X 33 . The given LHS matrix cannot be a general 3D pose since this is a

planar problem. The expression of the subspace of general 6-dof poses spanned by the

XY plane and described by T 03 is given below:

Equations to Solve:

Write 2 independent translational equations:

Write 2 rotational equations, only 1 is independent:

Rotational equations:


61/103

61

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 solve

for 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

211

2sin

t

t

Substitute into the EFG equation:

01

2

1

1

22

2

Gt

t F

t

t E 0121

22

t Gt F t E

022 E Gt F t E G quadratic formula: E G

GF E F t

222

2,1


62/103

62

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

Two 1 solutions result from the in the quadratic formula; both are correct

(multiple solutions – elbow up and elbow down).

To find 2 , return to original arranged translational equations:

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

Now we have two solutions for 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

,,

.


63/103

63




1) Given T H 0 and 1,2,3 321 L L L , calculate 2,1,,, 321 ii .

1000

0100

028.30259.0966.0

689.40966.0259.0

0T H

1303

003

T T T T T H H H

H

1000

0100

062.20259.0966.0

430.40966.0259.0

1000

0

0

1

1000

0100

028.30259.0966.0

689.40966.0259.0

03

I T

1) Answers (deg):

i 1 2 3

1 15 25 35

2 35 -25 65

The first line is expected since this is the same pose as the planar 3R robot Forward Pose

Kinematics example 1). Check the second solution to ensure it is correct (plug into

Forward Pose Kinematics and ensure you get the same T H 0 back).


64/103

64

2) Given T H 0

and 1,2,3 321 L L L , calculate 2,1,,, 321 ii .

10000100

6001

0010

0T H ;

10000100

5001

0010

03T

2) Answers (deg):

i 1 2 3

1 90 0 0

There is only one solution (why?).

3) Given T H 0 and 1,2,3 321 L L L , calculate 2,1,,, 321 ii .

1000

0100

928.60866.05.0

0.405.0866.0

0T H ;

1000

0100

428.60866.05.0

134.305.0866.0

03T

3) Answers (deg):

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


65/103

65

Inverse Pose Kinematics Examples (cont.)


X 0

Z 0

1 X Z Z 1 2

L 2

X 2

Z 3

1 L

X 3

21 Z Z

Top

Front

Inverse Pose Kinematics Symbolic Solution

Given T 03 , calculate 21 ,, L L .

Forward Kinematics Solution:

1000010

0

0

1

2

2

03

L

s Lsc

c Lcs

T

Inverse pose equations come from the forward pose expressions:

1000

010

0

0

1000

1

2

2

333231

232221

131211

L

s Lsc

c Lcs

pr r r

pr r r

pr r r

z

y

x


66/103

66

This robot has spatial motion, but rotation is limited to planar. Subspace of general 6-dof

pose (where the pose is represented by T 03 or by

T z y x X 333 :

1000

010

0

0

1000

010

0

0

1

2

2

3

3

3

L

s Lsc

c Lcs

z

ysc

xcs

We have a problem – there are only 3 joints (n=3) but there are m=4 Cartesian values.

This is an overconstrained problem and no solution exists in general – a dependency

exists among T

z y x X 333 and all four cannot be commanded independently.

Therefore, let us command only 3 Cartesian values T

red z y xP X 33330

; we will

treat this robot as a translational freedom robot. is not independent but is related to x3

and y3. The three equations to solve for the three unknowns are then taken only from the

translational equations:

13

23

23

L z

s L y

c L x

Inverse Pose Kinematics Solution: (LEFT TO THE INTERESTED STUDENT)

Mathematically, there are two solution sets; the 2 L solution is not a practical choice.

(If 2 L is allowed, then is the angle solution).

This is not a general spatial manipulator, i.e. 30P and R

03 cannot be specified

independently.


67/103

67

Inverse Pose Kinematics Examples (cont.)



1) Given 30

P , calculate 21 ,, L L .

3

1

732.1

3

3

3

30

z

y

x

P

1) Answers (m and deg):

i 1 L 2 L

1 3 30 2

2 3 210 -2

The first line is expected since this is the same position as the planar 3R robot Forward

Pose Kinematics example 1). Check the second solution to ensure it is correct (plug into

Forward Pose Kinematics and ensure you get the same 30P back). Also, for both cases

you can calculate R03 - they will be different (why?).

2) Given 30P , calculate 21 ,, L L .

3

2

0

3

3

3

30

z

y

x

P

2) Answers (m and deg):

i 1 L 2 L

1 3 -90 2

2 3 90 -2


68/103

68

Trajectory Generation

Robot Trajectory is the commanded/actual time history of position, velocity,

acceleration motion. Could be at joint level and/or Cartesian level (kinematics relates

these two levels). Command and control Cartesian trajectories of

T

z y x X via joint trajectories of

T 654321

(for a general 6-dof spatial robot).

Many robotics applications require motion only at discrete points:

spot welding

assembly

joining parts

etc.

How to move smoothly and efficiently between the required discrete poses?

Other robotics applications require commanded trajectory motion:

arc welding

spray painting

parts handling from moving conveyors

etc.

How to command smooth trajectories to satisfy motion requirements?

To both questions, there are infinite possible answers! e.g.:

straight-line motion

constant velocity motion (stay tuned for resolved-rate control)

avoid obstacles

shortest distance (same as straight-line motion? – Cartesian only)

shortest time

etc.

Even with one specific choice (e.g. avoid obstacles), there are infinite possible

trajectories. In our Trajectory Generation work, we will focus on a small subset of the

possible trajectory generation methods (Chapter 7, Craig): We will assume the control

architecture is Inverse Pose Kinematics Control and we will perform “smooth” Joint-

Space Trajectory Generation to move between discrete commanded Cartesian poses.


69/103

69

Inverse Pose Kinematics

Calculate the n joint angles (for revolute, or length for prismatic) required to place

the robot end-effector in a given set of required Cartesian poses. We will consider just an

initial and final pose; obviously more can be included by making the final pose the new

initial and adding a new final pose. The strongly-required poses are denoted as “path

points”, while intermediate poses to pass through are denoted “via points”.

For the initial and final poses, use the Inverse Pose Kinematics solution to

determine valid sets of joint angles that will accomplish these Cartesian pose commands

(remember, there are multiple solutions to the IPK problem in general – useful for

obstacle avoidance . . .). Now, how do we move “smoothly” between the initial and final

joint angle sets? We could just define a bunch of intermediate Cartesian poses and

calculate IPK a

Date post:	06-Jul-2018
Category:	Documents
Upload:	arun
View:	233 times
Download:	0 times

Mechanics Control of Robotic Manipulators

Documents