Sie sind auf Seite 1von 40

Analytical and Adaptive Dynamics in

Mechatronic Systems

Ch1: Kinematics

Dr. Hassan Diab


Spring 2019/2020
The Denavit-Hartenberg Representation
Methode one: Frames at the beginning of the links:
Definition of Frames :
 𝑧𝑖 is along the joint axis 𝑖.
 𝑂𝑖 is the origin where the common normal to 𝑧𝑖 and 𝑧𝑖+1
intersects 𝑧𝑖 . If 𝑧𝑖 and 𝑧𝑖+1 are intersecting, locate 𝑂𝑖 at the
intersection, and if they are parallel, locate 𝑂𝑖 in any
convenient position along 𝑧𝑖 .
 𝑥𝑖 is along the common normal between 𝑧𝑖 and 𝑧𝑖+1 through
𝑂𝑖 , or in the direction normal to the 𝑧𝑖 − 𝑧𝑖+1 plane if 𝑧𝑖 and
𝑧𝑖+1 intersect.
 𝑦𝑖 completes a right-hand frame.
 Assign {0} to match {1} when the first joint variable is zero.
For {N}, choose an origin location and 𝑥𝑁 direction freely,
but generally so as to cause as many linkage parameters as
possible to become zero.

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 2


Kinematics Revision
Definition of Parameters:
 𝑎𝑖 : distance from 𝑧𝑖 to 𝑧𝑖+1 along 𝑥𝑖 .
 𝛼𝑖 : the angle from 𝑧𝑖 to 𝑧𝑖+1 about 𝑥𝑖 .
 𝑑𝑖 : distance from 𝑥𝑖−1 to 𝑥𝑖 along 𝑧𝑖 . 𝑑𝑖 is variable if joint 𝑖 is
prismatic.
 𝜃𝑖 : the angle from 𝑥𝑖−1 to 𝑥𝑖 about 𝑧𝑖 . 𝜃𝑖 is variable if joint 𝑖 is
revolute.

Homogeneous Matrix:

cos 𝜃𝑖 − sin 𝜃𝑖 0 𝑎𝑖−1


𝑖−1 sin 𝜃𝑖 cos 𝛼𝑖−1 cos 𝜃𝑖 cos 𝛼𝑖−1 − sin 𝛼𝑖−1 −𝑑𝑖 sin 𝛼𝑖−1
𝑖 𝑇 =
sin 𝜃𝑖 sin 𝛼𝑖−1 cos 𝜃𝑖 sin 𝛼𝑖−1 cos 𝛼𝑖−1 𝑑𝑖 cos 𝛼𝑖−1
0 0 0 1

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 3


The Denavit-Hartenberg Representation
Methode Two: Frames at the end of the links:

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 4


The Denavit-Hartenberg Representation

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 5


The Denavit-Hartenberg Representation

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 6


Conventional Rotations

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 7


Description of an Orientation
The orientation of a body is described by attaching a coordinate system to the body {B} and then defining the relationship
between the body frame and the reference frame {A} using the rotation matrix.

The rotation matrix describing frame {B} relative to frame {A}

 r11 r12 r13 


A ˆ A ˆ A ˆ  r23 
B R [ X B , YB , Z B ]   r21
A
r22


 r31 r32 r33 

The rotation matrix describing frame {A} relative to frame {B}


T
 r11 r12 r13 
B
A R  BAR T [ B Xˆ A , B YˆA , B Zˆ A ]  
 r21 r22 r23 


 r31 r32 r33 

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 8


The Rotation Matrix
 r11 r12 r13  𝑅 is Orthonormal matrix:
A
R  r r22 r23  • The magnitude of all line and vector is = 1.
B  21 

 r31 r32 r33 
 𝑟11 2 + 𝑟12 2 + 𝑟13 2 = 1 𝑟11 2 + 𝑟21 2 + 𝑟31 2 = 1
𝑟21 2 + 𝑟22 2 + 𝑟23 2 = 1 𝑟12 2 + 𝑟22 2 + 𝑟32 2 = 1
B
R  BAR 1  BART 𝑟31 2 + 𝑟32 2 + 𝑟33 2 = 1 𝑟13 2 + 𝑟23 2 + 𝑟33 2 = 1
A

• The dot product between any two lines or any two


1 0 0 columns is 0 .
A
R 1 A
R  A
R T A
R  I  0 1 0 [ r11 , r12 , r13 ]  [ r21 , r22 , r23 ]T  0
B B B B  

0 0 1
 [ r11 , r12 , r13 ]  [ r31 , r32 , r33 ]T  0
[ r21 , r22 , r23 ]  [ r31 , r32 , r33 ]T  0
[ r11 , r21 , r31 ]  [ r12 , r22 , r32 ]T  0
det 𝑅 = +1
[ r11 , r21 , r31 ]  [ r13 , r23 , r33 ]T  0
[ r12 , r22 , r32 ]  [ r13 , r23 , r33 ]T  0

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 9


The Basic Rotations
The rotation matrices with respect to the reference frame are defined as follows:

1 0 0 
RX ( )  0 c  s 
 
0 s
 c 
 c 0 s 
RY (  )   0 1 0 
 
  s 0 c 
 
c  s 0
RZ ( )   s c 0
 

 0 0 1

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 10


The Basic Rotations

X-Y-Z Fixed Angles

The rotations perform about an axis of a fixed reference frame

Z-Y-X Euler Angles

The rotations perform about an axis of a moving reference frame

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 11


