ME 4135 Robotics & Control

Slide Set 3 – Review of Matrix Methods Applicable to Robot

Control

Creating a Rational Approach to Kinematics – A review of Matrix Methods

As the robots got more and more “Revolute” building Inertial models (FKS & IKS) was increasingly complicated

We would like a more logical approach to this problem

We will define a concept of Homogeneous Matrices in S–O3 Space to aid in this model building effort

Consider, we only built Positional DOF models to this point

We need both Position & Orientation models to drive real robots!

Considering Translation and Rotation

Translation, in a simple sense, is just the movement of one point from another without changing the orientation of space.

We can assign space frames (coordinate systems) to any object in space – (or all objects in space!).

If we wish to relate one object (and its space frame) to any other space frame we should be able to write a set of equations that represent each axis of the remote space in another’s systems axes and write a vector that relate the positions of the origins of the ‘systems’ to each other.

O0 (0,0,0)

O1

(12,35,45)

P1

A1

Defining Transformations – here lets consider translational transformations

Lets say that we have a point P1 sitting at the origin of Frame1, and a second point A1 located at (2,7,3)1 tell me, What is the pose of the coordinate frame attached to A1 as described in the space Fame 0?

Translational Transformation

In physics we said to just add the two vectors (because the vector numbers are ‘the same’ since the axes point in the same directions)

So if A1 is at (2,7,3) in ‘1th Space’ then it is at: (2,7,3) + (12,35,45) = (2+12, 7+35, 3+45) = (14,42,48) in Null Space

But simple vector addition techniques only works for simple translation where space is not ‘reorientated’!

We must then Generalize the method (to me this ‘general approach’ is easier – but it seems more cumbersome when we start thinking this way!)

Defining the Homogeneous Transformation Matrix It is a 4x4 Matrix that describes “3-Space” with

information that relates Orientation and Position (pose) of a remote space to a local space

nx ox ax dx ny oy ay dynz oz az dz 0 0 0 1

N vector projects the Xrem Axis to the Local Coordinate System

O vector projects the Yrem Axis to the Local Coordinate System

A vector projects the Zrem Axis to the Local Coordinate System

D vector is the position of the origin of the remote space in Local Coordinate dimensions

This 3x3 ‘Sub-Matrix’ is the information that relates orientation of Framerem to Frame Local

(This is called R the rotational Submatrix)

Defining the Homogeneous Transformation Matrix

nx ox ax dx ny oy ay dynz oz az dz 0 0 0 1

Perspective or Projection Vector

Scaling Factor

• This matrix is a transformation tool for space motion!

HTM – A Physical Interpretation

1. A representation of a Coordinate Transformation relating the coordinates of a point ‘P’ between 2 like-geometrid (-- ie SO3 --) different coordinate systems

2. A representation of the Position and Orientation (POSE) of a transformed coordinate frame in the “space” defined by a fixed coordinate frame

3. An OPERATION that takes a vector P and rotates and/or translates it to a new vector Pt in the same coordinate frame

Lets use it on our ‘Translational’ problem What is the n vector here Well, since X1 points in X0

direction, it is simply: (1,0,0) Using the same reasoning:

The o vector is: (0,1,0) And the a vector is: (0,0,1)

Here the d vector is: The definition of the origin of Frame1 in

Frame0 coordinates: (12,35,45)

Solving: H. Transformation Matrix is:

Point A1 to ‘1 space’:

1 0 0 12 0 1 0 35 0 0 1 45 0 0 0 1

1 0 0 2 0 1 0 7 0 0 1 3 0 0 0 1

The solution of where A1 is in Frame0 is the product of these two matrices!

T01 =

T1A =

1 0 0 140 1 0 420 0 1 480 0 0 1

Solution is given by:

T01· T1

A =

Hey, This works! (we got the same answer)-at least for this translational stuff!

What about Rotational Transformations?

Lets start with a “Pure Rotation” A Pure Rotation is one about only 1

axis (a separable rotation) We will consider this about Z0

(Initially) This means: Rotate the ‘Remote’

Frame1 by an angle Q about the Z0 axis of ‘Local’ Frame0

After Rotation we find this Relationship

O0,O1

Z0,Z1

Y1 Y0

X0

P1

X1

