ME 555.09: INTRODUCTION TO ROBOTICS Dr.

Hamed
LECTURE 2 Mohammadbagherpoor
 Teaching Assistant:

Siobhan Oca,
Email: siobhan.rigby@duke.edu
Office hours: 3:00-4:00 PM the day the HW is due!

 Form a team for projects

Fill out the spreadsheet that has been shared on Sakai.

(Maximum 5 members per team)
“Due on next Wednesday 01/19/2022”
 KINEMATICS
Transformations
 LECTURE 2: 01-10-2022
What we cover:
oReview of Linear Algebra
oHow to describe an object in 3D space
Descriptions : Describes position, orientation and frames.
o How to map objects in 3D space
Mapping: Change of description of one point from one frame to another.
o Learn about transformation operators
Operators: Transformation and rotation of vectors within the same frame.
o Learn about transformation matrices
Transformation Arithmetic: includes confound transform, inverting a transform,
etc.
o Learn about rotation matrices, Euler angles, and equivalent single-axis
representation
 OPERATIONS WITH MATRICES
Matrix:
 a11 a12 a13  a1n 
a a a  a 
 21 22 23 2n 

A = [aij ] =  a31 a32 a33  a3n  ∈ M m×n

 
    
am1 am 2 am 3  amn 
m×n

(i, j)-th entry (or element): aij

number of rows: m
number of columns: n
size: m×n

Square matrix: m = n
 Equal matrices: Two matrices are equal if they have the same size (m × n) and
entries corresponding to the same position are equal.

=For A [a=
ij ]m × n and B [bij ]m× n ,
=A B if and only if=aij bij for 1 ≤ i ≤ m, 1 ≤ j ≤ n

Example: Equality of matrices

1 2 a b 
A=  B= 
3 4 c d 

If=
A B, then=
a 1,=
b 2,=
c 3, and=
d 4
Matrix addition:

=If A [a=
ij ]m × n , B [bij ]m× n ,

then A + B = [aij ]m×n + [bij ]m×n = [aij + bij ]m×n = [cij ]m×n = C

Example : Matrix addition

− 1 2  1 3 − 1 + 1 2 + 3  0 5
 0 1 + − 1 2 =  0 − 1 1 + 2 = − 1 3
       

 1 − 1  1 − 1  0 
 − 3 +  3 =  − 3 + 3  = 0 
       
− 2  2 − 2 + 2 0
Scalar multiplication:
If A = [aij ]m× n and c is a constant scalar,
then cA = [caij ]m× n

Matrix subtraction:
A − B = A + (−1) B

Example : Scalar multiplication and matrix subtraction

 1 2 4  2 0 0
A = − 3 0 − 1 B =  1 − 4 3
   
 2 1 2 − 1 3 2

Find (a) 3A, (b) –B, (c) 3A – B?

Solution:
(a)
 1 2 4  3(1) 3(2 ) 3(4 )  3 6 12
3A = 3− 3 0 − 1 = 3(− 3) 3(0 ) 3(− 1) = − 9 0 − 3
     
 2 1 2  3(2 ) 3(1) 3(2 )  6 3 6
(b)
 2 0 0  − 2 0 0
− B = (− 1) 1 − 4 3 =  − 1 4 − 3

 
− 1 3 2  1 − 3 − 2
(c)
 3 6 12  2 0 0  1 6 12
3 A − B = − 9 0 − 3 −  1 − 4 3 = − 10 4 − 6
     
 6 3 6 − 1 3 2  7 0 4
Matrix multiplication:
= If A [a=
ij ]m × n and B [bij ]n× p ,
then
= AB [aij ]m× n [=
bij ]n× p [c=
ij ]m × p C,
should be equal
size of C=AB is m × p
n
where cij = ∑ aik bkj = ai1b1 j + ai 2b2 j +  + ain bnj
k =1

 a11 a12  a1n   c11 c12  c1 j  c1n 

  b
  11  b1j  b1n   
        
  b21  b2 j  b2 n
 ai1 ai 2  ain    = c
i1 ci 2  cij  cin 
 
       
            
 b  b  b 
 an1 an 2  ann   n1 nj nn  c
 n1 cn 2  cnj  cnn 


※ The entry cij is obtained by calculating the sum of the entry-by-entry

product between the i-th row of A and the j-th column of B
Example: Find AB
 −1 3
   −3 2 
= A  4 −2  B= 
 −4 1  2× 2
 5 0  3× 2
Sol:
 (−1)(−3) + (3)(−4) (−1)(2) + (3)(1) 
= (4)(−3) + (−2)(−4) (4)(2) + (−2)(1) 
AB
 (5)(−3) + (0)(−4) (5)(2) + (0)(1)  3× 2
 −9 1 
=  −4 6 
 −15 10  3× 2
Note: (1) BA is not multipliable
(2) Even BA is multipliable, it could be that AB ≠ BA
 Transpose of a matrix:

 a11 a12  a1n 

a a22  a2 n 
=If A  21 ∈ M m×n ,
     
 
 am1 am 2  amn 

 a11 a21  am1 

a a22  am 2 
=then AT  12 ∈ M n× m
     
 
 a1n a2 n  amn 
※ The transpose operation is to move the entry aij (original at the position (i, j)) to
the position (j, i)
※ Note that after performing the transpose operation, AT is with the size n×m
Properties of transposes:
(1) ( AT )T = A
(2) ( A + B)T = AT + B T
(3) (cA)T = c( AT )
(4) ( AB)T = B T AT
※ Properties (2) and (4) can be generalized to the sum or product of
multiple matrices. For example, (A+B+C)T = AT+BT+CT and (ABC)T =
CTBTAT
※ Since a real number also can be viewed as a 1 × 1 matrix, the transpose
of a real number is itself, that is, for a ∈ R , aT = a. In other words,
transpose operation has no effect on real numbers
Commutative property of matrix multiplication and cancellation
law

Real number:
ab = ba (Commutative property of real-number multiplication)
Matrix:
AB ≠ BA
m× n n× p n× p m× n

Three results for BA:

(1) If m ≠ p , then AB is defined, but BA is undefined
(2) If m = p, m ≠ n, then AB ∈ M m×m , BA ∈ M n×n (Sizes are not the same)
(3) If m =n, then AB ∈ M m×m , BA ∈ M m×m
p=
(Sizes are the same, but resulting matrices may not be equal)
 A matrix 𝐴𝐴 such that 𝐴𝐴𝑇𝑇 = 𝐴𝐴 is called symmetric, i.e., 𝑎𝑎𝑗𝑗𝑗𝑗 = 𝑎𝑎𝑖𝑖𝑖𝑖 for all 𝑖𝑖 and 𝑗𝑗.

 𝐴𝐴 + 𝐴𝐴𝑇𝑇 must be symmetric.

1 2 3
 Example: 𝐴𝐴 = 2 4 −5 is symmetric.
3 −5 6

 A matrix 𝐴𝐴 such that 𝐴𝐴𝑇𝑇 = −𝐴𝐴 is called skew-symmetric, i.e., 𝑎𝑎𝑗𝑗𝑗𝑗 = −𝑎𝑎𝑖𝑖𝑖𝑖 for all 𝑖𝑖 and 𝑗𝑗.

 A matrix 𝐴𝐴 is called orthogonal if 𝐴𝐴𝐴𝐴𝑇𝑇 = 𝐴𝐴𝑇𝑇 𝐴𝐴 = 𝐼𝐼, i.e., 𝐴𝐴𝑇𝑇 = 𝐴𝐴−1

1/ 3 1/ 6 −1/ 2
 Example: prove that 𝐴𝐴 = 1/ 3 −2/ 6 0 is orthogonal.
1/ 3 1/ 6 1/ 2
1/ 3 1/ 3 1/ 3
Since 𝐴𝐴𝑇𝑇 = 1/ 6 −2/ 6 1/ 6 . Hence, 𝐴𝐴𝐴𝐴𝑇𝑇 = 𝐴𝐴𝑇𝑇 𝐴𝐴 = 𝐼𝐼.
−1/ 2 0 1/ 2
Determinant and matrix inversion
𝑎𝑎11 𝑎𝑎12
Consider a 2 × 2 matrix: 𝐴𝐴 = 𝑎𝑎 𝑎𝑎22
21

 Determinant of 𝐴𝐴, denoted |𝐴𝐴|, is a number and can be evaluated by

𝑎𝑎11 𝑎𝑎12
|𝐴𝐴| = 𝑎𝑎 = 𝑎𝑎11 𝑎𝑎22 − 𝑎𝑎12 𝑎𝑎21
21 𝑎𝑎22

1 2
 If every element of a row (column) is zero, e.g., = 1 × 0 − 2 × 0 = 0, then
0 0
|𝐴𝐴| = 0.

 𝐴𝐴𝑇𝑇 = 𝐴𝐴

 |𝐴𝐴𝐴𝐴| = |𝐴𝐴||𝐵𝐵|

𝑎𝑎11 𝑎𝑎12
 For any 2 × 2 matrix 𝐴𝐴 = 𝑎𝑎 𝑎𝑎22
21

1 𝑎𝑎22 −𝑎𝑎12
Its inverse can be written as 𝐴𝐴−1 = −𝑎𝑎21 𝑎𝑎11
|𝐴𝐴|
 Inverse of a non-singular matrix

 Given any non-singular matrix A, its inverse can be found from the formula
adj 𝐴𝐴
𝐴𝐴−1 =
|𝐴𝐴|

𝑎𝑎11 𝑎𝑎12 𝑎𝑎13

𝐀𝐀 = 𝑎𝑎21 𝑎𝑎22 𝑎𝑎23
𝑎𝑎31 𝑎𝑎32 𝑎𝑎33

1+1 𝑎𝑎
𝑎𝑎22 𝑎𝑎23 1+2 𝑎𝑎
𝑎𝑎21 𝑎𝑎23 1+3 𝑎𝑎
𝑎𝑎21 𝑎𝑎22
𝐴𝐴 = −1 11 𝑎𝑎32 𝑎𝑎33 + −1 12 𝑎𝑎31 𝑎𝑎33 + −1 13 𝑎𝑎31 𝑎𝑎32

𝑎𝑎22 𝑎𝑎23 𝑎𝑎12 𝑎𝑎13 𝑎𝑎12 𝑎𝑎13

+ 𝑎𝑎 𝑎𝑎33 − 𝑎𝑎 𝑎𝑎33 + 𝑎𝑎 𝑎𝑎23
32 32 22
𝑎𝑎21 𝑎𝑎23 𝑎𝑎11 𝑎𝑎13 𝑎𝑎11 𝑎𝑎13
adj(𝐀𝐀) = − 𝑎𝑎 𝑎𝑎33 + 𝑎𝑎 𝑎𝑎33 − 𝑎𝑎 𝑎𝑎23 .
31 31 21
𝑎𝑎21 𝑎𝑎22 𝑎𝑎11 𝑎𝑎12 𝑎𝑎11 𝑎𝑎12
+ 𝑎𝑎 𝑎𝑎32 − 𝑎𝑎 𝑎𝑎32 + 𝑎𝑎 𝑎𝑎22
31 31 21

 (𝐴𝐴𝐴𝐴)−1 = 𝐵𝐵 −1 𝐴𝐴−1
Example : Find the inverse of the following matrix:

1 −1 3
𝐴𝐴 = 2 1 2
−2 −2 1

1+1 1 2 2+1 −1 3 3+1 −1 3

𝐴𝐴 = 1 ∗ −1 ∗ + 2 ∗ −1 ∗ + −2 ∗ −1 ∗
−2 1 −2 1 1 2
= 5 − 10 + 10 = 5

1 2 −1 3 −1 3
+ − +
−2 1 −2 1 1 2 5 −5 −5
2 2 1 3 1 3
𝑎𝑎𝑎𝑎𝑎𝑎 𝐴𝐴 = − + − = −6 7 4
−2 1 −2 1 2 2
2 1 1 −1 1 −1 −2 4 3
+ − +
−2 −2 −2 −2 2 1

𝐴𝐴𝐴𝐴𝐴𝐴 𝐴𝐴 1 −1 −1
𝐴𝐴−1 = = −1.2 1.4 0.8
𝐴𝐴
−0.4 0.8 0.6
 Trigonometric Identities

 Periodic identities:

sin(𝜃𝜃) = 𝑠𝑠𝑠𝑠 = −sin(−𝜃𝜃) = −cos 𝜃𝜃 + 90∘ = cos 𝜃𝜃 − 90∘

cos(𝜃𝜃) = 𝑐𝑐𝑐𝑐 = cos(−𝜃𝜃) = sin 𝜃𝜃 + 90∘ = −sin 𝜃𝜃 − 90∘

 Sum or difference of angles:

sin 𝜃𝜃1 + 𝜃𝜃2 = 𝑠𝑠12 = 𝑠𝑠1 𝑐𝑐2 + c1 𝑠𝑠2 sin 𝜃𝜃1 − 𝜃𝜃2 = 𝑠𝑠1 c2 − 𝑐𝑐1 𝑠𝑠2

cos 𝜃𝜃1 + 𝜃𝜃2 = c12 = c1 c2 − 𝑠𝑠1 𝑠𝑠2 cos 𝜃𝜃1 − 𝜃𝜃2 = c1 c2 + 𝑠𝑠1 𝑠𝑠2

 Sum of squares:
𝑐𝑐 2 𝜃𝜃 + 𝑠𝑠 2 𝜃𝜃 = 1

 Law of cosines in a triangle with A, B, C sides and angle 𝜃𝜃 opposite to side A:

𝐴𝐴2 = 𝐵𝐵2 + 𝐶𝐶 2 − 2𝐵𝐵𝐵𝐵cos 𝜃𝜃

 The "tangent to the half angle" substitution:

𝜃𝜃 2𝑢𝑢 1 − 𝑢𝑢2
𝑢𝑢 = tan sin 𝜃𝜃 = cos 𝜃𝜃 =
2 1 + 𝑢𝑢2 1 + 𝑢𝑢2
 Dot product: Cross Product:

Let 𝐴𝐴 and 𝐵𝐵 be arbitrary vectors in The cross product of two vector 𝑎𝑎 and 𝑏𝑏
𝑅𝑅3 and 𝜃𝜃 be the angle from 𝐴𝐴 to 𝐵𝐵, produce a vector that is perpendicular
then to both of the vectors.
𝐴𝐴 � 𝐵𝐵 = | 𝐴𝐴 | | 𝐵𝐵 | cos 𝜃𝜃 | 𝐴𝐴 × 𝐵𝐵 | = | 𝐴𝐴 | | 𝐵𝐵 | sin 𝜃𝜃
 Properties of Dot Product : Properties of Cross Product :
If 𝐮𝐮, 𝐯𝐯, and 𝐰𝐰 are vectors, then  The length of the cross product of two vectors is
equal to the area of the parallelogram determined
 Commutative Property by the two vectors (see figure below).

 Anticommutativity:
v�u=u�v
 Distributive Property 𝑎𝑎⃗ × 𝑏𝑏 = −𝑏𝑏 × 𝑎𝑎⃗

v∙(u+w)=v∙u+v∙w  Multiplication by scalars:

0 ⋅ 𝑣𝑣 = 0 𝑣𝑣 ⋅ 𝑣𝑣 =∥ 𝑣𝑣 ∥2 (c𝑎𝑎)
⃗ × 𝑏𝑏 = 𝑐𝑐(𝑎𝑎⃗ × 𝑏𝑏) = 𝑎𝑎⃗ × (𝑐𝑐𝑏𝑏)

 Properties of orthonormal coordinate  Distributivity:

frame
𝑎𝑎⃗ × (𝑏𝑏 + 𝑐𝑐)
⃗ = 𝑎𝑎⃗ × 𝑏𝑏 + 𝑎𝑎⃗ × 𝑐𝑐⃗
Mutually perpendicular Unit vectors
  
i ⋅ j =0 | i |= 1  The scalar triple product of the vectors 𝐚𝐚, 𝐛𝐛, and c:
  
i ⋅k = 0 | j |= 1 𝑎𝑎⃗ ⋅ (𝑏𝑏 × 𝑐𝑐)
⃗ = (𝑎𝑎⃗ × 𝑏𝑏) ⋅ 𝑐𝑐⃗
  
k⋅ j =0 | k |= 1
 Descriptions
Describes position, orientation and frames.
Position:
Define the position of a point “P” relative to coordinate frame {A}

Vector 𝐴𝐴𝑃𝑃 ∶ Describes point P relative to frame {A}

𝑃𝑃𝑥𝑥
𝐴𝐴
The “position vector” is: 𝑃𝑃 = 𝑃𝑃𝑦𝑦 P
𝑃𝑃𝑧𝑧 {A}
 Example:
Describe and draw the position of point “P” relative to coordinate frame {A}
if point “P” is positioned at: 3,4 and 5 in x, y, and z of frame {A.}
𝑃𝑃𝑥𝑥 3
𝐴𝐴
The “position vector” is: 𝑃𝑃 = 𝑃𝑃𝑦𝑦 = 4
𝑃𝑃𝑧𝑧 5
 Example:
Describe and draw the position of point “P” relative to coordinate frame {B}
if point “P” is positioned at: 3,-3 and 2 in x, y, and z of frame {B}.

Solution:
𝑃𝑃𝑥𝑥 3
𝐴𝐴
The “position vector” is: 𝑃𝑃 = 𝑃𝑃𝑦𝑦 = −3
𝑃𝑃𝑧𝑧 2
 Orientation:
Define the orientation of an object relative to coordinate frame {A}
regardless of position.
 A new frame {B} on the object is required.
 Orientation:
Define the orientation of an object relative to coordinate frame {A}
regardless of position.
 A new frame {B} on the object is required.
Unit vector of frame {B} are:
𝑋𝑋�𝐵𝐵 , 𝑌𝑌�𝐵𝐵 , 𝑍𝑍̂𝐵𝐵
Considering those unit vector relative to frame
{A} we have: 𝐴𝐴
𝑋𝑋𝐵𝐵 , 𝐴𝐴𝑌𝑌𝐵𝐵 , 𝐴𝐴𝑍𝑍𝐵𝐵

The rotation matrix will be defined as:

𝐵𝐵 𝑇𝑇
3∗1 3∗1 3∗1 𝑟𝑟11 𝑟𝑟12 𝑟𝑟13 𝑋𝑋�𝐵𝐵 ⋅ 𝑋𝑋�𝐴𝐴 𝑌𝑌�𝐵𝐵 ⋅ 𝑋𝑋�𝐴𝐴 𝑍𝑍̂𝐵𝐵 ⋅ 𝑋𝑋�𝐴𝐴 𝑋𝑋𝐴𝐴
𝐵𝐵 𝑇𝑇
𝐴𝐴
𝐵𝐵𝑅𝑅 = �
𝐴𝐴 � �
𝐴𝐴 � �
𝐴𝐴 ̂ = 𝑟𝑟21 𝑟𝑟22 𝑟𝑟23 = 𝑋𝑋�𝐵𝐵 ⋅ 𝑌𝑌�𝐴𝐴 𝑌𝑌�𝐵𝐵 ⋅ 𝑌𝑌�𝐴𝐴 𝑍𝑍̂𝐵𝐵 ⋅ 𝑌𝑌�𝐴𝐴 = 𝑌𝑌𝐴𝐴
𝑋𝑋𝐵𝐵 𝑌𝑌𝐵𝐵 𝑍𝑍𝐵𝐵 𝑟𝑟31 𝑟𝑟32 𝑟𝑟33 𝑋𝑋�𝐵𝐵 ⋅ 𝑍𝑍̂𝐴𝐴 𝑌𝑌�𝐵𝐵 ⋅ 𝑍𝑍̂𝐴𝐴 𝑍𝑍̂𝐵𝐵 ⋅ 𝑍𝑍̂𝐴𝐴 𝐵𝐵 𝑇𝑇
𝑍𝑍𝐴𝐴
 Orientation:
From linear algebra, the inverse of a matrix with orthogonal columns are the
same as its transpose. i.e.:
𝐵𝐵 𝐴𝐴 𝑇𝑇 𝐴𝐴 −1
𝐴𝐴 𝑅𝑅 = 𝐵𝐵 𝑅𝑅 = 𝐵𝐵𝑅𝑅 𝑋𝑋𝐵𝐵

For two Unit vector 𝑋𝑋𝐴𝐴 and 𝑋𝑋𝐵𝐵 with an angle 𝜃𝜃:
𝜃𝜃
𝑋𝑋𝐴𝐴 ⋅ 𝑋𝑋𝐵𝐵 = cos(𝜃𝜃)
𝑋𝑋𝐴𝐴
Then the rotation matrix will be as:
cos(𝑋𝑋�𝐵𝐵 , 𝑋𝑋�𝐴𝐴 ) cos(𝑌𝑌�𝐵𝐵 , 𝑋𝑋�𝐴𝐴 ) cos(𝑍𝑍̂𝐵𝐵 , 𝑋𝑋�𝐴𝐴 )
𝐴𝐴 � �
𝐵𝐵𝑅𝑅 = 𝑐𝑐𝑐𝑐𝑐𝑐(𝑋𝑋𝐵𝐵 , 𝑌𝑌𝐴𝐴 ) cos(𝑌𝑌�𝐵𝐵 , 𝑌𝑌�𝐴𝐴 ) cos(𝑍𝑍̂𝐵𝐵 , 𝑌𝑌�𝐴𝐴 )
cos(𝑋𝑋�𝐵𝐵 , 𝑍𝑍̂𝐴𝐴 ) cos(𝑌𝑌�𝐵𝐵 , 𝑍𝑍̂𝐴𝐴 ) cos(𝑍𝑍̂𝐵𝐵 , 𝑍𝑍̂𝐴𝐴 )

Direction Cosine matrix

 Example:
Find the rotation matrix of point “P” if its frame {B} is rotated 30∘ about the
z-axis relative to frame {A}.
1) By using direction cosine matrix, we have:
cos(𝑋𝑋𝐵𝐵 , 𝑋𝑋𝐴𝐴 ) cos(𝑌𝑌𝐵𝐵 , 𝑋𝑋𝐴𝐴 ) cos(𝑍𝑍𝐵𝐵 , 𝑋𝑋𝐴𝐴 )
𝐴𝐴
𝐵𝐵𝑅𝑅 = 𝑐𝑐𝑐𝑐𝑐𝑐(𝑋𝑋𝐵𝐵 , 𝑌𝑌𝐴𝐴 ) cos(𝑌𝑌𝐵𝐵 , 𝑌𝑌𝐴𝐴 ) cos(𝑍𝑍𝐵𝐵 , 𝑌𝑌𝐴𝐴 )
cos(𝑋𝑋𝐵𝐵 , 𝑍𝑍𝐴𝐴 ) cos(𝑌𝑌𝐵𝐵 , 𝑍𝑍𝐴𝐴 ) cos(𝑍𝑍𝐵𝐵 , 𝑍𝑍𝐴𝐴 )
cos(30) cos(120) cos(90) cos(30) −sin(30) 0
𝐴𝐴
𝐵𝐵𝑅𝑅 = 𝑐𝑐𝑐𝑐𝑐𝑐(60) cos(30) cos(90) = sin(30) cos(30) 0
cos(90) cos(90) cos(0) 0 0 1

𝝅𝝅
Note1: 𝑪𝑪𝑪𝑪𝑪𝑪 + 𝜽𝜽 = − 𝐬𝐬𝐬𝐬𝐬𝐬(𝜽𝜽)
𝟐𝟐

Note2: Positive rotation should follow the right-hand rule

 Example:
Find the rotation matrix of point “P” if its frame {B} is rotated 30∘ about the
z-axis relative to frame {A}.
2) Using 2D projection

𝐴𝐴 𝐴𝐴 𝐴𝐴
𝑋𝑋𝐵𝐵 𝑌𝑌𝐵𝐵 𝑍𝑍𝐵𝐵
cos(30) −sin(30) 0

𝑋𝑋𝐴𝐴 𝑌𝑌𝑌𝑌 𝑍𝑍𝑍𝑍

𝐴𝐴
𝐵𝐵𝑅𝑅 = 𝑠𝑠𝑠𝑠𝑠𝑠(30) cos(30) 0
0 0 1
 Exercise
Exercise1: Find the rotation matrix of point “P” if its frame {B} is rotated 30∘
about x-axis relative to frame {A}.

1 0 0
𝐴𝐴
𝐵𝐵𝑅𝑅 = 0 cos(30) −sin(30)
0 sin(30) cos(30)

Exercise2: Find the rotation matrix of point “P” if its frame {B} is rotated 30∘
about y-axis relative to frame {A}.

cos(30) 0 sin(30)
𝐴𝐴 0 1 0
𝐵𝐵𝑅𝑅 =
−sin(30) 0 cos(30)
Rotation matrices about x, y, and z :
1 0 0
𝐴𝐴
𝐵𝐵𝑅𝑅𝑥𝑥 (𝜃𝜃) = 0 cos(𝜃𝜃) −sin(𝜃𝜃)
0 sin(𝜃𝜃) cos(𝜃𝜃)