X-Y-Z Fixed Angles
Start with frame {B} coincident with a known reference frame {A}.


• Rotate frame {B} about X̂ by an angle 
A
• Rotate frame {B} about YˆA by an angle  Fixed Angles

• Rotate frame {B} about Ẑ A by an angle 


Note - Each of the three rotations takes place about an axis in the fixed reference
frame {A}

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 12


X-Y-Z Fixed Angles
Compound Rotations Represented Globally

When successive rotations are described with respect to a single fixed


(global) frame, we may combine these rotations by pre-multiplication.
3 2 1

 c  s 0  c 0 s  1 0 0 
 0  0 1 0  0 c   s 
B R XYZ ( ,  ,  )  RZ ( ) RY (  ) R X ( )   s c
A
  
 0 0 1   s 0 c  0 s c 

cc css  sc csc  ss 


 ssc  cs 
B R XYZ ( ,  ,  )   sc sss  cc
A

  s cs cc 

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 13


X-Y-Z Fixed Angles
 r11 r12 r13  cc css  sc csc  ss 
A
R ( ,  ,  )  r r r23    sc sss  cc ssc  cs 
B XYZ  21 22   
 r31 r32 r33    s cs cc 

  Atan2( -r31, r112  r212 ) for - 90o    90o


  Atan2( r21 /cβ ,r11 /cβ )
  Atan2( r32 /cβ ,r33 /cβ )

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 14


Z-Y-X Euler Angles
Start with frame {B} coincident with a known reference frame {A}.


• Rotate frame {B} about Ẑ A by an angle 
ˆ
• Rotate frame {B} about Y B by an angle  Euler Angles

• Rotate frame {B} about X̂ B by an angle
Note - Each rotation is preformed about an axis of the moving reference frame {B},
rather then a fixed reference frame {A}.

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 15


Z-Y-X Euler Angles
Compound Rotations Represented Locally
We may combine successive rotations by post-multiplication if each rotation is about a
vector represented in the current rotated frame.

1 2 3

c  s 0  c 0 s  1 0 0 
A
R ( ,  ,  )  R ( ) R (  ) R ( )   s c 0  0 1 0  0 c  s 
B Z 'Y ' X ' Z Y X 
 0 0 1  s 0 c  0 s c 

cc css  sc csc  ss 


A
R ( ,  ,  )   sc sss  cc ssc  cs 
B Z 'Y ' X ' 
  s cs cc 

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 16


Fixed Angles Versus Euler Angles

A
B RXYZ ( ,  ,  ) BARZ 'Y ' X ' ( ,  ,  )

Fixed Angles Euler Angles


XYZ ZYX

Three rotations taken about fixed axes (Fixed Angles - XYZ) yield the
same final orientation as the same three rotation taken in an opposite
order about the axes of the moving frame (Euler Angles ZYX)

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 17


Rotation matrix about any axe
Rotation R(n,∅) is a rotation about the axe that has a unit directional vector n with an angle ∅:

r‘ is the rotated vector of r by the rotation R(n,∅)


(1− cos ∅) (𝑛𝑥 )2 + cos ∅ (1− cos ∅) 𝑛𝑥 𝑛𝑦 −𝑛𝑧 sin ∅ (1− cos ∅) 𝑛𝑥 𝑛𝑧 +𝑛𝑦 sin ∅
𝑅 𝒏, ∅ = (1− cos ∅) 𝑛𝑥 𝑛𝑦 +𝑛𝑧 sin ∅ (1− cos ∅) (𝑛𝑦 )2 + cos ∅ (1− cos ∅) 𝑛𝑦 𝑛𝑧 −𝑛𝑥 sin ∅
(1− cos ∅) 𝑛𝑥 𝑛𝑧 −𝑛𝑦 sin ∅ (1− cos ∅) 𝑛𝑦 𝑛𝑧 +𝑛𝑥 sin ∅ (1− cos ∅) (𝑛𝑧 )2 + cos ∅

𝑅 𝑅
𝑻

Trace 𝑅 − 1 𝑟11 − cos ∅ 𝑟22 − cos ∅ 𝑟33 − cos ∅


∅= cos−1 𝒏= ∓ ∓ ∓
2 1− cos ∅ 1− cos ∅ 1− cos ∅

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 18


VELOCITY
• ABSOLUTE METHOD:
0 𝑑 0
𝒗𝑃 = 𝒓𝑃 where 0𝒓𝑃 is the position vector of point P in frame {0}
𝑑𝑡
0𝒓 = 𝑛0𝑇 𝑛𝒓𝑃
𝑃
• VELOCITY "PROPAGATION" METHOD
𝑖+1𝝎
𝑖+1 =
𝑖+1 𝑖 ሶ 𝑖+1𝒁
𝑖𝑅 𝝎𝑖 + 𝜃𝑖+1
෡ 𝑖+1
𝑖+1𝒗
𝑖+1 = 𝑖+1 𝑖 𝑖
𝑖𝑅 𝒗𝑖 + 𝝎𝑖 × 𝒓𝑃
𝑖
+ 𝑑ሶ 𝑖+1 𝑖+1𝒁
෡ 𝑖+1
𝑖+1
With,
𝑖+1𝒁 ෡ 𝑖+1 = 0 0 1 𝑇
𝜃ሶ 𝑖+1 = 0 for prismatic joint and 𝑑ሶ 𝑖+1 = 0 for revolute joint
0𝒗 = 𝑛0𝑅 𝑛𝒗𝑛 and 0𝝎𝑛 = 𝑛0𝑅 𝑛𝝎𝑛
𝑛
𝑃 is 𝑂𝑛

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 19