What is the Representation of P1 in Both Frames?

Assume the (identical) point is at (2,4,6)1 And we had rotated Frame 1 by 37

degrees about the Z0 Axis Where is the point as defined in Frame0? We will employ the Method of Inner

Products to find this.

By Inner Products:

1 1 1 1 1 1 1

0 0 0 0 0 0 0

p p p

p p p

P x i y j z k

P x i y j z k

0 1 0 1 1 1 1 1 1 0

0 1 0 1 1 1 1 1 1 0

0 1 0 1 1 1 1 1 1 0

( )

( )

( )

p p p p

p p p p

p p p p

x P i x i y j z k i

y P j x i y j z k j

z P k x i y j z k k

Relating these two definition of the SAME Point:

Collecting & Simplifying :0 1 0 1 1 0 1 1 0 1

0 1 0 1 1 0 1 1 0 1

0 1 0 1 1 0 1 1 0 1

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

p p p p

p p p p

p p p p

x i i x j i y k i z

y i j x j j y k j z

z i k x j k y k k z

0 1 0 1 0 1 0 1

0 1 0 1 0 1 0 1

0 1 0 1 0 1 0 1

p p

p p

p p

x i i j i k i xy i j j j k j yz i k j k k k z

Rewriting it in Matrix Form:

Psst:This is a R matrix!

Converting it to a HTM Form (4x4)

0 1

0 1

0 1

0 00 0

0 0 1 00 0 0 11 1

p p

p p

p p

Cos Sinx xSin Cosy y

z z

1 0

1 0

For the Dot (inner) Product:

( ( ))1 1 (360 )

where:(360 ) (360) * (360) *

thus:

similarly for all other Dot Terms

a b a b Cos a bi i Cos

Cos Cos Cos Sin SinCos

i i Cos

Vector of origin1 to orgin0 is (0,0,0) – they are the same point!

Lets See? is 37deg and P1 is (2,4,6)

Cos = 0.799 Sin = 0.602 HTM is:

Model is:

0.799 0.601 0 00.601 0.799 0 0

0 0 1 00 0 0 1

0

0

0

0.799 0.601 0 0 20.601 0.799 0 0 4

0 0 1 0 60 0 0 1 11

p

p

p

xyz

Solving then: XP

0 = (row1 * P1) = .799*2-.601*4+0*6+0*1=-0.806

YP0 = (row2*P1)

= .601*2+.799*4+0*6+0*1 = 4.398

ZP0 = (row3*P1) =

0*2+0*4+1*6+0*1 = 6 This is the same as we Observed!

What about Pure Rotation about X or Y Axes? Uses the same Inner Product

approach (Cosines of angles between vectors after rotation)

Trotx =

Troty =

1 0 0 00 00 00 0 0 1

1 0 0 00 00 00 0 0 1

Cos SinSin Cos

0 00 1 0 0

0 00 0 0 1

Cos Sin

Sin Cos

Lets look at Another Issue! Since we are in Matrix Math now, We

remember that the “order of multiplication” matters

That is A*B B*A (in general) When we deal with physical space this is true

as well. But it even offers one more added difficulty: Did we take motion Relatively (space is redefined

after an operation) or are all operations taken W.R.T. a fixed geometric space?

First Define two simple Operations: Simple Translation of (4,0,0)

A =

Simple Rotation of 90 about Zaxis

B =

1 0 0 40 1 0 00 0 1 00 0 0 1

0 1 0 01 0 0 00 0 1 00 0 0 1

Now Define 2 Cases:

Case 1 is where we “redefine” Space after each operation

Case 2 is where all operations are taken against a fixed (inertia) space frame

Check Order issue: 1st Translate then rotate Its almost like drawing a cat!

Autocad Here! (Case 1:Trans – rot)

Given P2 (1,1,0)2 Where is it in Space0?

Let’s Guess it is found by applying an (overall) Transformation given by:

20

20

20

1 0 0 4 0 1 0 00 1 0 0 1 0 0 00 0 1 0 0 0 1 00 0 0 1 0 0 0 1

0 1 0 41 0 0 00 0 1 00 0 0 1

A BT T T

T

T

Is (3,1,0,1)0 Equal to T2

0*(1,1,0,1)2?

?