cos(𝜃𝜃) 0 sin(𝜃𝜃)
𝐴𝐴 0 1 0
𝐵𝐵𝑅𝑅𝑦𝑦 (𝜃𝜃) =
−sin(𝜃𝜃) 0 cos(𝜃𝜃)

cos(𝜃𝜃) −sin(𝜃𝜃) 0
𝐴𝐴
𝐵𝐵𝑅𝑅𝑧𝑧 (𝜃𝜃) = sin(𝜃𝜃) cos(𝜃𝜃) 0
0 0 1
 Frames:
Define the position and orientation of an object relative to coordinate frame {A} when
a new frame {B} has been assigned to the object.
𝐴𝐴
{𝐵𝐵} = { 𝐵𝐵𝐴𝐴𝑅𝑅 , 𝑃𝑃𝐵𝐵 }

3 Directional Vector (3*3) Position vector (3*1)

In order to simplify the rotation matrix and the position vector combination the
“Homogeneous transformation” is used.
• Working with square matrices will be simpler (Due to: Inversion,
multiplications, etc.)
Translation
𝐴𝐴 𝐴𝐴
𝐴𝐴
= 𝐵𝐵𝑅𝑅3∗3 𝑃𝑃𝐵𝐵 3∗1 Notes:
𝐵𝐵𝑇𝑇
0 0 0 1 4∗4
1. Three unit vectors on each frame for orientation and one
vector from one frame to another frame for position.
𝐴𝐴
𝑃𝑃𝐵𝐵
Transformation 2. Frame {B} is known relative to frame {A}, i.e. : A 𝐵𝐵
Frames relative to other frames:
Example1:
Describe frame {B} relative to frame {A} if the origin of {B} is positioned
at -2,3, and 5 in x, y, and z relative to frame {A}, and {B} is rotated
30° about z-axis relative to frame {A}.
c30 −s30 0 −2
𝐴𝐴
𝑇𝑇 = s 30 c 30 0 3
𝐵𝐵 0 0 1 5
0 0 0 1

Example2:
Describe frame {B} relative to frame {A} if the origin of {B} is positioned
at 5,-12, and -4 in x, y, and z relative to frame {A}, and {B} is rotated
45° about x-axis relative to frame {A}.
1 0 0 5
𝐴𝐴 0 c 45 −𝑠𝑠45 −12
𝐵𝐵𝑇𝑇 = 0 𝑠𝑠45 𝑐𝑐45 −4
0 0 0 1
Example3:
Describe frame {B} relative to frame {A} if the origin of {B} is positioned
at 7,0, and -1 in x, y, and z relative to frame {A}, and {B} is rotated
80° about y-axis relative to frame {A}.
c80 0 𝑠𝑠80 7
𝐴𝐴 0 1 0 0
𝐵𝐵𝑇𝑇 = −𝑠𝑠𝑠𝑠 0 𝑐𝑐80 −1
0 0 0 1
 Mapping
Definition:
Change the description of a point from being relative to a frame to be
relative to another frame.

Translation only:
The second frame translated without rotation relative to the first frame.
𝐴𝐴 𝐵𝐵 𝐴𝐴
𝑃𝑃 = 𝑃𝑃 + 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂
 Example:
Frame {B} is translated 10 units in 𝑋𝑋𝐴𝐴 and 5 units in 𝑌𝑌𝐴𝐴 . Find 𝐴𝐴𝑃𝑃 if 𝐵𝐵
𝑃𝑃
= 3 7 0 𝑇𝑇
3 10 13
𝐴𝐴 𝐵𝐵 𝐴𝐴
𝑃𝑃 = 𝑃𝑃 + 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂 = 7 + 5 = 12
0 0 0

Exercise:
Frame {B} is translated 12 units in 𝑋𝑋𝐴𝐴 and 7 units in 𝑌𝑌𝐴𝐴 and -3 units in 𝑍𝑍𝐴𝐴 .
Find 𝐴𝐴𝑃𝑃 if 𝐵𝐵𝑃𝑃 = −6 13 9 𝑇𝑇
−6 12 6
𝐴𝐴 𝐵𝐵 𝐴𝐴
𝑃𝑃 = 𝑃𝑃 + 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂 = 13 + 7 = 20
9 −3 6
Rotation Only:
The second frame is only rotated relative
to the first frame.

𝐴𝐴 𝐴𝐴 𝐵𝐵
𝑃𝑃 = 𝐵𝐵 𝑅𝑅 𝑃𝑃

Example:
Frame {B} is rotated relative to frame {A} about z-axis by 30° . Find 𝐴𝐴𝑃𝑃 if
𝐵𝐵
𝑃𝑃 = 0 2 0 𝑇𝑇
𝑐𝑐30 −𝑠𝑠𝑠𝑠 0 0 −1
𝐴𝐴
𝑃𝑃 = 𝐵𝐵𝐴𝐴𝑅𝑅 𝐵𝐵𝑃𝑃 = 𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐 0 2 = √3
0 0 1 0 0
 Exercise:
Frame {B} is rotated relative to frame {A} about y-axis by 60° . Find 𝐴𝐴𝑃𝑃 if
𝐵𝐵
𝑃𝑃 = 3 0 −5 𝑇𝑇
3 5 3
𝑐𝑐60 0 𝑠𝑠𝑠𝑠 3 −
2 2
𝐴𝐴
𝑃𝑃 = 𝐵𝐵𝐴𝐴𝑅𝑅 𝐵𝐵𝑃𝑃 = 0 1 0 0 = 0
−𝑠𝑠𝑠𝑠 0 𝑐𝑐𝑐𝑐 −5 3 3 5
− −
2 2
Translation and rotation:
The second frame is translated and rotated relative to the first frame.

𝐴𝐴
𝑃𝑃 = 𝐵𝐵𝐴𝐴𝑅𝑅 𝐵𝐵𝑃𝑃 + 𝐴𝐴𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂

𝐴𝐴
𝐴𝐴
𝑃𝑃 = 𝐵𝐵𝐴𝐴𝑅𝑅 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂 𝐵𝐵
𝑃𝑃 = 𝐴𝐴
𝐵𝐵
𝑃𝑃
𝐵𝐵𝑇𝑇
1 01∗3 1 1 1
 Example:
Frame {B} is rotated relative to frame {A} about z-axis by 30° , then translated 10 units in
𝑋𝑋𝐴𝐴 and 5 units in 𝑌𝑌𝐴𝐴 . Find the 𝐵𝐵𝐴𝐴𝑇𝑇. Find 𝐴𝐴𝑃𝑃 if 𝐵𝐵𝑃𝑃 = 3 7 0 𝑇𝑇

𝐴𝐴
𝑃𝑃 = 𝐵𝐵𝐴𝐴𝑅𝑅 𝐵𝐵𝑃𝑃 + 𝐴𝐴𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂

𝑐𝑐30 −𝑠𝑠𝑠𝑠 0 10 3
𝐴𝐴 𝐴𝐴 𝐴𝐴 𝐵𝐵
𝑃𝑃 = 𝐵𝐵𝑅𝑅 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂 𝑃𝑃 = 𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐 0 5 7
1 01∗3 1 1 0 0 1 0 0
0 0 0 1 1

0.866 −0.5 0 10 3 9.098

= 0.5 0.866 0 5 7 = 12.562
0 0 1 0 0 0
0 0 0 1 1 1

9.098
𝐴𝐴
𝑃𝑃 = 12.562
0
 Exercise:
Frame {B} is rotated relative to frame {A} about x-axis by 45° , then translated -12 units
in 𝑋𝑋𝐴𝐴 and 3 units in 𝑌𝑌𝐴𝐴 and 10 units in 𝑍𝑍𝐴𝐴 . Define frame {B} relative to frame {A} (Find the
𝐴𝐴 𝐵𝐵 𝑇𝑇
𝐴𝐴
𝐵𝐵 𝑇𝑇 ). Find 𝑃𝑃 if 𝑃𝑃 = −2 6 −5
𝐴𝐴
𝑃𝑃 = 𝐵𝐵𝐴𝐴𝑅𝑅 𝐵𝐵𝑃𝑃 + 𝐴𝐴𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂

1 0 0 −12 −2 −14
𝐴𝐴 𝐴𝐴 𝐴𝐴 𝐵𝐵
𝑃𝑃 = 𝐵𝐵𝑅𝑅 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂 𝑃𝑃 = 0 𝑐𝑐𝑐𝑐 −𝑠𝑠45 3 6 = 6𝑐𝑐𝑐𝑐 + 5𝑠𝑠𝑠𝑠 + 3
1 01∗3 1 1 0 𝑠𝑠45 𝑐𝑐45 10 −5 6𝑠𝑠𝑠𝑠 − 5𝑐𝑐𝑐𝑐 + 10
0 0 0 1 1 1

−14
𝐴𝐴
𝑃𝑃 = 6𝑐𝑐45 + 5𝑠𝑠45 + 3
6𝑠𝑠45 − 5𝑐𝑐45 + 10
 Operators
Definition: Translate and rotate a vector on the same frame

Translation Only: Translate a vector on a same frame

𝐴𝐴
𝑃𝑃2 = 𝐴𝐴𝑃𝑃1 + 𝐴𝐴𝑄𝑄
𝐴𝐴
𝑄𝑄 is the translation vector
 Example:
Vector 𝐴𝐴𝑃𝑃1 is translated 10 units in 𝑋𝑋𝐴𝐴 and 5 units in 𝑌𝑌𝐴𝐴 . Find the new resulting vector
𝐴𝐴
𝑃𝑃2 if the original vector 𝐴𝐴𝑃𝑃1 = 3 7 0 𝑇𝑇 .

3 10 13
𝐴𝐴
𝑃𝑃2 = 𝐴𝐴𝑃𝑃1 + 𝐴𝐴𝑄𝑄 = 7 + 5 = 12
0 0 0
Exercise:
Vector 𝐴𝐴𝑃𝑃1 is translated -7 units in 𝑋𝑋𝐴𝐴 and 11 units in 𝑌𝑌𝐴𝐴 and -4 in 𝑍𝑍𝐴𝐴 . Find the new
resulting vector 𝐴𝐴𝑃𝑃2 if the original vector 𝐴𝐴𝑃𝑃1 = 6 −3 −9 𝑇𝑇 .

6 −7 −1
𝐴𝐴
𝑃𝑃2 = 𝐴𝐴𝑃𝑃1 + 𝐴𝐴𝑄𝑄 = −3 + 11 = 8
−9 −4 −13
Rotational Operators:
Definition: Rotate of a vector on the same frame
𝐴𝐴 𝐴𝐴
𝑃𝑃2 = 𝑅𝑅𝑘𝑘 𝜃𝜃 𝑃𝑃1

𝑘𝑘 is the rotation axis and 𝜃𝜃 is the rotation angle.

 Example:
Vector 𝐴𝐴𝑃𝑃1 is rotated 30° about 𝑍𝑍𝐴𝐴 axis. Find the new resulting vector 𝐴𝐴𝑃𝑃2 if the original
vector 𝐴𝐴𝑃𝑃1 = 0 2 0 𝑇𝑇 .
−1
𝑐𝑐30 −𝑠𝑠30 0 0
𝐴𝐴 𝐴𝐴 3
𝑃𝑃2 = 𝑅𝑅𝑧𝑧 30 𝑃𝑃1 = 𝑠𝑠30 𝑐𝑐30 0 2 =
0 0 1 0 2
0
Exercise:
Vector 𝐴𝐴𝑃𝑃1 is rotated relative to frame {A} about 𝑦𝑦 −axis by 60° . Find 𝑅𝑅𝑦𝑦 60° ? Find
the new resulting vector 𝐴𝐴𝑃𝑃2 if the original vector 𝐴𝐴𝑃𝑃1 = 3 0 −5 𝑇𝑇 ?
𝑐𝑐60 0 𝑠𝑠𝑠𝑠 0.5 0 0.866
𝑅𝑅𝑦𝑦 60 = 0 1 0 = 0 1 0
−𝑠𝑠𝑠𝑠 0 𝑐𝑐𝑐𝑐 −0.866 0 0.5
𝑐𝑐60 0 𝑠𝑠𝑠𝑠 3 −2.86
𝐴𝐴
𝑃𝑃2 = 𝑅𝑅𝑦𝑦 60 𝐴𝐴𝑃𝑃1 = 0 1 0 0 = 0
−𝑠𝑠𝑠𝑠 0 𝑐𝑐𝑐𝑐 −5 −5.098
 Operators:
General Case: Translation and Rotation on a same frame:
𝐴𝐴 𝐴𝐴 𝐴𝐴
𝑃𝑃2 = 𝑅𝑅𝑘𝑘 𝜃𝜃 𝑃𝑃1 + 𝑄𝑄

𝑘𝑘 is the rotation axis and 𝜃𝜃 is the rotation angle.

𝐴𝐴
𝑄𝑄 is the translation vector.

Using Homogeneous transformation, we have:

𝐴𝐴 𝐴𝐴 𝐴𝐴 𝐴𝐴
𝑃𝑃2 = 𝑅𝑅𝑘𝑘 𝜃𝜃 𝑄𝑄 𝑃𝑃1 = 𝑇𝑇 𝑃𝑃1
1 01∗3 1 1 1
 Example:
Vector 𝐴𝐴𝑃𝑃1 is rotate relative to frame {A} about z-axis by 30° and translated
10 units in 𝑋𝑋𝐴𝐴 and 5 units in 𝑌𝑌𝐴𝐴 . Define the transformation matrix (T), then find
𝐴𝐴
𝑃𝑃2 if 𝐴𝐴𝑃𝑃1 = 3 7 0 𝑇𝑇

𝑐𝑐𝑐0 −𝑠𝑠𝑠𝑠 0 10
𝐴𝐴
𝑅𝑅𝑘𝑘 𝜃𝜃 𝑄𝑄
𝑇𝑇 = = 𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐 0 5
01∗3 1 0 0 1 0
0 0 0 1

𝑐𝑐30 −𝑠𝑠30 0 10 3 3𝑐𝑐30 − 7𝑠𝑠30 + 10 9.098

𝐴𝐴
𝑃𝑃2 = 𝑠𝑠30 𝑐𝑐30 0 5 7 = 3𝑠𝑠30 + 7𝑐𝑐30 + 5 = 12.562
1 0 0 1 0 0 0 0
0 0 0 1 1 1 1
 Exercise:
Vector 𝐴𝐴𝑃𝑃𝑃 is rotate relative to frame {A} about x-axis by 45° and translated -
12 units in 𝑋𝑋𝐴𝐴 and 3 units in 𝑌𝑌𝐴𝐴 and 10 units in 𝑍𝑍𝐴𝐴 . Define the transformation
matrix (T), then find 𝐴𝐴𝑃𝑃2 if 𝐴𝐴𝑃𝑃1 = −2 6 −5 𝑇𝑇

1 0 0 −12
𝐴𝐴
𝑅𝑅𝑘𝑘 𝜃𝜃 𝑄𝑄
𝑇𝑇 = = 0 𝑐𝑐45 −𝑠𝑠45 3
01∗3 1 0 𝑠𝑠45 𝑐𝑐45 10
0 0 0 1
1 0 0 −12 −2 −14
𝐴𝐴
𝑃𝑃2 = 0 𝑐𝑐45 −𝑠𝑠45 3 6 = 6𝑐𝑐45 + 5𝑠𝑠45 + 3
1 0 𝑠𝑠45 𝑐𝑐45 10 −5 6𝑠𝑠45 − 5𝑐𝑐45 + 10
0 0 0 1 1 1
−14
𝐴𝐴
𝑃𝑃2 = 6𝑐𝑐𝑐𝑐 + 5𝑠𝑠𝑠𝑠 + 3
6𝑠𝑠𝑠𝑠 − 5𝑐𝑐𝑐𝑐 + 10
 Transformation Arithmetic
Compound Transform:

𝑃𝑃 = 𝐴𝐴𝑇𝑇 𝐵𝐵𝑇𝑇 𝑐𝑐𝑃𝑃

𝐴𝐴
𝐵𝐵 𝐶𝐶
1 𝐴𝐴
1
𝐶𝐶𝑇𝑇
 Example:
Frame {B} is rotated relative to fame {A} about z axis by 30° and translated 4
units in x-axis and 3 units in y-axis. Frame {C} is rotated relative to frame {B}
about x-axis by 60° and translated 6 units in x-axis and 5 units in z-axis. Find
the position of point “P” relative to frame {A} if 𝐶𝐶 𝑃𝑃 = 8 7 9 𝑇𝑇

𝐴𝐴 𝑐𝑐 𝐴𝐴 𝐵𝐵 𝑐𝑐
𝑃𝑃 = 𝐴𝐴𝑇𝑇 𝐵𝐵𝑇𝑇 𝑃𝑃 = 𝑅𝑅𝑧𝑧 30 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂 𝑅𝑅𝑥𝑥 60 𝑃𝑃𝐶𝐶 𝑂𝑂𝑂𝑂𝑂𝑂 𝑃𝑃
=
𝐵𝐵 𝐶𝐶
1 𝐴𝐴
1 0 1∗3
1 01∗3 1 1
𝐶𝐶𝑇𝑇
𝑐𝑐30 −𝑠𝑠30 0 4 1 0 0 6 8 18.27
𝑠𝑠30 𝑐𝑐30 0 3 0 𝑐𝑐60 −𝑠𝑠60 0 7 = 6.28
0 0 1 0 0 𝑠𝑠60 𝑐𝑐60 5 9 15.56
0 0 0 1 0 0 0 1 1 1
 Inverting transformation:
 For Rotation matrices 𝐵𝐵𝐴𝐴𝑅𝑅 = 𝐵𝐵𝐴𝐴𝑅𝑅−1 = 𝐵𝐵𝐴𝐴𝑅𝑅𝑇𝑇 . This is valid only for rotation
matrices.

 For transformation matrices following formula can be used to calculate the

inverse:

𝐵𝐵
𝐴𝐴 𝑇𝑇
𝐵𝐵𝑅𝑅 − 𝐵𝐵𝐴𝐴𝑅𝑅𝑇𝑇 𝐴𝐴𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂
𝐴𝐴𝑇𝑇 = 𝐵𝐵𝐴𝐴𝑇𝑇 −1 = ≠ 𝐵𝐵𝐴𝐴𝑇𝑇 𝑇𝑇
01∗3 1

−1 −1
𝐴𝐴 𝐵𝐵
= 𝐴𝐴 −𝐴𝐴−1 𝐵𝐵𝐷𝐷−1
𝐶𝐶 𝐷𝐷 𝐶𝐶 𝐷𝐷 −1
 Example:
Frame {B} is rotated relative to frame {A} about z-axis by 30° and translated 4
units in x-axis and 3 units in y-axis. Find the transformation matrix that describes
frame {A} relative to frame {B}.
𝑐𝑐𝑐0 −𝑠𝑠𝑠𝑠 0 4
𝐴𝐴
𝑅𝑅 (30) 𝑃𝑃𝐵𝐵 𝑂𝑂𝑂𝑂𝑂𝑂
𝐴𝐴
𝐵𝐵𝑇𝑇 = 𝑧𝑧 = 𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐 0 3
01∗3 1 0 0 1 0
0 0 0 1
𝑇𝑇 𝑇𝑇
𝑐𝑐30 −𝑠𝑠30 0 𝑐𝑐30 −𝑠𝑠30 0 4
𝐵𝐵
= 𝐵𝐵𝐴𝐴𝑇𝑇 −1 = 𝑠𝑠30 𝑐𝑐30 0 − 𝑠𝑠30 𝑐𝑐30 0 3
𝐴𝐴𝑇𝑇
0 0 1 0 0 1 0
0 0 0 1

𝑐𝑐30 𝑠𝑠30 0 −4𝑐𝑐30 − 3𝑠𝑠30 0.866 0.5 0 −4.96

= −𝑠𝑠30 𝑐𝑐30 0 +4𝑠𝑠30 − 3𝑐𝑐30 = −0.5 0.866 0 −0.598
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
 Exercise:
𝑐𝑐30 0 𝑠𝑠𝑠0 2
Find 𝐵𝐵𝐴𝐴𝑇𝑇 if 𝐵𝐵𝐴𝐴𝑇𝑇 = 0 1 0 −5
−𝑠𝑠30 0 𝑐𝑐𝑐0 0
0 0 0 1

𝑇𝑇 𝑇𝑇
𝑐𝑐30 0 𝑠𝑠30 𝑐𝑐30 0 𝑠𝑠30 2
𝐵𝐵
= 𝐵𝐵𝐴𝐴𝑇𝑇 −1 = 0 1 0 − 0 1 0 −5
𝐴𝐴𝑇𝑇
−𝑠𝑠30 0 𝑐𝑐30 −𝑠𝑠30 0 𝑐𝑐30 0
0 0 0 1

𝑐𝑐30 0 −𝑠𝑠30 −2𝑐𝑐30 0.866 0 −0.5 −1.732

= 0 1 0 5 = 0 1 0 5
𝑠𝑠30 0 𝑐𝑐30 −2𝑠𝑠30 0.5 0 0.866 −1
0 0 0 1 0 0 0 1
 Transformation Equations:
How to derive the unknown transformation
using the other transformations.

𝑈𝑈
𝐷𝐷𝑇𝑇 = 𝑈𝑈𝐴𝐴𝑇𝑇 𝐷𝐷𝐴𝐴𝑇𝑇 and 𝑈𝑈
𝐷𝐷 𝑇𝑇 = 𝑈𝑈 𝐵𝐵 𝐶𝐶
𝐵𝐵𝑇𝑇 𝐶𝐶 𝑇𝑇 𝐷𝐷𝑇𝑇
𝑈𝑈 𝐴𝐴 𝑈𝑈 𝐵𝐵 𝐶𝐶
𝑇𝑇
𝐴𝐴 𝐷𝐷 𝑇𝑇= 𝐵𝐵𝑇𝑇 𝐶𝐶 𝑇𝑇 𝐷𝐷𝑇𝑇

𝐵𝐵
𝐶𝐶 𝑇𝑇 = 𝑈𝑈𝐵𝐵𝑇𝑇 −1 𝑈𝑈𝐴𝐴𝑇𝑇 𝐴𝐴 𝐶𝐶 −1
𝐷𝐷𝑇𝑇 𝐷𝐷𝑇𝑇
 Example1:
Find 𝐴𝐴𝐶𝐶𝑇𝑇 if the following transformation matrices are defined as:

0.866 0.5 0 −4.964 1 0 0 6

𝐵𝐵 −0.5 0.866 0 −0.598 𝐵𝐵 0 0.5 −0.866 0
𝐴𝐴 𝑇𝑇 = 𝐶𝐶 𝑇𝑇 =
0 0 1 0 0 0.866 0.5 5
0 0 0 1 0 0 0 1

𝐴𝐴 𝐴𝐴 𝐵𝐵
𝐶𝐶 𝑇𝑇 = 𝐵𝐵𝑇𝑇 𝐶𝐶 𝑇𝑇

𝐴𝐴
𝐶𝐶 𝑇𝑇 = 𝐵𝐵𝐴𝐴𝑇𝑇 −1 𝐵𝐵𝐶𝐶 𝑇𝑇
0.866 −0.5 0 4
𝐵𝐵 −1
𝐵𝐵 𝑇𝑇
𝐴𝐴𝑅𝑅 − 𝐵𝐵𝐴𝐴𝑅𝑅𝑇𝑇 𝐵𝐵𝑃𝑃𝐴𝐴 𝑂𝑂𝑂𝑂𝑂𝑂 0.5 0.866 0 3
𝐴𝐴𝑇𝑇 = =
01∗3 1 0 0 1 0
0 0 0 1
0.866 −0.5 0 4 1 0 0 6 0.866 −0.25 0.433 9.196
𝐴𝐴 0.5 0.866 0 3 0 0.5 −0.866 0 0.5 0.433 −0.75 6
𝐶𝐶 𝑇𝑇 = =
0 0 1 0 0 0.866 0.5 5 0 0.866 0.5 5
0 0 0 1 0 0 0 1 0 0 0 1
 Example2:
Assume that we know the transform 𝐵𝐵𝑇𝑇𝑇𝑇, which describes the frame at the manipulator’s fingertips
{T } relative to the base of the manipulator, {B}, that we know where the tabletop is located in
space relative to the manipulator’s base (because we have a description of the frame {S} that is
attached to the table as shown, 𝐵𝐵𝑆𝑆𝑇𝑇 ), and that we know the location of the frame attached to the
bolt lying on the table relative to the table frame—that is, 𝐺𝐺𝑆𝑆𝑇𝑇 . Calculate the position and
orientation of the bolt relative to the manipulator’s hand, 𝐺𝐺𝑇𝑇𝑇𝑇 .

𝐺𝐺 𝐵𝐵 −1 𝐵𝐵 𝑆𝑆
𝑇𝑇𝑇𝑇 = 𝑇𝑇 𝑇𝑇 𝑆𝑆𝑇𝑇 𝐺𝐺 𝑇𝑇
MATLAB Example