JACOBIAN
• FROM THE VELOCITY OF END-EFFECTOR:

0𝒗
𝑝
0 = 0𝐽(𝑞)𝑞ሶ
𝝎𝑝

𝐴
𝐴 𝐵𝑅 0 𝐵
𝐽 𝜃 = 𝐴 𝐽(𝜃)
0 𝐵𝑅

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 20


JACOBIAN
• FROM THE POSITION OF END-EFFECTOR:

𝜕𝑥 𝜕𝑥 𝜕𝑥
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝜕𝑦 𝜕𝑦 𝜕𝑦

𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
where 0𝒓𝑃 = 𝑥 𝑦 𝑧 𝑇
𝜕𝑧 𝜕𝑧 𝜕𝑧
Is the position vector of End-Effector in fixed frame.
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝑱 = and 0𝜽𝑃 = 𝜃𝑥 𝜃𝑦 𝜃𝑧 𝑇
𝜕𝜃𝑥 𝜕𝜃𝑥 𝜕𝜃𝑥
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 Is the orientation of the End-Effector around
𝜕𝜃𝑦 𝜕𝜃𝑦 𝜕𝜃𝑦 𝑥 0 , 𝑦 0 and 𝑧 0 .

𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝜕𝜃𝑧 𝜕𝜃𝑧 𝜕𝜃𝑧
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 21


EXERCISE 1: ROTATION MATRIX
• Are these Rotation matrices? - det(R)=1
1 1 1
- All rows and columns
R have magnitude 1 1 1 0 0 0
0 0 0 5 - Each 2 rows and each 2
3
det(R)=0: This is
0 0 0 −2
columns are 1 0 0.5 − 0
not a Rotation orthogonal (dot 2
Matrix 0 0 0 3 product zero)
1 0
3
0.5 0
This is a Rotation Matrix
0 0 0 1 2
R 0 0 0 1
R
2 2
0 0 det(R)=0.29 0.2 −0.5 0 −2
det(R)=0: This is 2 2
not a Rotation 0 1 0 5 This is not a Rotation 0.5 0.2 0 −1
Matrix 2 2 Matrix 0 0 1 0
0 1
2 2 0 0 0 1
0 0 0 1

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 22


EXERCISE 2: ROTATION MATRIX
• For the Rotation below find the matrix that describe it.
• What will be the axis of rotation and the angle of rotation if we want to replace this rotation by only one rotation?
A
B RXYZ (  30,   90,   60)

3/24/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 23


EXERCISE 2: ROTATION MATRIX
A
B RXYZ (  30,   90,   60)

cc cs s  sc cs c  ss 


A
R ( ,  ,  )   sc ss s  cc ss c  cs 
B XYZ 
  s c s c c 
3
cc
0 cs s  sc −0.5
cs c  ss 
2
 
B R XYZ ( ,  ,  )   s0c ss s 3 cc ss c 0.5
 cs 
A
2
 −1s c s0 c c0 

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 24


EXERCISE 2: ROTATION MATRIX
 0 −0.5 3

 
2

B R ( n,  )  
A 3

0
2 0.5

 −1 0 0 

(1− cos ∅) (𝑛𝑥 )2 + cos ∅ (1− cos ∅) 𝑛𝑥 𝑛𝑦 −𝑛𝑧 sin ∅ (1− cos ∅) 𝑛𝑥 𝑛𝑧 +𝑛𝑦 sin ∅
𝑅 𝒏, ∅ = (1− cos ∅) 𝑛𝑥 𝑛𝑦 +𝑛𝑧 sin ∅ (1− cos ∅) (𝑛𝑦 )2 + cos ∅ (1− cos ∅) 𝑛𝑦 𝑛𝑧 −𝑛𝑥 sin ∅
(1− cos ∅) 𝑛𝑥 𝑛𝑧 −𝑛𝑦 sin ∅ (1− cos ∅) 𝑛𝑦 𝑛𝑧 +𝑛𝑥 sin ∅ (1− cos ∅) (𝑛𝑧 )2 + cos ∅

3
Trace 𝑅−1 0+ 2 +0−1
∅ = cos−1 = cos −1 =93.841°
2 2

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 25