3 0 1 0 4 11 1 0 0 0 10 0 0 1 0 01 0 0 0 1 1

Simplifying (RHS):0 1 0 4 1 31 0 0 0 1 10 0 1 0 0 00 0 0 1 1 1

They are equal! (so its a good model!)

Check Order issue: 2nd – Rotate then Translate Should be different

physically! Lets See

AutoCad Again (Case1: Rot-Trans)

Given P2 (1,1,0)2 Where is it in Space0? Lets Guess it is found by applying an

(overall) Transformation given by:2

0

20

20

0 1 0 0 1 0 0 41 0 0 0 0 1 0 00 0 1 0 0 0 1 00 0 0 1 0 0 0 1

0 1 0 01 0 0 40 0 1 00 0 0 1

B AT T T

T

T

Is (-1,5,0,1)0 Equal to T2

0*(1,1,0,1)2?

?

1 0 1 0 0 15 1 0 0 4 10 0 0 1 0 01 0 0 0 1 1

Simplifying (RHS):0 1 0 0 1 11 0 0 4 1 (1 4) 50 0 1 0 0 00 0 0 1 1 1

They are equal! (so its a good model!)

Checking Case Two Now we don’t redefine space

between operations That is, all operations are taken

WRT a fixed coordinate system

Autocad: (Case2 Trans – Rot)

Looks Familiar! The effect is just like the

Rotate then Translate operational order when we were in Case 1

Therefore, To get the Transformation Model, we must write: Trot*Ttrans

Yes, the order of multiplying is reversed from the order of operating!

Case 2: Rot - Translate

What about here? Lets see what we get

Autocad: (Case 2, Rot-Trans)

Looks Familiar Too This is just like what happened in

Case 1 when we did Translate then Rotate

The overall effect here must be:Ttrans * Trot

Yes the order of multiplying is again reversed from the order of operating!

This can be Generalized For Case 1 operations (space is

redefined between each operation), the OVERALL EFFECT is found by taking the product of the operations in the order they are taken

For Case 2 operations (all operations taken W.R.T. a fixed Frame), the OVERALL EFFECT is found by forming the product of the individual operations taken in reverse order

This can be Generalized in an Algorithm! As an Algorithm, we would say,

when considering any and all additional individual Operations:

Initially Place the First Operation (must have been taken WRT Frame 0!)

Then write each successive operation as a matrix and place it in “multiply order” by the iterative application of the two steps below (for Operations 2 thru j)

Step 1: Considering Operation i (taken wrt any Frame n), its effect is modeled AFTER all prior operations taken WRT any Frames < n

And Step 2: Considering the same Operation i

(taken about frame n), its effect is modeled BEFORE all prior operations taken WRT Frame n or any frame > n

Looking at our earlier “modeling” When we studied Case I (First

Time) Model Pure Rotation about Z – Call it

OpA Model Translation along X – Call it

OpB 1st Op A about Z0; then 2nd OpB along

X1 By the Algorithm: Place A first then

Determine placement of B – Step 1: place B after A if Frame 1 > Frame 0

(yes!) Step 2: place B before A if Frame 1 0 (No!) Correct order is A*B (as we saw)

If Operational Order was reversed (but still Case I)

What of: Op B along X0 Op B about Z1?

Place A first then determine placement of B – again by the algorithm:

Step 1 place B after A if Frame 0 > Frame 1 (no!)

Step 2: place B before A if Fr0 Fr 1 (Yes!)

Correct order is B*A (as seen)

Setting Case II by Algorithm

Op A is Rot about Z0; Op B is Translation wrt X in Fr0

Place A then apply algorithm to place B Step 1: Place B after A if Fr 0 > Fr 0 (no!) Step 2: Place B before A if Fr 0 Fr 0 (yes!) Correct order is B*A (as we observed)

BUT REMEMBER: The algorithm can easily be extended to any

number of operations (remembering that the a higher number frame results after each successive operation!) regardless of if the individual operation are purely Case I or Case II

This Does Matter! Robotic Modeling is a Case 1

problem Euler Orientation is a Case 1

problem However, RPY Orientation is a Case

2 problem Finally, Robot Mapping is (typically)

a Case 1 problem Lets look into robot mapping

One Last (cool!) General Idea: Robot Mapping