EXERCISE 2: ROTATION MATRIX
3
(1− cos ∅) (𝑛𝑥 )2 + cos ∅ (1− cos ∅) 𝑛𝑥 𝑛𝑦 −𝑛𝑧 sin ∅ (1− cos ∅) 𝑛𝑥 𝑛𝑧 +𝑛𝑦 sin ∅ 0 −0.5 2
𝑅 𝒏, ∅ = (1− cos ∅) 𝑛𝑥 𝑛𝑦 +𝑛𝑧 sin ∅ (1− cos ∅) (𝑛𝑦 )2 + cos ∅ (1− cos ∅) 𝑛𝑦 𝑛𝑧 −𝑛𝑥 sin ∅ = 3
0 0.5
(1− cos ∅) 𝑛𝑥 𝑛𝑧 −𝑛𝑦 sin ∅ (1− cos ∅) 𝑛𝑦 𝑛𝑧 +𝑛𝑥 sin ∅ (1− cos ∅) (𝑛𝑧 )2 + cos ∅ 2
−1 0 0
𝑟11 −cos ∅ 0−cos 93.841
𝑛𝑥 = ∓ =∓ = ∓0.25 the sign of is opposite to the sign of 𝑛𝑥 × 𝑛𝑦 which means if 𝑛𝑥
1− cos ∅ 1− cos 93.841
and 𝑛𝑦 of the same sign 𝑛𝑧 is negative and if 𝑛𝑥 and 𝑛𝑦 of the
𝑟22 −cos ∅
3
−cos 93.841 opposite sign 𝑛𝑧 is positive.
2
𝑛𝑦 = ∓ = ∓ 1− cos 93.841 = ∓0.935 Try 𝑛𝑥 and 𝑛𝑦 of the same sign;
1− cos ∅
𝑛𝑧 = −1.06939 × 0.25 × 0.935 = −0.25 accepted.
33𝑟 −cos ∅ 0−cos 93.841 or
𝑛z = ∓ 1− = ∓ 1− cos 93.841 = ∓0.25
cos ∅ 𝑛𝑧 = −1.06939 × −0.25 × −0.935 = −0.25 accepted.
Try 𝑛𝑥 and 𝑛𝑦 of the opposite sign;
To check the sign of 𝑛𝑥 , 𝑛𝑦 and 𝑛𝑧 we have to 𝑛𝑧 = −1.06939 × 0.25 × −0.935 = +0.25 accepted.
test them in the other cells. or
𝑟21 = (1− cos ∅) 𝑛𝑥 𝑛𝑦 +𝑛𝑧 sin ∅=0 𝑛𝑧 = −1.06939 × −0.25 × 0.935 = +0.25 accepted.
1− cos 93.841
𝑛𝑧 =−𝑛𝑥 𝑛𝑦 × sin 93.841 = −1.06939 𝑛𝑥 𝑛𝑦
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 26
EXERCISE 2: ROTATION MATRIX
𝑟31 = (1− cos ∅) 𝑛𝑥 𝑛𝑧 −𝑛𝑦 sin ∅ = −1 𝑟12 = (1− cos ∅) 𝑛𝑥 𝑛𝑦 −𝑛𝑧 sin ∅ = −0.5
1+ 1− cos 93.841 𝑛𝑥 𝑛𝑧 0.5+ 1− cos 93.841 𝑛x 𝑛y
𝑛𝑦 = = 1.00225 + 1.06939 𝑛𝑥 𝑛z 𝑛𝑧 = = 0.501 + 1.06939 𝑛x 𝑛y
sin 93.841 sin 93.841
Try 𝑛𝑥 =+0.25 and 𝑛𝑧 = +0.25; Try 𝑛𝑥 =+0.25 and 𝑛𝑦 = +0.935;
𝑛𝑦 = 1.00225 + 1.06939 × 0.25 × 0.25 = 1.069 Not 𝑛𝑧 = 0.501 + 1.06939 × 0.25 × 0.935 = 0.75 Not
accepted. accepted.
Try 𝑛𝑥 = − 0.25 and 𝑛𝑧 = −0.25; Try 𝑛𝑥 = − 0.25 and 𝑛𝑦 = −0.935;
𝑛𝑦 = 1.00225 + 1.06939 × −0.25 × −0.25 = 1.069 Not 𝑛𝑧 = 0.501 + 1.06939 × −0.25 × −0.935 = 0.75 Not
accepted. accepted.
Try 𝑛𝑥 =+0.25 and 𝑛𝑧 = −0.25; Try 𝑛𝑥 =+0.25 and 𝑛𝑦 = −0.935;
𝑛𝑦 = 1.00225 + 1.06939 × 0.25 × −0.25 = +0.935 𝑛𝑧 = 0.501 + 1.06939 × 0.25 × −0.935 = +0.25 accepted.
accepted. Try 𝑛𝑥 = −0.25 and 𝑛𝑦 = +0.935;
Try 𝑛𝑥 =−0.25 and 𝑛𝑧 = +0.25; 𝑛𝑧 = 0.501 + 1.06939 × −0.25 × +0.935 = +0.25
𝑛𝑦 = 1.00225 + 1.06939 × −0.25 × +0.25 = +0.935 accepted.
accepted. So we deduce that 𝒏𝒙 and 𝒏𝒚 should be of the opposite sign
So we deduce that 𝒏𝒙 and 𝒏𝒛 should be of the opposite ֜ 𝒏𝒛 should be positive (1st test) ֜ 𝒏𝒙 should be negative
sign.
(2nd test) ֜ 𝒏𝒚 should be positive (3rd test) .
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 27
EXERCISE 2: ROTATION MATRIX
−0.25
𝑅 𝒏 = 0.935 , ∅ = 93.841° = RXYZ (  30,   90,   60)
0.25

• Notice that the magnitude of n is equal to 1 because it


is a unit direction vector that define the axe of rotation.

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 28


EXERCISE 3 :𝑥 KINEMATICS – METHOD 1
2

𝑦2
𝑦1 0 0 0 𝜃1
0 𝐿1 0 𝜃2

𝑥1 cos 𝜃𝑖 − sin 𝜃𝑖 0 𝑎𝑖−1


𝑖−1 sin 𝜃𝑖 cos 𝛼𝑖−1 cos 𝜃𝑖 cos 𝛼𝑖−1 − sin 𝛼𝑖−1 −𝑑𝑖 sin 𝛼𝑖−1
𝑖 𝑇 =
sin 𝜃𝑖 sin 𝛼𝑖−1 cos 𝜃𝑖 sin 𝛼𝑖−1 cos 𝛼𝑖−1 𝑑𝑖 cos 𝛼𝑖−1
0 0 0 1
𝐶𝜃1 −𝑆𝜃1 0 0 𝐶𝜃2 −𝑆𝜃2 0 𝐿1 𝐶1 𝐶2 − 𝑆1 𝑆2 −𝐶1 𝑆2 − 𝑆1 𝐶2 0 𝐿1 𝐶1
0 𝑆𝜃1 𝐶𝜃1 0 0 1𝑇 = 𝑆𝜃2 𝐶𝜃2 0 0 02𝑇 = 01𝑇 12𝑇 = 𝑆1 𝐶2 + 𝐶1 𝑆2 −𝑆1 𝑆2 + 𝐶1 𝐶2 0 𝐿1 𝑆1
1𝑇 = 2
0 0 1 0 0 0 1 0
0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
cos 𝜃1 + 𝜃2 − sin 𝜃1 + 𝜃2 0 𝐿1 cos 𝜃1
𝐶1 𝐶2 − 𝑆1 𝑆2 = cos 𝜃1 + 𝜃2 0 sin 𝜃1 + 𝜃2 cos 𝜃1 + 𝜃2 0 𝐿1 sin 𝜃1
Since ֜ 2𝑇 =
𝑆1 𝐶2 + 𝐶1 𝑆2 = sin 𝜃1 + 𝜃2 0 0 1 0
0 0 0 1
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 29
EXERCISE𝑥3 : KINEMATICS – METHOD 2
2

𝑦2 0 𝐿1 0 𝜃1
0 𝐿2 0 𝜃2
𝑦1
𝑥1

𝐶1 𝐶2 − 𝑆1 𝑆2 −𝐶1 𝑆2 − 𝑆1 𝐶2 0 𝐿2 (𝐶1 𝐶2 − 𝑆1 𝑆2 ) + 𝐿1 𝐶1
𝑆 𝐶 + 𝐶1 𝑆2 −𝑆1 𝑆2 + 𝐶1 𝐶2 0 𝐿2 (𝑆1 𝐶2 − 𝐶1 𝑆2 ) + 𝐿1 𝑆1
𝐴02 = 𝐴10 𝐴12 = 1 2
0 0 1 0
0 0 0 1
𝐶1 𝐶2 − 𝑆1 𝑆2 = cos 𝜃1 + 𝜃2
Since
𝐶1 −𝑆1 0 𝐿1 𝐶1 𝐶2 −𝑆2 0 𝐿2 𝐶2 𝑆1 𝐶2 + 𝐶1 𝑆2 = sin 𝜃1 + 𝜃2
𝑆 𝐶1 0 𝐿1 𝑆1 𝑆 𝐶2 0 𝐿2 𝑆2
𝐴10 = 1 𝐴12 = 2 cos 𝜃1 + 𝜃2 − sin 𝜃1 + 𝜃2 0 𝐿2 cos 𝜃1 + 𝜃2 +𝐿1 cos 𝜃1
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 𝐴02 = sin 𝜃1 + 𝜃2 cos 𝜃1 + 𝜃2 0 𝐿2 sin 𝜃1 + 𝜃2 + 𝐿1 sin 𝜃1
0 0 1 0
0 0 0 1
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 30
EXERCISE𝑥 3 : KINEMATICS – VERIFICATION
2

𝑷 is a specific point located at the end effector, to verify that both methods are true
𝑦2 We will find 𝑷𝟎 the position vector of P in the fixed frame. It should be the same.
𝑃
𝐿2 0
𝑥2 𝑷𝟐 = 0 𝑷𝟐 = 0
𝑦2 0 0

cos 𝜃1 + 𝜃2 − sin 𝜃1 + 𝜃2 0 𝐿1 cos 𝜃1 𝐿2 𝐿2 cos 𝜃1 + 𝜃2 +𝐿1 cos 𝜃1


0
𝑷𝟎 = 02𝑇 𝑷𝟐 = sin 𝜃1 + 𝜃2 cos 𝜃1 + 𝜃2 0 𝐿1 sin 𝜃1 = 𝐿2 sin 𝜃1 + 𝜃2 + 𝐿1 sin 𝜃1
0 0 1 0 0 0
0 0 0 1 1 1
cos 𝜃1 + 𝜃2 − sin 𝜃1 + 𝜃2 0 𝐿2 cos 𝜃1 + 𝜃2 +𝐿1 cos 𝜃1 0 𝐿2 cos 𝜃1 + 𝜃2 +𝐿1 cos 𝜃1
0
𝑷𝟎 = 𝐴02 𝑷𝟐 = sin 𝜃1 + 𝜃2 cos 𝜃1 + 𝜃2 0 𝐿2 sin 𝜃1 + 𝜃2 + 𝐿1 sin 𝜃1 = 𝐿2 sin 𝜃1 + 𝜃2 + 𝐿1 sin 𝜃1
0 0 1 0 0 0
0 0 0 1 1 1

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 31