This is an offline tool for finding Robot Targets (IKS targeting!)

Moves Robot programming ‘Upstairs’ – to the engineering office

Relies on CAD models and geometry defined in increasingly complex spaces

Looks at chains of Transforms to define targets and robot tooling in common coordinate frames

Robot Mapping:

CeTa

P

Ho

n

Tool

R

0

Note: Drill not shown, (the Tool frame is actually located at the drill tip!)

Robot Mapping The idea here is to match up the Tool’s

geometric pose with the pose of a Target in our work space.

If we have a part that needs a hole drilled at a certain location, we must get the tool, carried by the robot, to this location (actually a point right above the drilled hole and also at the bottom of the drilled hole will be needed).

Remembering Dynamics, to equate poses, they must be defined in a common coordinate system.

Robot Mapping (cont.) Typically, we would have a lot of geometric

information about our working environment ‘just laying around’

This data would be in the CAD drawings of parts and in CAD facilities plans of our cells and factories

Additionally the information is also provided in equipment drawings (tables, fixtures, even robots to some extent)

But, if we are going to talk about a robot, that machine is a series of adjustable joints and links that can be moved around (in an IKS sense) to put the tool accurately at the working positions we need.

The Necessary Pose is (within the robot): Tn

o

Robot Mapping (cont.) Thinking about what we know:

We would know where we want a hole in a part (in part coordinates) or: TP

Ho

We likely want to place the part at a specific location on a table or in a fixture or: TTa

P or TF

P (known in process documentation)

The Table in the Cell (or fixture on a Table) TCe

Ta or (TTaF and TCe

Ta) would be known in our facility designs and/or process documentation

Robot Mapping (cont.) Other thing we would know:

Were our robot is placed in the cell: TCeR

from facility drawings Were the robot Cartesian home frame is

located in the Robot space: TR0 from

equipment drawings And finally where a tool is mounted to

the end of the robot wrist: Tntool by

measuring the tool and holder What we would like to find is: T0

n which contains the information about where the robot needs to ‘pose’ to do the hole drilling operation!

Robot Mapping (cont.)

Knowing this stuff, we should be able to generate a kinemetic chain (of HTM’s)– or map – that defines the hole in the cell:

TCeTa*TTa

P*TPHo

At the same time we can build a kinematic chain for mapping the Tool (the drill) back to the Cell too:

TCER*TR

0*T0n*Tn

tool Now equate the two – they are defined in a

common reference system – and isolate the desired information (T0

n) That is, extract the ‘unknown’ from the ‘known

stuff’

Robot Mapping (cont.)

Equating the two kinematic chains: TCe

Ta*TTaP*TP

Ho = TCER*TR

0*T0n*Tn

tool

Now, to isolate the T0n information (our desired

pose for the robot), we must remove the “knowns” step-by-step on the RHS of this equation.

To do this we multiply by their inverses. BUT we must maintain order when we do it!

Robot Mapping (cont.) 1st we move TCE

R (TCE

R)-1*TCeTa*TTa

P*TPHo = TR

0*T0n*Tn

tool

Here we have to ‘pre-multiply’ both the LHS & RHS by the inverse of the TCE

R matrix

Then we move TR0 by pre-multiplying by (TR

0)-1 -- both sides

(TR0)-1*(TCE

R)-1*TCeTa*TTa

P*TPHo

=T0n*Tn

tool

Robot Mapping (cont.) Finally we isolate the T0

n matrix by ‘post-multiplying’ both side by: (Tn

tool)-1

(TR0)-1*(TCE

R)-1*TCeTa*TTa

P*TPHo* (Tn

tool)-1 = T0

n

This T0n would contain the data we

need to solve the IKS equations for any Robot type!

Robot Mapping (cont.) Taking the Inverse of a

HTM is “EASY!!!” =

Easy because, physically, it is the same as defining the original ‘close’ frame in the ‘remote’ frame’s space (we have changed – or inversed – our point of view!)

0 0 0 1

x y z

x y z

x y z

n n n n d

o o o o d

a a a a d

This is the Transpose of the R sub-matrix of the original HTM

Now its your turn: Homework

Do Text problems from Chapter 2 in Spong & Vidyasagar: #5, 7, 8, 15, and 18

Approach Problem 18 as a Mapping Problem