EXERCISE 3 : KINEMATICS – JACOBIAN: 1 1
2𝑅 𝒓𝑃 2
METHOD1 𝑥 𝝎 = 0 and 𝒗 = 0
𝑛
0
0
0
0 𝐶𝜃2 −𝑆𝜃2 0 𝐿1
𝑖+1 ෡ 𝑆𝜃2 𝐶𝜃2 0 0
𝒁𝑖+1 = 0 0 1 𝑇 Always. 1
𝑦𝑛 𝑃 2𝑇 =
𝜃ሶ 𝑖+1 = 0 for prismatic joint and 0 0 1 0
𝑥2 𝑑ሶ 𝑖+1 = 0 for revolute joint 0 0 0 1
𝑦2
1𝝎 = 1𝑅 0𝝎 + 𝜃ሶ 1𝒁 ෡1 = 0 + 0 0 𝜃ሶ1 𝑇 𝐶2 𝑆2 0
1 0 0 1 2 1 𝑇
𝑦1 1𝒗 = 1𝑅 0𝒗 + 0𝝎 × 0𝒓 ሶ 1෡ 1𝑅= 2𝑅 = −𝑆2 𝐶2 0
1 0 0 0 𝑃 1 + 𝑑1 𝒁1 = 0 + 0
0 0 1
𝐶2 𝑆2 0 0 0
𝑥1 2𝝎 = 2𝑅 1𝝎 + 𝜃ሶ 2𝒁
2 1 1 2
෡ 2 = −𝑆2 𝐶2 0 0 + 0 0 𝜃ሶ 2 𝑇 = 0
0 0 1 𝜃ሶ1 𝜃ሶ1 + 𝜃ሶ 2
𝐶2 𝑆2 0 0 𝐿1
2
𝒗2 = 2
1𝑅
1
𝒗1 1
+ 𝝎1 × 𝒓𝑃 21
+ 𝑑ሶ 2 𝒁2 = −𝑆2
2෡
𝐶2 0 0+ 0 × 0 +0
0 0 1 𝜃ሶ1 0
𝐶2 𝑆2 0 0 𝐿1 𝑆2 𝜃ሶ1
= −𝑆2 𝐶2 0 𝐿1 𝜃ሶ1 = 𝐿1 𝐶2 𝜃ሶ1
0 0 1 0 0

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 32


EXERCISE 3 : KINEMATICS – JACOBIAN: 𝑅 𝒓 1
2
1
𝑃2

METHOD1 𝑥 (𝑃, 𝑥 , 𝑦 ) is the End-Effector frame


𝑛 𝑛
oriented as the last joint frame.
𝑛
1 0 0 𝐿 2 1 0 0
2 0
1 0 0 𝑛 2 𝑇
2𝑅= 𝑛𝑅 = 0 1 0
𝑛𝑇 =
𝑦𝑛 𝑃 0
0 1 0 0 0 1
0
0 0 1
𝑥2
1 0 0 0 0
𝑦2
𝑦1
𝑛
𝝎𝑛 = 𝑛 2
2 𝑅 𝝎2 + 𝜃ሶ𝑛 𝒁2 = 0
2෡
1 0 0 +0= 0
0 0 1 𝜃ሶ1 + 𝜃ሶ2 𝜃ሶ1 + 𝜃ሶ 2

𝑥1 1 0 0 𝐿1 𝑆2 𝜃ሶ1 0 𝐿2
𝑛
𝒗𝑛 = 𝑛2𝑅 2
𝒗2 + 2𝝎2 × 2𝒓𝑃 𝑛 ሶ 2෡
+ 𝑑𝑛 𝒁2 = 0 1 0 𝐿1 𝐶2 𝜃ሶ1 + 0 × 0 +0
0 0 1 0 𝜃ሶ1 + 𝜃ሶ2 0

1 0 0 𝐿1 𝑆2 𝜃ሶ1 0 𝐿1 𝑆2 𝜃ሶ1
= 0 1 0 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ 2 ) = 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ 2 )
0 0 1 0 0 0

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 33


EXERCISE 3 : KINEMATICS – JACOBIAN:
METHOD1 𝑛
0
0
2
𝐿1 𝑆2 𝜃ሶ1
𝒗2 = 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ2 ) 0𝒗 = 𝑛0𝑅 𝑛𝒗𝒏
𝝎𝑛 = 0𝝎
𝑛 = 02𝑅 𝑛𝝎𝑛 𝑛
𝜃ሶ1 + 𝜃ሶ2 0
Bcause 𝑛2𝑅 = 𝐼 Bcause 𝑛2𝑅 = 𝐼
0
2𝑅
cos 𝜃1 + 𝜃2 − sin 𝜃1 + 𝜃2 0 𝐿1 cos 𝜃1
0 sin 𝜃1 + 𝜃2 cos 𝜃1 + 𝜃2 0 𝐿1 sin 𝜃1
2𝑇 =
0 0 1 0
0 0 0 1

cos 𝜃1 + 𝜃2 − sin 𝜃1 + 𝜃2 0 0 0
0 0 0
𝝎𝑛 = sin 𝜃1 + 𝜃2 cos 𝜃1 + 𝜃2 0 =
0 0 1 𝜃ሶ1 + 𝜃ሶ 2 𝜃ሶ1 + 𝜃ሶ 2
C 𝜃1 + 𝜃2 − S 𝜃1 + 𝜃2 0 𝐿1 𝑆2 𝜃ሶ1 𝐿1 𝑆2 𝜃ሶ1 C 𝜃1 + 𝜃2 − 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ 2 ) 𝑆 𝜃1 + 𝜃2
0𝒗 =
𝑛 S 𝜃1 + 𝜃2 C 𝜃1 + 𝜃2 0 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ2 ) = 𝐿1 𝑆2 𝜃ሶ1 S 𝜃1 + 𝜃2 + 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ2 ) C 𝜃1 + 𝜃2
0 0 1 0 0

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 34


EXERCISE 3 : KINEMATICS – JACOBIAN:
METHOD1 𝝎 = 00 𝐿 𝑆 𝜃ሶ C 𝜃 + 𝜃 − 𝐿 𝐶 𝜃ሶ + 𝐿 (𝜃ሶ + 𝜃ሶ ) 𝑆 𝜃 + 𝜃
0
𝑛
0
1 2 1
𝒗 = 𝐿 𝑆 𝜃ሶ S 𝜃 + 𝜃 + 𝐿 𝐶 𝜃ሶ + 𝐿 (𝜃ሶ + 𝜃ሶ ) C 𝜃 + 𝜃
𝑛
1 2 1 2 1 2 1 2 1 2

1 2 1 1 2 1 2 1 2 1 2 1 2
𝜃ሶ1 + 𝜃ሶ 2
0
0
𝒗𝑛 0
0𝝎 = 𝐽(𝑞)𝑞ሶ
𝑛

𝐿1 𝑆2 𝜃ሶ1 C 𝜃1 + 𝜃2 − 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ2 ) 𝑆 𝜃1 + 𝜃2


𝐿1 𝑆2 𝜃ሶ1 S 𝜃1 + 𝜃2 + 𝐿1 𝐶2 𝜃ሶ1 + 𝐿2 (𝜃ሶ1 + 𝜃ሶ 2 ) C 𝜃1 + 𝜃2
0 𝜃ሶ1
=
0 𝜃ሶ 2
0
𝜃ሶ1 + 𝜃ሶ 2
𝐿1 (𝑆𝜃2 C 𝜃1 + 𝜃2 − 𝐶𝜃2 S 𝜃1 + 𝜃2 ) − 𝐿2 𝑆 𝜃1 + 𝜃2 −𝐿2 𝑆 𝜃1 + 𝜃2 −𝐿1 𝑆𝜃1 − 𝐿2 𝑆 𝜃1 + 𝜃2 −𝐿2 𝑆 𝜃1 + 𝜃2
𝐿1 (𝑆𝜃2 S 𝜃1 + 𝜃2 + 𝐶𝜃2 C 𝜃1 + 𝜃2 ) + 𝐿2 C 𝜃1 + 𝜃2 𝐿2 C 𝜃1 + 𝜃2 𝐿1 𝐶𝜃1 + 𝐿2 C 𝜃1 + 𝜃2 𝐿2 C 𝜃1 + 𝜃2
0
𝐽 𝑞 = 0 0 = 0 0
0 0 0 0
0 0 0 0
1 1 1 1

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 35


EXERCISE 3 : KINEMATICS – JACOBIAN: 𝟎 𝑇
cos 𝜃 + 𝜃 − sin 𝜃 + 𝜃 0 𝐿 cos 𝜃 where 𝑷 = 𝑥 𝑦 𝑧
METHOD2 𝑇 = sin 𝜃 + 𝜃 cos 𝜃 + 𝜃 0 𝐿 sin 𝜃
0
2
1
1 2
2
1
1
2
2 1
1
1
1
𝑥2 0 0 1 0
𝑃 0 0 0 1

and 0𝜽𝑃 = 𝜃𝑥 𝜃𝑦 𝜃𝑧 𝑇
𝜕𝑥 𝜕𝑥 Is the orientation of the End-Effector around
𝑦2
𝑦1 𝜕𝜃1 𝜕𝜃2 𝑥 0 , 𝑦 0 and 𝑧 0 . Here it is 0𝜽𝑃 = 0 0 𝜃1 +𝜃2 𝑇

𝜕𝑦 𝜕𝑦
𝜕𝜃1 𝜕𝜃2
𝑥1 −𝐿2 𝑆 𝜃1 + 𝜃2 − 𝐿1 𝑆 𝜃1 −𝐿2 𝑆 𝜃1 + 𝜃2
𝜕𝑧 𝜕𝑧
𝐿2 𝐶 𝜃1 + 𝜃2 + 𝐿1 𝐶 𝜃1 𝐿2 𝐶 𝜃1 + 𝜃2
𝜕𝜃1 𝜕𝜃2 0 0
𝑱 = =
𝜕𝜃𝑥 𝜕𝜃𝑥 0 0
𝜕𝜃1 𝜕𝜃2 0 0
𝐿2 cos 𝜃1 + 𝜃2 +𝐿1 cos 𝜃1 𝜕𝜃𝑦 𝜕𝜃𝑦 1 1
𝑷𝟎 = 𝐿2 sin 𝜃1 + 𝜃2 + 𝐿1 sin 𝜃1
𝜕𝜃1 𝜕𝜃2
0 𝜕𝜃𝑧 𝜕𝜃𝑧
𝜕𝜃1 𝜕𝜃2

3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 36


EXERCISE
𝑧 ,𝑧 ,𝑧
4:
0
KINEMATICS
1 2
– METHOD 1

0 0 0 𝜃1
0 0 0 0
𝑥3
𝑦3 −90 0 𝑑2 0
𝑧3 𝑃 cos 𝜃𝑖 − sin 𝜃𝑖 0 𝑎𝑖−1
𝑖−1 sin 𝜃𝑖 cos 𝛼𝑖−1 cos 𝜃𝑖 cos 𝛼𝑖−1 − sin 𝛼𝑖−1 −𝑑𝑖 sin 𝛼𝑖−1
𝑖 𝑇 =
sin 𝜃𝑖 sin 𝛼𝑖−1 cos 𝜃𝑖 sin 𝛼𝑖−1 cos 𝛼𝑖−1 𝑑𝑖 cos 𝛼𝑖−1
0 0 0 1
𝐶𝜃1 −𝑆𝜃1 0 0 1 0 0 0 1 0 0 0
0 𝑆𝜃1 𝐶𝜃1 0 0 1𝑇 = 0 1 0 0 1 0 0 1 0
1𝑇 = 2
0 0 1 0 2 𝑇 =
𝑥0 , 𝑥1 , 𝑥2 0 0 1 0 0 −1 0 𝑑2
𝑦0 , 𝑦1 , 𝑦2 0 0 0 1 0 0 0 1 0 0 0 1
𝐶𝜃1 −𝑆𝜃1 0 0 1 0 0 0 𝐶𝜃1 0 −𝑆𝜃1 0
0 0 1 2 𝑆𝜃1 𝐶𝜃1 0 0 0 0 1 0 𝑆𝜃1 0 𝐶𝜃1 0
3 𝑇 = 𝑇 𝑇
1 2 3 𝑇 = =
0 0 1 0 0 −1 0 𝑑2 0 −1 0 𝑑2
0 0 0 1 0 0 0 1 0 0 0 1
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 37
EXERCISE
𝑧 ,𝑧
4: KINEMATICS
0 1
– METHOD 2

0 0 0 𝜃1
𝑥2 −90 0 𝑑2 0
𝑧2 0 0 𝑑3 0
𝑦2 𝑃
𝑧3
𝑥3

𝑦3
𝑦0 , 𝑦1 𝐶1 −𝑆1 0 0 1 0 0 0 1 0 0 0
𝑆 𝐶1 0 0 0 0 1 0 𝐴2 = 0 1 0 0
𝑥0 , 𝑥1 𝐴10 = 1 𝐴12 =
0 0 1 0 0 −1 0 𝑑2 3 0 0 1 𝑑3
0 0 0 1 0 0 0 1 0 0 0 1
𝐶1 −𝑆1 0 0 1 0 0 0 𝐶1 0 −𝑆1 −𝑑3 𝑆1
𝑆 𝐶1 0 0 0 0 1 𝑑3 𝑆 0 𝐶1 𝑑3 𝐶1
𝐴03 = 𝐴10 𝐴12 𝐴23 = 1 = 1
0 0 1 0 0 −1 0 𝑑2 0 −1 0 𝑑2
0 0 0 1 0 0 0 1 0 0 0 1
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 38
EXERCISE 4: KINEMATICS – VERIFICATION
𝐶𝜃1 0 −𝑆𝜃1 0 0
0 𝑆𝜃1 0 𝐶𝜃1 0 𝑷𝟑 = 0
3 𝑇 =
0 −1 0 𝑑2 𝑑3
0 0 0 1
𝑃 𝐶1 0 −𝑆1 −𝑑3 𝑆1
𝑆 0 𝐶1 𝑑3 𝐶1 0
𝐴03 = 1 𝑷𝟑 = 0
0 −1 0 𝑑2
0 0 0 1 0
𝐶𝜃1 0 −𝑆𝜃1 0 0 −𝑑3 𝑆1
𝑆𝜃1 0 𝐶𝜃1 0 0 𝑑3 𝐶1
𝑷𝟎 = 03𝑇 𝑷𝟐 = =
0 −1 0 𝑑2 𝑑3 𝑑2
0 0 0 1 1 1

𝐶1 0 −𝑆1 −𝑑3 𝑆1 0 −𝑑3 𝑆1


𝑆 0 𝐶1 𝑑3 𝐶1 0 𝑑3 𝐶1
𝑷𝟎 = 𝐴02 𝑷𝟐 = 1 =
0 −1 0 𝑑2 0 𝑑2
0 0 0 1 1 1
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 39
EXERCISE 4: KINEMATICS – JACOBIAN
and 0𝜽𝑃 = 𝜃𝑥 𝜃𝑦 𝜃𝑧 𝑇
Is the orientation of the End-Effector around
𝑥 0 , 𝑦 0 and 𝑧 0 . Here it is 0𝜽𝑃 = 0 0 𝜃1 𝑇
𝜕𝑥 𝜕𝑥 𝜕𝑥
𝜕𝜃1 𝜕𝑑2 𝜕𝑑3
𝜕𝑦 𝜕𝑦 𝜕𝑦
𝜕𝜃1 𝜕𝑑2 𝜕𝑑3 −𝑑3 𝐶1 0 −𝑆1
𝜕𝑧 𝜕𝑧 𝜕𝑧 −𝑑3 𝑆1 0 𝐶1
𝜕𝜃1 𝜕𝑑2 𝜕𝑑3 0 1 0
𝑱 = =
𝜕𝜃𝑥 𝜕𝜃𝑥 𝜕𝜃𝑥 0 0 0
−𝑑3 𝑆1 𝜕𝜃1 𝜕𝑑2 𝜕𝑑3 0 0 0
𝑷𝟎 = 𝑑3 𝐶1 𝜕𝜃𝑦 𝜕𝜃𝑦 𝜕𝜃𝑦 1 0 0
𝑑2
𝜕𝜃1 𝜕𝑑2 𝜕𝑑3
where 𝑷𝟎 = 𝑥 𝑦 𝑧 𝑇 𝜕𝜃𝑧 𝜕𝜃𝑧 𝜕𝜃𝑧
𝜕𝜃1 𝜕𝑑2 𝜕𝑑3
3/25/2020 Dr. Hassan Diab – MTE 500– Spring 2019/2020 40