2.

Kinematics: Transformation

• For controlling a robot, understand the r/p’s between the joints’ motion (input) and the end-effector motion
(output), because the joint motions control the end-effector’s movements.
• Describe position , orientation, and frames.
i. position: define the position of a point “ P ” relative to coordinate frame {A}.
𝑃𝑋
the “position vector” is: 𝐴𝑃 = 𝑃𝑦
𝑃𝑧
Eg. Describe and draw the position of point “P” relative to coordinate
frame {A} if point “P” is positioned at:2,4,& 3 in x, y, & z of frame {A}
2
𝐴𝑃 = 4
3

1
2.Kinematics
ii. 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 vectors of {B} are:
𝑋෠𝐵 , 𝑌෠𝐵 , 𝑍መ𝐵
• When written in terms of {A} they became:
𝐴𝑋෠ 𝐵 , 𝐴𝑌෠ 𝐵 , 𝐴𝑍෠𝐵
• The 3x3 rotation matrix can be defined as:
𝑋෠𝐴 𝑟11 𝑟12 𝑟13 𝑋෠𝐵 •𝑋෠𝐴 𝑌෠𝐵 •𝑋෠𝐴 𝑍መ𝐵 •𝑋෠𝐴
𝐴 ෠𝐴 ∙ 𝑋෠𝐵 𝑌෠𝐵 𝑍መ𝐵 = 𝑟21 𝑟22 𝑟23 = 𝑋෠𝐵 •𝑌෠𝐴 𝑌෠𝐵 •𝑌෠𝐴 𝑍መ𝐵 •𝑌෠𝐴
𝐵𝑅 = 𝐴𝑋෠ 𝐵 𝐴𝑌෠ 𝐵 𝐴𝑍෠𝐵 = 𝑌
𝑍መ𝐴 𝑟31 𝑟32 𝑟33 𝑋෠𝐵 •𝑍መ𝐴 𝑌෠𝐵 •𝑍መ𝐴 𝑍መ𝐵 •𝑍መ𝐴
• From linear algebra, the inverse of a matrix with orthogonal columns is the same as its transpose.
This means that:
𝐵 𝐴 𝑇 𝐴 −1
𝐴𝑅 = 𝐵 𝑅 = 𝐵 𝑅
• The rotation matrix is the direction cosines matrix.

cos(𝑋෠𝐵 •𝑋෠𝐴 ) cos(𝑌෠𝐵 •𝑋෠𝐴 ) cos(𝑍መ𝐵 •𝑋෠𝐴 )

𝐴 ෠ ෠
𝐵𝑅 = cos(𝑋𝐵 •𝑌𝐴 ) cos(𝑌෠𝐵 •𝑌෠𝐴 ) cos(𝑍መ𝐵 •𝑌෠𝐴 )
cos(𝑋෠𝐵 •𝑍መ𝐴 ) cos(𝑌෠𝐵 •𝑍መ𝐴 ) cos(𝑍መ𝐵 •𝑍መ𝐴 )
2
 2.Kinematics
• Eg. Find the rotation matrix of point ‘’P’’ if its frame {B} is rotated 30˚ about the z-axis relative to
frame {A}.
Sol. Using the direction cosines method

cos(𝑋෠𝐵 •𝑋෠𝐴 ) cos(𝑌෠𝐵 •𝑋෠𝐴 ) cos(𝑍መ𝐵 •𝑋෠𝐴 ) cos(30) cos(120) cos(90)

𝐴 ෠ ෠
𝐵𝑅 = cos(𝑋𝐵 •𝑌𝐴 ) cos(𝑌෠𝐵 •𝑌෠𝐴 ) cos(𝑍መ𝐵 •𝑌෠𝐴 ) = cos(60) cos(30) cos(90)
cos(𝑋෠𝐵 •𝑍መ𝐴 ) cos(𝑌෠𝐵 •𝑍መ𝐴 ) cos(𝑍መ𝐵 •𝑍መ𝐴 ) cos(90) cos(90) cos(0)

cos(30) −s 𝑖𝑛(30) 0
= sin(30) cos(30) 0
0 0 1

3
2.Kinematics
iii. Frame : define the position and orientation of an object relative to coordinate frame {A}. A new
frame {B} on the object is required.
{B} = { 𝐵𝐴𝑅3𝑥3 , 𝐴𝑃3𝑥1 }
𝐵𝑜𝑟𝑔

• To simplify the matrix/vector combination, we use the “homogeneous transformation "matrix as

follows:
𝐴 3𝑥3
𝐴 𝑇 𝐵𝑅 𝐴𝑃3𝑥1
𝐵𝑅 = 𝐵𝑜𝑟𝑔
000 1

• The above transformation matrix has 3 unit vectors on each frame for orientation, and 1 vector
from a frame to another frame for position.

4
2.Kinematics
• Eg. Describe frame {B} relative to frame {A} if the origin of {B} is positioned at -2, 3. & 5 in x, y, & z
relative to frame {A}, and {B} is rotated 30˚ about the z-axis relative to frame {A}
• Sol.
𝑐30 −𝑠30 0 −2
𝐴 𝑠30 𝑐30 0 3
𝐵 𝑇 =
0 0 1 5
0 0 0 1

5
2.Kinematics:Mappings
• Change the description of a point from being relative to a frame to being relative to another
frame.
i. Translation only:
• The second frame is translated (without rotation ) relative to the first frame. Only adding vectors.
𝐴𝑃 = 𝐵𝑃 + 𝐴𝑃𝐵𝑜𝑟𝑔
Eg. Frame {B} is translated 10 , & 5 unit in x and y of
frame {A} . Find 𝐴𝑃 if 𝐵𝑃 =[3 7 0]T
Sol
𝐴𝑃 = 𝐵𝑃 + 𝐴𝑃𝐵𝑜𝑟𝑔
3 10 13
𝐴𝑃 = 7 + 5 = 12
0 0 0

6
2.Kinematics:Mapping
ii. Rotation only:
The 2nd frame is rotated relative to the 1st frame , it implies multiply by the rotation matrix.
𝐴
𝐴𝑃 = 𝐵𝑅 ∙ 𝐵𝑃
Eg. Frame {B} is rotated relative to frame {A}
𝐴
About z-axis by 30˚. Find 𝐵𝑅, then find 𝐴𝑃
if 𝐵𝑃 = 0 2 0 𝑇

Soln
𝑐30 −𝑠30 0 0.866 −0.5 0
𝐴
𝐵𝑅 = 𝑠30 𝑐30 0 = 0.5 0.866 0
0 0 1 0 0 1

0.866 −0.5 0 0 −1
𝐴
𝐴𝑃 = 𝐵𝑅 ∙ 𝐵𝑃 = 0.5 0.866 0 2 = 1.732
0 0 1 0 0

7
 2.Kinematics: Mapping
iii. Translation and rotation: the 2nd frame is translated and rotated relative to the first frame
𝐴𝑃 = 𝐵𝐴𝑅 ∙ 𝐵𝑃 + 𝐴𝑃𝐵𝑜𝑟𝑔 , To simplify the equation for easier , we use the homogeneous transformation
matrix (T) as follows:
4𝑥4
𝐴𝑃 4𝑥1 𝐴
𝐵𝑅 𝐴𝑃𝐵𝑜𝑟𝑔 𝐵𝑃 4𝑥1 𝐴 𝐵𝑃
= = 𝐵𝑇
1 000 1 1 1
Eg. Frame {B} is rotated relative to frame {A} about z-axis by
30˚, then translated 10 & 5 unit in x & y respectively. Define
Frame {B} relative to frame {A}. Find 𝐵𝐴𝑇, then find 𝐴𝑃 if
𝐵𝑃 =[3 7 0]T.
0.866 −0.5 0 10
𝐴 0.5 0.866 0 5
Sol. 𝐵 𝑇 =
0 0 1 0
0 0 0 1

0.866 −0.5 0 10 3 9.098

𝐴𝑃 𝐵 0.5 0.866 0 5 7 12.562
= 𝐵𝐴𝑇 𝑃 = =
1 1 0 0 1 0 0 0
0 0 0 1 1 1
8
2.Kinematics:General transformation operator
• The translational and rotational of vectors on the same frame .
• 𝐴𝑃2 = 𝑅𝑘 𝜃 ∙ 𝐴𝑃1 + 𝐴𝑄
Where k is the rotation axis , 𝜃 is the angle of rotation, Q is the
translational vector.
• For homogeneous transformation matrix (T) as follows:
4𝑥4
𝐴𝑃2 4𝑥1 𝑅𝑘 𝜃 𝐴𝑄 𝐴𝑃1 4𝑥1 𝐴
= = 𝑇 𝑃1
1 000 1 1 1
If we decompose it, we can get the original equation.
Eg. Vector 𝐴𝑃1 is rotated relative to frame {A} about z-axis by 30˚ , and
translated 10,& 5 unites in XA &YA resp.ly. Define The transformation matrix (T), then find 𝐴𝑃2 if
𝐴𝑃1 = 3 7 0 𝑇.

0.866 −0.5 0 10
𝑅𝑘 30 𝐴𝑄 0.5 0.866 0 5
Soln. 𝑇 = =
000 1 0 0 1 0
0 0 0 1
0.866 −0.5 0 10 3 9.098
𝐴𝑃2 𝐴 0.5 0.866 0 5 7 12.562
= 𝑇 𝑃1 = =
1 1 0 0 1 0 0 0
0 0 0 1 1 1
9
2.Kinematics:Transformation Arithmetic
• Compound transform:
𝐴𝑃 𝐶
= 𝐵𝐴𝑇 𝐵𝐶𝑇 ∙ 𝑃
1 1
Eg. Frame {B} is rotated relative to frame {A} about z-axis by 30˚ and
Translated 4 & 3 units in x, y axis respectively. Frame {C} is rotated
Relative to frame {B} about x-axis by 60˚ and translated 6 & 5 units
In x , z axis respectively. Find the position of point “P” relative to frame {A}
If Cp = [8 7 9]T .
0.866 −0.5 0 4
𝑅 30 𝐴𝑃𝐵𝑜𝑟𝑔 0.5 0.866 0 3
Soln. 𝐵𝐴𝑇 = 𝑧 = ,
000 1 0 0 1 0
0 0 0 1
1 0 0 6
𝐵 𝑅𝑥 60 𝐵𝑃𝐶𝑜𝑟𝑔 0 0.5 −0.866 0 𝐴𝑃 𝐶
𝐶𝑇 = = , = 𝐵𝐴𝑇 𝐵𝐶𝑇 ∙ 𝑃
000 1 0 0.866 0.5 5 1 1
0 0 0 1
0.866 −0.5 0 4 1 0 0 6 8 18.27
𝐴𝑃 0.5 0.866 0 3 0 0.5 −0.866 0 7 6.28
= =
1 0 0 1 0 0 0.866 0.5 5 9 15.56
0 0 0 1 0 0 0 1 1 1
10
 2.Kinematics:Arithmetic transform

• Inverting transform :
• Remember that for rotation matrices: 𝐵𝐴𝑅 = 𝐵𝐴𝑅−1 = 𝐵𝐴𝑅. This is valid only for rotation matrices.
• For transformation matrices , we can use the following formula to find the inverse:

𝐴 𝑇
𝐵 𝐴 −1 𝐵𝑅 − 𝐵𝐴𝑅𝑇 ∙ 𝐴𝑃𝐵𝑜𝑟𝑔
𝐴𝑇 = 𝐵𝑇 =
000 1
Eg : frame {B} is rotated relative to frame {A} about z-axis by 30˚ and translated 4, 3 units in x, y axis
respectively. Find the transformation matrix that describe frame {A} relative to frame {B}.
0.866 −0.5 0 4
𝐴 𝑅𝑧 30 𝐴𝑃𝐵𝑜𝑟𝑔 0.5 0.866 0 3
Soln,. 𝐵𝑇 = = ,
000 1 0 0 1 0
0 0 0 1
𝑇 𝑇 0.866 0.5 0 −4.964
0.866 −0.5 0 0.866 −0.5 0 4
𝐵 0.5 0.866 0 0.5 0.866 0 . 3 −0.5 0.866 0 −0.598
𝐴𝑇 = =
0 0 1 0 0 1 0 0 0 1 0
000 1 0 0 0 1
11
2.KinematicsMore than one axis of Rotation

• Rotation about Z, X, Y axis

α

β
γ

Please Check EXAMPLE 2.7 on text book

12
Angles?

• we see that, by taking the square root of the sum of the squares of r11 and r21 , we can compute cos β.
Then, we can solve for β with the arc tangent of r31 over the computed cosine.
• Then, as long as cβ ≠ 0, we can solve for α by taking the arc tangent of r21/cβ over r11/cβ and we
can solve for γ by taking the arc tangent of r32/cβ over r33/cβ.
Reading assignment
• Equivalent angle–axis representation
• Euler parameters
• TRANSFORMATION OF FREE VECTORS
Written assignment
Exercise: 2.1, 2.3, 2.6, 2.12,
2.13
Submit date :for next
Monday 13
 2.Kinematics: MANIPULATOR KINEMATICS

• Manipulator is a set of bodies (links) connected in a chain by joints (form a connection

between a neighboring pair of links)

• kinematics is the science of motion that treats the subject without regard to the forces that
cause it.

• Within the science of kinematics, one studies the position, the velocity, the acceleration, and
all higher order derivatives of the position variables (with respect to time or any other
variable(s)).

• It is a method to compute the position and orientation of the manipulator’s end-effector

relative to the base of the manipulator as a function of the joint variables.

• The relationships between these motions and the forces and torques that cause them
constitute the problem of dynamics.

14
2.Kinematics:MANIPULATOR KINEMATICS

• Most manipulators have revolute joints (single-axis rotational joints that connect two rigid links
together) or have sliding joints called prismatic joints (single axis translational joints that
connect two rigid links together ).
• The links are numbered starting from the immobile base of the arm, which might be called link
0. The first moving body is link 1, and so on, out to the free end of the arm, which is link n.

• The kinematic function of a link is to maintain a fixed relationship between the two joint axes it supports.
• This relationship can be described with two parameters: the link length, a, and the link twist, α
15
 2.Kinematics: MANIPULATOR KINEMATICS - link-joint description
• For the purposes of obtaining the kinematic equations of the mechanism, a link is considered only as a
rigid body that defines the relationship b/n two neighboring joint axes of a manipulator.
• Joint axes are defined by lines in space. Joint axis i is defined by a line in space, or a vector direction,
about which link i rotates relative to link i-1.
• For kinematic purposes, a link can be specified with two numbers, which define the relative location of
the two axes in space (joints).
• The link offset, d, and the joint angle, θ, are
two parameters that may be used to describe the
nature of the connection between neighboring links.
 Intermediate link in the chain
• The distance along this common axis from one link
to the next. It is variable if joint i is prismatic link offset
• The amount of rotation about this common axis
b/n one link and its neighbor. It is variable for
a revolute joint
joint angle

16
 2.Kinematics: MANIPULATOR KINEMATICS-link-joint description
• Link-joint pairs can be described using four parameters, two that describe the link, and two that
describe the joint.
• Link description : a robotic manipulator can contain two or more links, it can be described using
two parameters.

i. Link length(𝒂𝒊−𝟏 ): the length along the common perpendicular line between the joints axes.

i. Twist angle (𝜶𝒊−𝟏 ): the angle about the common perpendicular line between the joint axes.

17
2.Kinematics:MANIPULATOR KINEMATICS

• E.g. find the link length(a) and the twist angle (α) for the shown link that supports two revolute
joints,

Sol. From the shown figure and dimensions: link length(a) = 0.5*2 + 5 + 0.5*2 = 7 in and twist
angle(α) = 45˚

18
 2.Kinematics:MANIPULATOR KINEMATICS-link joint description
• Joint description: a robotic manipulator can contain one or more joints. Joints are described using
two parameters :

i. Joint offset (d𝒊): The length along the axis of motion b/n two common perpendicular lines.

ii. Joint angle (ϑ𝒊 ): The angle about the axis of motion b/n two common perpendicular lines.

Note: for a R. joint, ϑ is a variable & d is constant. For a prismatic joint d is a variable and ϑ is constant .

19
2.Kinematics:Link-joint description- Denavit – Hartenberg (DH) parameters
 Denavit – Hartenberg (DH) parameters:
• the four link-joint para meters are defined as
DH parameters: a𝒊 , 𝛂𝒊 , d𝒊 , ϑ𝒊
• One of the four parameters is a variable (d for prismatic joints,
Or ϑ for revolute joints), and the other three parameters are
constant for each link-joint pair.

• For convenience and to reduce calculations, it is preferred to

set the following parameters to zero: a𝟎 = 𝛂𝟎 = a𝒏 = 𝛂𝒏 = 𝟎, and for revolute joints d𝟏 = 𝟎, and
for prismatic joints 𝛝𝟏 = 𝟎

20
 2.Kinematics:Frame Attachment to links

• Procedure for frame assignment: (attach frame {i} to link ‘I’)

 Z-axes: assign the z-axes along the axis of rotation (for RJ) or axis of translation( for PJ).
 Frame origins: identify the common perpendicular between Z-axes, or the point of intersection b/n
them. These intersection will be the frame origins.
 X-axes: assign all x-axes starting from the frame origin and pointing towards the next joint along the
common perpendicular. If z-axes intersect, then the x-axes should be the normal to the plane that
contains the two z-axes (for 𝑥𝑖 , look at 𝑧𝑖 and 𝑧𝑖+1 )
 Y-axes: assign all y-axes to complete the right-hand rule (when the hand is flat open, the thump is
pointing toward the z-axes, the four finger are pointing towards the x-axis, then moving the four fingers
90 degrees will indicate the direction of the y-axis).
• Frame {0}: assign frame {0} to match frame {1} when the first joint variables is zero. This is recommended
for convenience so that some DH parameters can be set to zero reduce calculations. Otherwise , frame
{0} can be assigned arbitrarily.
• Frame {n}: assign frame {n} such that you can cause as many DH parameters as possible to be zero.
Otherwise , frame{n} can be assigned arbitrarily.
21
2.Kinematics:Frame Attachment to links

22
 2.Kinematics:Frame attachment to links

 procedure for extracting the DH parameters :

• 𝑎𝑖 : the distance from 𝑧𝑖 to 𝑧𝑖+1 along 𝑥𝑖

• α𝑖 : the angle from 𝑧𝑖 to 𝑧𝑖+1 about 𝑥𝑖

• 𝑑𝑖 : the distance from 𝑥𝑖−1 to 𝑥𝑖 along 𝑧𝑖

• ϑ𝑖 : the angle from 𝑥𝑖−1 to 𝑥𝑖 about 𝑧𝑖

Special cases:

• When joint axes (𝑧𝑖 , 𝑧𝑖+1 ) intersect, the direction of 𝑥𝑖 can be either of the two directions.

• When joints axes (𝑧𝑖 , 𝑧𝑖+1 ) are parallel, the choice of origin for frame {i} is arbitrary, but it is
recommended to be chosen such that 𝑑𝑖 = 0.

23
Example
• Assign frame {0} to {4} for the shown non-planer 3R robotic manipulator, then find the DH
parameters table i αi-1 ai-1 di ϑi
Soln. 1 0 0 L1 ϑ1
2 90 0 0 ϑ2
3 0 L2 0 ϑ3
4 0 L3 0 0

24
 E.g. 2 assign frame {0} to {4} for the shown planer 3R (RRR) robotic manipulator, then
find the DH parameter table.
Soln
• Z-Axes: assign all z-axes along the axis
of rotation (for revolute joints) or axis of

i αi-1 ai-1 di ϑi
1 0 0 0 ϑ1
2 0 L1 0 ϑ2
3 0 L2 0 ϑ3
4 0 L3 0 0

25
2.Kinematics:Manipulator forward kinematics
 Define the transformation matrix from one frame to the other in terms of joint variables.
• Frame {Q} differs from {R} by a translation ai-1.
• Frame {P} differs from {Q} by a rotation θi, and
• frame {i} differs from {P} by a translation di.
• If we wish to write the transformation that transforms
vectors defined in {i} to their description in {i - 1},
we may write

Please check example 3.6

26
Quiz : assign frames {0} to {4} for the shown RPR and 3R robotic manipulator. Then find the DH parameters
table.

27
2.Kinematics:manipulator inverse kinematics
 Introduction
• Forward kinematics: given ϑ1, ϑ2… and link length
Find 𝑛0𝑇
• IV kinematics: given 𝑛0𝑇
Find ϑ1, ϑ2

28
Solvability IK problems

 Existence of a solution:

• Workspace : is defined as the volume of space that the end-effector (gripper) can reach.

o For a Soln to exist, the end-effector goal must be within the workspace.

• DOF: degree of freedom are the number of independent variable in a manipulator.

o For a solution to exist, the number of joint should be more than or equal to the number of
independent Cartesian coordinate to be controlled at the end effector.

29
Solvability IK problems
• E.g. for the shown 2R planer manipulator, determine the conditions where an inverse kinematics
solution exists.

• Sol. For IK Soln to exist, the workspace

boundary is determined as follows:

i. When the arm is fully stretched

(ϑ2 = 0˚) the outer workspace boundary is

(L1 + L2) frame {0} origin.

30
Solvability IK problems
i. When the arm is fully folded

(ϑ2 = 180˚) the outer workspace boundary is

(L1 - L2) frame {0} origin.

For a solution to exist, the destination point for the

end - effector must be within the workspace area.

31
Solvability of IK problems
• Eg. For the 3R planar manipulator, the transformation matrix that relate frame {3} to the
ground frame {0} is defined as:
𝑐123 −𝑠123 0 𝐿1 𝑐1 + 𝐿2 𝑐12
0 𝑠123 𝑐123 0 𝐿1 𝑠1 + 𝐿2 𝑠12
3𝑇 =
0 0 1 1
0 0 0 0
Can a solution exit if the desired transformation at frame {3} is of the following form, and why?
0.866 −0.5 0 6 0.866 0 0.5 4
a) 03𝑇𝑑𝑒𝑠𝑖𝑟𝑒𝑑 = 0.5 0.866 0 5 b) 03𝑇𝑑𝑒𝑠𝑖𝑟𝑒𝑑 =
0 1 0 5
0 0 1 0 −0.5 0 0.866 2
0 0 0 1 0 0 0 1
Soln
a) a solution can exist, b/c the robot is 3 DoF and we can extract three unique equations from
the transformation matrix (one x, one in y, and one in Rz)
b) A Soln does not exist, b/c the desired transformation matrix the robot to move in Cartesian
coordinate along the Z axis and rotate about the Y-axis, which the planer robot cannot do.

32
2.Kinematics:Solvability of IK problems
• Multiple Soln: it exist if the robot can reach the desired goal in more than one configuration.
• finite number of Soln : if the number of joints in the robot (DOF) is equal to the number of independent
Cartesian coordinate that can be controlled within the robot’s workspace.
In 3D space, the maximum no of CCO is six: x, y, z γ, β, α.
• infinite no of soln: if the number of joints in the robot (DoF) is more than the no of independent CCO
that can be controlled within the robot’s workspace.
In this case, the robot would be a “redundant” robot.
• Eg. For the 3R planar manipulator, how many soln can exist
for reaching the same pose within the planer workspace
of the robot? Give an eg. Of how to choose one of the solns.
• Soln
For Eg. To go from point A to point B when there is an obstacle,
the robot can reach the target through two d/t configurations.
• The closest soln is when the elbow is up
(red dotted line) => collision with the obstacle.
• The safest soln is when the elbow is down
(blue dotted line) => no collision, but further motion.

33
Solution of inverse kinematics problems
• Inverse kinematic problems can be solved geometrically or algebraically.
• Geometric sol can be achieved by decomposing the spatial geometry of the robot into plane-
geometry problems. Joint angles can be solved by using the tools of plane-geometry.
• Algebraic solutions can be achieved by comparing elements from the general transformation matrix
of a robot to elements from the desired transformation matrix where that robot is commanded to
reach.
• We focus on the algebraic soln
• Eg. For the 3R planar manipulator shown, the general transformation matrix that defines frame {3}
relative to frame {0} is found through forward kinematics as follows:
𝑐123 −𝑠123 0 𝐿1 𝑐1 + 𝐿2 𝑐12
0 𝑠123 𝑐123 0 𝐿1 𝑠1 + 𝐿2 𝑠12
3𝑇 =
0 0 1 0
0 0 0 1

Use IK to find the soln for the joint variables (in terms of x, y, φ) for the robot to reach the following
Cartesian pose at frame {3}
34
Solution of inverse kinematics problems
𝑐φ −𝑠𝑣 0 𝑥
𝑠 𝑐φ 0 𝑦
• 03𝑇 = φ
0 0 1 0
0 0 0 1
• Then find the soln for the joint variables if X=3 units, Y=4 units, φ = 150˚ L1 = 2 units, and l2 = 5 units. Plot
the solutions.
𝑐φ −𝑠𝑣 0 𝑥 𝑐123 −𝑠123 0 𝐿1 𝑐1 + 𝐿2 𝑐12 𝑡11 𝑡12 0 𝑡14
𝑠 𝑐φ 0 𝑦 𝑠 𝑐123 0 𝐿1 𝑠1 + 𝐿2 𝑠12 𝑡 𝑡 0 𝑡24
• 03𝑇 = φ = 123 = 21 22
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
• From t11 and t21
𝑐φ = 𝑐123 = cos(𝜗1 + 𝜗2 + 𝜗3 ) and 𝑠φ = 𝑠123 = sin(𝜗1 + 𝜗2 + 𝜗3 …………….(1)
From t14 : x = 𝐿1 𝑐1 + 𝐿2 𝑐12 …….(2)
From t24 : 𝑦 = 𝐿1 𝑠1 + 𝐿2 𝑠12 …….(3)
Squaring and adding equation (2) and (3) yields: 𝑥 2 + 𝑦 2 = 𝐿21 + 𝐿22 + 2𝐿1 𝐿2 𝑐2
𝑥 2 +𝑦 2 −𝐿21 +𝐿21
Solving for 𝑐2 = 2𝐿1 𝐿2

35
Solution of inverse kinematics problems
• For a solution to exist, -1 ≤ c2 ≤ 1 , otherwise T is out of reach.
𝑥 2 +𝑦 2 −𝐿21 +𝐿21 2
• From trigonometry: 𝑠2 = ± 1 − 𝑐22 = ± 1− ( ) (two soln)
2𝐿1 𝐿2
2
𝑥 2 +𝑦 2 −𝐿21 +𝐿21 𝑥 2 +𝑦 2 −𝐿21 +𝐿21
• 𝜃2 = 𝐴𝑡𝑎𝑛2 𝑠2 , 𝑐2 = 𝐴𝑡𝑎𝑛2(± 1 − 2𝐿1 𝐿2
, 2𝐿1 𝐿2
)( two soln)

• Now, we use equation (2), (3) and 𝜃2 to find 𝜃1 as follows:

• 𝜃1 = 𝐴𝑡𝑎𝑛2 𝑦, 𝑥 − 𝐴𝑡𝑎𝑛2 𝐿2 𝑠2 , 𝐿1 + 𝐿2 𝑐2 (two soln, one for each 𝜃2 )
• We use equation (1) to find 𝜃3 as follows:
• 𝜃1 + 𝜃2 + 𝜃3 = 𝜑 = 𝐴𝑡𝑎𝑛2(𝑠φ , 𝑐φ )
• Solving for 𝜃3 : 𝜃3 = 𝐴𝑡𝑎𝑛2(𝑠φ , 𝑐φ ) − 𝜃1 − 𝜃2 (two soln, one for each 𝜃1,2 )
• This complete the soln for finding the joint values in terms of x, y, φ.
• To find the soln for joint values when 𝑥 = 3 & 𝑦 = 4 𝑢𝑛𝑖𝑡𝑠, φ = 150˚, 𝐿1 = 2, 𝐿2 = 5 𝑢𝑛𝑖𝑡𝑠, the desired
transformation matrix would be:
−0.866 −0.6 0 3
0.5 −0.866 0 4
• 03𝑇𝑑𝑒𝑠𝑟𝑖𝑒𝑑 =
0 0 1 0
0 0 0 1
36
Solution of inverse kinematics problems
• Substituting these values to the equations found earlier yields:
𝑥 2 +𝑦 2 −𝐿21 − 𝐿21 32 +42 −22 −52
• 𝑐2 = 2𝐿1 𝐿2
= 2∗2∗5
= −0.2 𝑎𝑛𝑑 𝑠2 = ± 1 − −0.2 2 = ±0.98
• Then:
𝜃2 = 𝐴𝑡𝑎𝑛 2 ±0.98, −0.2 = 1.7721
𝑎𝑛𝑑 − 1.7721 𝑟𝑎𝑑𝑖𝑎𝑛𝑠
𝜃1 = 𝐴𝑡𝑎𝑛 2 4,3 − 𝐴𝑡𝑎𝑛2 ±5 0.98 , 2 − 5 ∗ 0.2
= 0.4422 𝑎𝑛𝑑 2.2968 𝑟𝑎𝑑𝑖𝑎𝑛𝑠
𝜃3 = 𝐴𝑡𝑎𝑛 2 0.5, −0.866 − 2.2968 + 1.7721
= 2.0933 𝑟𝑎𝑑𝑖𝑎𝑛𝑠
• The two sets of soln for (𝜃1 , 𝜃2 , 𝜃3 ) are :
• Configuration 1: (-0.4422, 1.7721, 1.2881) radians
(-25.3, 101.5, 73.8) degrees
• Configuration 2: (2.2968, -1.7721,2.0933) radians
(131.6, -101.5, 119.9) degrees

37
Matlab commands for manipulator inverse kinematics
• Create link parameter : L(1) =link(DH(1,1:4), ‘modified’)
• Create the robot object: Robot = SerialLink(L, ’name’, ‘description’)
• Find the robot object for puma 560(predefined): mdl_puma560akb
• Plot the robot at a specific configuration(q): plot(Robot, q)

38
2.Kinematics:: Jacobians Velocities and Static Forces
• Deals r/n ship b/n Cartesian velocities of the tip the joint velocities.
• To find the Jacobian using velocity propagation method, direct differentiation method, and
force/moment propagation method.
• Identify singularities and avoid permanent ones
• Cartesian transformation of velocities and force
 Jacobians: MD matrices that relate the Cartesian velocities to the joint velocities.
 Time variables:
• Θ(t): ϑi=>(t)=> ϑf
• X(t): Xi => (t) =>Xf
 Derivative with respect to time:
𝒅ϑ
• 𝒅𝒕 = ϑሶ
𝒅x
• 𝒅𝒕 = xሶ

39
Jacobian using velocity propagation method
• a) angular velocities:
𝑤 = 𝑤𝑥 𝑤𝑦 𝑤𝑧 𝑇
• Add velocities relative to the same frame:
𝑖𝑤𝑖+1 = 𝑖𝑤𝑖 + 𝑖+1𝑖𝑅. 𝜗ሶ𝑖+1 . 𝑖 + 1𝑍𝑖+1
• Where 𝑖 + 1𝑍𝑖+1 = 0 0 1 𝑇
• Pre-multiply by the rotation matrix:
• For revolute joints:
𝑖 + 1𝑤𝑖+1 = 𝑖+1𝑖𝑅. 𝑖𝑤 𝑖 + 𝜗ሶ𝑖+1 . 𝑖 + 1𝑍𝑖+1
• For prismatic joints
• 𝑖 + 1𝑤𝑖+1 = 𝑖+1𝑖𝑅. 𝑖𝑤 𝑖

40
Jacobian using velocity propagation method
b) Linear velocities :
𝑣 = 𝑥ሶ 𝑦ሶ 𝑧ሶ T
Add velocities relative to the same frame:
𝑖𝑣 𝑖 𝑖 𝑖
𝑖+1 = 𝑣𝑖 + 𝑤𝑖 + 𝑝𝑖+1

• pre – multiply by the rotation matrix:

• For revolute joints:
𝑖+1 𝑖
𝑖 + 1𝑣𝑖+1 = 𝑖 𝑅. ( 𝑣𝑖 + 𝑖𝑤𝑖 + 𝑖𝑝𝑖+1 )
• For prismatic joints:

𝑖 + 1𝑣𝑖+1 = 𝑖+1𝑖𝑅. 𝑖𝑣𝑖 + 𝑖𝑤𝑖 + 𝑖𝑝𝑖+1 + 𝑑ሶ 𝑖+1 . 𝑖 + 1𝑍𝑖+1

• Please read about cross product of matrix

41
Jacobian using velocity propagation method
c) The Jacobian, relative to the last frame{n}:

• For linear velocities : [𝑛𝑣𝑛 ]3𝑥1 = [𝑛𝐽𝑙𝑖𝑛𝑒𝑎𝑟 ]3𝑥𝑚 . 𝜃ሶ 𝑚𝑥1

• For angular velocities: [𝑛𝑤𝑛 ]3𝑥1 = [𝑛𝐽𝑎𝑛𝑔𝑢𝑙𝑎𝑟 ]3𝑥𝑚 . 𝜃ሶ 𝑚𝑥1

𝑛𝑣
𝑛 ] 6𝑥1 = 𝑛 6𝑥𝑚 .
• For general velocities: 𝑛𝑤 𝐽 𝜃ሶ 𝑚𝑥1
𝑛

Where m is the number of joints

42
Jacobian using velocity propagation method
d) Changing the reference frame:
• To change the reference frame for linear and angular velocities:
0 𝑛𝑣 0𝑤𝑛 = 0𝑛𝑅. 𝑛𝑤𝑛
𝑛 = 𝑛𝑅.
0𝑣
𝑛
• To change the reference frame for linear or angular Jacobians:

𝑛𝑣
𝑛 = 𝑛𝐽 𝜃ሶ ⇒ 0𝑣
𝑛 = 0𝑛𝑅. 𝑛𝐽. [𝜃ሶ ] ⇒ 0𝑣𝑛 = 0𝐽. [𝜃ሶ ]
.
0𝐽
𝑙𝑖𝑛𝑒𝑎𝑟/𝑎𝑛𝑔𝑢𝑙𝑎𝑟
3𝑥𝑚 = 0𝑛𝑅3𝑥3 𝑛𝐽𝑙𝑖𝑛𝑒𝑎𝑟/𝑎𝑛𝑔𝑢𝑙𝑎𝑟 3𝑥𝑚

To change the reference frame for general Jacobians:

0 6𝑥6
0𝐽 6𝑥𝑚 𝑛𝑅 [0]
= 0 . 𝑛𝐽 6𝑥𝑚
[0] 𝑛𝑅

43
Jacobian using velocity propagation method
E.g. For the 2R robot, calculate the linear and angular velocities of the tool tip (end – effector)
based on frame {0} and {3}, then find the Jacobian matrix relative to frame {0} and {3}.
𝑐1 −𝑠1 0 0 𝑐2 −𝑠2 0 𝑙1 1 0 0 𝑙2
0 𝑠1 𝑐1 0 0 1 𝑠2 𝑐2 0 0 2 0 1 0 0
1 𝑇 = 2 𝑇 = 3 𝑇 =
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

𝑐12 −𝑠12 0 𝐿1𝑐1 + 𝐿2𝑐12

0 𝑠12 𝑐12 0 𝐿1𝑠1 + 𝑙2𝑠12
3 𝑇 = i αi-1 ai-1 di θi
0 0 1 0
0 0 0 1 1 0 0 0 Θ1
2 0 L1 0 Θ2
3 0 L2 0 0

44
• Now we start the velocity propagation with the ground frame {0}, whose velocities are known to be zeros.
• Frame {0}:
0 0
• 0𝑤0 = 0 0𝑣0 = 0
0 0
• Frame {1}
𝑐1 𝑠1 0 0 0 0
1𝑤1 = 10𝑅 ∙ 0𝑤0 + 𝜃ሶ1 ∙ 1𝑍1 = −𝑠1 𝑐1 0 0 + 𝜃ሶ1 0 = 0
0 0 1 0 1 𝜃ሶ1
𝑐1 𝑠1 0 0 0 0 0
1
1𝑣1 = 0𝑅 ∙ (0𝑣0 +0𝑤0 × 0𝑃1 ) = −𝑠1 𝑐1 0 0 + 0 × 0 = 0
0 0 1 0 0 0 0
• Frame {2}:
𝑐2 𝑠2 0 0 0 0
2𝑤2 = 21𝑅 ∙ 1𝑤1 + 𝜃ሶ2 ∙ 2𝑍2 = −𝑠2 𝑐2 0 0 + 𝜃ሶ 2 0 = 0
0 0 1 𝜃ሶ1 1 𝜃ሶ1 + 𝜃ሶ 2

𝑐2 𝑠2 0 0 0 𝐿1 𝐿1𝑠2𝜃ሶ1
2𝑣2 = 21𝑅 ∙ (1𝑣1 +1𝑤1 × 1𝑃2 ) = −𝑠2 𝑐2 0 0 + 0 × 0 = 𝐿1𝑐2𝜃ሶ1
0 0 1 0 𝜃ሶ1 0 0
45
• Frame {3}:
1 0 0 0 0 0
3𝑤3 = 32𝑅 ∙ 2𝑤2 + 0 ∙ 3𝑍3 ) = 0 1 0 0 +0 0 = 0
0 0 1 𝜃ሶ1 + 𝜃ሶ 2 1 𝜃ሶ1 + 𝜃ሶ 2

1 0 0 𝐿1𝑠2𝜃ሶ1 0 𝐿2 𝐿1𝑠2𝜃ሶ1
3 0
3𝑣3 = 2𝑅 ∙ (2𝑣2 +2𝑤2 × 2𝑃3 ) = 0 1 0 𝐿1𝑐2𝜃ሶ1 + × 0 = 𝐿1𝑐2𝜃ሶ1 + 𝐿2(𝜃ሶ1 + 𝜃ሶ2 )
0 0 1 0 𝜃ሶ1 + 𝜃ሶ 2 0 0
• Rearrange the velocities of frame {3} to find the Jacobean:

𝑤𝑥 0 0 0 𝜃ሶ 𝑥ሶ 𝐿1𝑠2 0 𝜃ሶ
1
3𝑤3 = 𝑤𝑦 = 0 = 0 0 , 3𝑣3 = 𝑦ሶ = 𝐿1𝑐2 + 𝐿2 𝐿2 1
ሶ ሶ
𝑤𝑧 𝜃ሶ1 + 𝜃ሶ 2 1 1 𝜃2 𝑧ሶ 0 0 𝜃2
The general jacobian relative to frame {3} will then be:

𝑥ሶ 𝐿1𝑠2 0
𝑦ሶ 𝐿1𝑐2 + 𝐿2 𝐿2 𝑥ሶ
3𝑣3 𝐿1𝑠2 0 𝜃ሶ
𝑧ሶ 0 0
= 𝑤 = ======⇒ 𝑦ሶ = 𝐿1𝑐2 + 𝐿2 𝐿2 1
3𝑤3 𝑥 0 0 𝜃ሶ 2
𝑤𝑦 𝑤𝑧 1 1
0 0
𝑤𝑧 1 1

46
• The velocities of the tool tip (end effector) based on frame{0}:

𝑐12 −𝑠12 0 𝐿1𝑠2𝜃ሶ1

0𝑣3 = 03𝑅 ∙ 3𝑣3 = 𝑠12 𝑐12 0 𝐿1𝑐2𝜃ሶ1 + 𝐿2 𝜃ሶ1 + 𝜃ሶ2
0 0 1 0

𝐿1𝑐12𝑠2𝜃ሶ1 − 𝐿1𝑐2𝑠12𝜃ሶ1 − 𝑙2𝑠12(𝜃ሶ1 + 𝜃ሶ2 ) −𝐿1𝑠1 − 𝐿2𝑠12 −𝐿2𝑠12 𝜃ሶ

1
= 𝐿1𝑠2𝑠12𝜃ሶ1 + 𝐿1𝑐2𝑐12𝜃ሶ1 + 𝐿2𝑐12(𝜃ሶ1 + 𝜃ሶ2 ) = 𝐿1𝑐1 + 𝐿2𝑐12 𝐿2𝑐12
𝜃ሶ2
0 0 0
And
𝑐12 −𝑠12 0 0 0 0 0 𝜃ሶ
0𝑤3 = 03𝑅 ∙ 3𝑤3 = 𝑠12 𝑐12 0 0 = 0 = 0 0 1
ሶ
0 0 1 𝜃ሶ1 + 𝜃ሶ2 𝜃ሶ1 + 𝜃ሶ2 1 1 𝜃2

47
• The jacobian based on frame {0}:
𝑐12 −𝑠12 0 0 0 0 𝐿1𝑠2 0
𝑠12 𝑐12 0 0 0 0 𝐿1𝑐2 + 𝐿2 𝐿2
0𝑅 0
3 0 0 1 0 0 0 0 0
0𝐽 = ∙ 3𝐽 =
0 0𝑅 0 0 0 𝑐12 −𝑠12 0 0 0
3
0 0 0 𝑠12 𝑐12 0 0 0
0 0 0 0 0 1 1 1
𝐿1𝑠2𝑐12 − 𝐿2𝑐2𝑠12 − 𝐿2𝑠12 −𝐿2𝑠12
𝐿1𝑠2𝑠12 + 𝐿1𝑐2𝑐12 + 𝐿2𝑐12 𝐿2𝑐12
𝐿1𝑠2𝑐12 − 𝐿2𝑐2𝑠12 − 𝐿2𝑠12 −𝐿2𝑠12
0 0
= ==⇒ 𝐿1𝑠2𝑠12 + 𝐿1𝑐2𝑐12 + 𝐿2𝑐12 𝐿2𝑐12
0 0
1 1
0 0
1 1
−𝐿1𝑠1 − 𝐿1𝑠12 −𝐿2𝑠12
⇒ 𝐿1𝑐1 + 𝐿2𝑐12 𝐿2𝑐12
1 1
Or, since this is a planer robot , we can take the linear components only and pre-multiply it by R:

𝑐12 −𝑠12 0 𝐿1𝑠2 0 −𝐿1𝑠1 − 𝐿1𝑠12 −𝐿2𝑠12

0
0𝐽 = 3𝑅 ∙ 3𝐽 = 𝑠12 𝑐12 0 𝐿1𝑐2 + 𝐿2 𝐿2 = 𝐿1𝑐1 + 𝐿2𝑐12 𝐿2𝑐12
0 0 1 1 1 0 0

48
Jacobian using direct differentiation Method

• The linear velocities jacobian: 3×1

𝜕𝑥 𝜕𝑥 𝜕𝑥
𝜕𝜃1 𝜕𝜃2 𝜕𝜃𝑚 𝑚𝑥1
𝑥ሶ 3×1 𝜃1ሶ
𝜕𝑦 𝜕𝑦 𝜕𝑦
0𝑣𝑛 = 0 𝑦ሶ = 𝜃2ሶ 𝑜𝑟: 0𝑣 𝑛 = 0𝐽(𝜃) ∙ 𝜃ሶ
𝜕𝜃1 𝜕𝜃2 𝜕𝜃𝑚
𝑧ሶ 𝑛 𝜃𝑚ሶ
𝜕𝑧 𝜕𝑧 𝜕𝑧
𝜕𝜃1 𝜕𝜃2 𝜕𝜃𝑚

Jacobian (0𝐽𝑙𝑖𝑛𝑒𝑎𝑟 )

Where m is the number of joints (revolute or prismatic), and n is the last frame at the end – effector.
Notes :
• This method finds the jacobian of the last frame {n} relative to the ground frame {0}.
• 𝜃𝑖 represents the joint angles variable for revolute joints. If the joints is prismatic, it can be replaced by the
joint displacement variable 𝑑𝑖 .

49
Jacobian using direct differentiation method

• The Angular velocities jacobian:

0
0𝑤𝑛 = ∙ 𝑖𝑧𝑖 . 𝜃ሶ𝑖 where : 𝑘𝑖 = 0 𝑓𝑜𝑟 𝑝𝑟𝑖𝑠𝑚𝑎𝑡𝑖𝑐 𝑗𝑜𝑖𝑛𝑡𝑠, 1 𝑓𝑜𝑟 𝑟𝑒𝑣𝑜𝑙𝑢𝑡𝑒 𝑗𝑜𝑖𝑛𝑡𝑠 𝑎𝑛𝑑 𝑖𝑧𝑖 = 0
σ𝑛𝑖=1 𝑘𝑖 . 0𝑖𝑅
1
ሶ
0𝑤𝑛 = 𝑘1 . 01𝑅 ∙ 1𝑧1 . 𝜃ሶ1ሶ + 𝑘2 . 02𝑅 ∙ 2𝑧2 . 𝜃ሶ 2 + ⋯ 𝑘𝑚 . 𝑚0𝑅 ∙ 𝑚𝑧𝑚 . 𝜃ሶ,𝑚
ሶ
𝜃ሶ1
𝑤𝑥 3𝑥1 𝜃ሶ2
0𝑤𝑛 = 0 𝑤𝑦 = 𝑘1 . 01𝑅 ∙ 1𝑧1 𝑘2 . 02𝑅 ∙ 2𝑧2 … 𝑘𝑚 . 𝑚0𝑅 ∙ 𝑚𝑧𝑚 3𝑥𝑚 .
𝑤𝑧 𝑛 .
.
Jacobian (0𝐽𝑎𝑛𝑔𝑢𝑙𝑎𝑟 ) 𝜃ሶ𝑚
• The general velocities Jacobians:
0𝐽
𝑜𝐽 = 𝐿𝑖𝑛𝑒𝑎𝑟
0𝐽
𝑎𝑛𝑔𝑢𝑙𝑎𝑟

50
• E.g. For the 2R robot, calculate the linear and angular velocities of the tool tip (end – effector) based on
frame {0} and {3}, then find the Jacobian matrix relative to frame {0} and {3}.

𝑐1 −𝑠1 0 0 𝑐2 −𝑠2 0 𝑙1 1 0 0 𝑙2
0 𝑠1 𝑐1 0 0 1 𝑠2 𝑐2 0 0 0 0 1 0 0
1 𝑇 = 2 𝑇 = 1 𝑇 =
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

𝑐12 −𝑠12 0 𝐿1𝑐1 + 𝐿2𝑐12 i αi-1 ai-1 di θi

0 𝑠12 𝑐12 0 𝐿1𝑠1 + 𝑙2𝑠12
3 𝑇 = 1 0 0 0 Θ1
0 0 1 0
0 0 0 1 2 0 L1 0 Θ2
3 0 L2 0 0

• To start the direct differentiation, we need to extract

𝑥 𝐿1𝑐1 + 𝐿2𝑐12
the position vector from the transition matrix: 0𝑝3 = 0 𝑦 = 𝐿1𝑠1 + 𝐿2𝑠12
𝑧 3 0

51
 𝑥 𝐿1𝑐1 + 𝐿2𝑐12
0𝑝
3 =0 𝑦 = 𝐿1𝑠1 + 𝐿2𝑠12 <=== differentiate this vector w.r.t each joint variables
𝑧 3 0
𝜕𝑥 𝜕𝑥 3×2
2𝑥1
𝑥ሶ 3×1 𝜕𝜃1 𝜕𝜃2 𝜃1ሶ −𝐿1𝑠1 − 𝐿2𝑠12 −𝐿2𝑠12 𝜃ሶ
𝜕𝑦 𝜕𝑦 1
0 𝑦ሶ = 𝜃2ሶ = 𝐿1𝑐1 + 𝐿2𝑐12 𝐿2𝑐12
𝑧ሶ
𝜕𝜃1 𝜕𝜃2
0 0 𝜃ሶ 2
3 𝜕𝑧 𝜕𝑧 𝜃𝑚ሶ
𝜕𝜃1 𝜕𝜃2
Jacobian (0𝐽𝑎𝑛𝑔𝑢𝑙𝑎𝑟 )
−𝐿1𝑠1 − 𝐿2𝑠12 −𝐿2𝑠12
=⇒ 0J = 𝐿1𝑐1 + 𝐿2𝑐12 𝐿2𝑐12
0 0
To find the jacobian relative to frame {3}, pre – multiply by the rotation by the rotation matrix

𝑐12 −𝑠12 0 −𝐿1𝑠1 − 𝐿2𝑠12 −𝐿2𝑠12 𝐿1𝑠2 0

0J = 3𝑅. 0J =
0 𝑠12 𝑐12 0 𝐿1𝑐1 + 𝐿2𝑐12 𝐿2𝑐12 = 𝐿2𝑐2 𝐿2
0 0 1 0 0 0 0
0

52
• E.g.: for the 3R non-planar robot, find the jacobian matrix of the end – effector frame {4} relative to frame
{0} using direct differentiation method.
𝑐1 −𝑠1 0 0 𝑐2 −𝑠2 0 𝐿1 𝑐3 −𝑠3 0 𝐿2
0 𝑠1 𝑐1 0 0 1𝑇 = 0 0 0 0 2 𝑠3 𝑐3 0 0
1𝑇 = 2 3𝑇 =
0 0 1 0 −𝑠2 −𝑐2 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
1 0 0 𝐿3 𝑐1𝑐2 −𝑐1𝑠2 −𝑠1 𝐿1𝑐1
3 0 1 0 0 𝑠1𝑐2 −𝑠1𝑠2 𝑐1 𝐿1𝑠1
4𝑇 = 0
2𝑇 =
0 0 1 0 −𝑠2 −𝑐2 0 0
0 0 0 1 0 0 0 1
𝑐1𝑐23 −𝑐1𝑠23 −𝑠1 𝑐1(𝐿1 + 𝐿2𝑐2) i αi-1 ai-1 di θi
0 𝑠1𝑐23 −𝑠1𝑠23 𝑐1 𝑠1(𝐿1 + 𝐿2𝑐2)
3 𝑇 = 1 0 0 0 Θ1
−𝑠23 −𝑐23 0 −𝐿2𝑠2
0 0 0 1 2 -90 L1 0 Θ2
𝑐1𝑐23 −𝑐1𝑠23 −𝑠1 𝑐1(𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23) 3 0 L2 0 θ3
0 𝑠1𝑐23 −𝑠1𝑠23 𝑐1 𝑠1(𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23) 4 0 L3 0 0
3𝑇 =
−𝑠23 −𝑐23 0 −𝐿2𝑠2 − 𝐿3𝑠23
0 0 0 1

53
• To start the direct differentiation, we need to extract the position vector from the transform matrix:

𝑥 𝑐1 𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23
0𝑝
3 = 0 𝑦 = 𝑠1 𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23 <=== differentiate this vector w.r.t each joint variable.
𝑧 4 −𝐿2𝑠2 − 𝐿3𝑠23

−𝑠1 𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23 𝑐1 −𝐿2𝑠2 − 𝐿3𝑠23 −𝐿3𝑠23𝑐1

0𝐽
𝑙𝑖𝑛𝑒𝑎𝑟 = 𝑐1 𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23 𝑠1(−𝐿2𝑠2 − 𝐿3𝑠23) −𝐿3𝑠23𝑠1
0 −𝐿2𝑐2 − 𝐿3𝑐23 −𝐿3𝑐23
For the angular velocities Jacobian:
0 0 3𝑥3
0𝐽 1 2
𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = 𝑘1 . 1𝑅 ∙ 𝑧1 𝑘2 . 2𝑅 ∙ 𝑧2 … 𝑘𝑚 . 𝑚0𝑅 ∙ 𝑚𝑧𝑚

𝑐1 −𝑠1 0 0 𝑐1𝑐2 −𝑠1𝑠2 −𝑠1 0 𝑐1𝑐23 −𝑠1𝑠23 −𝑠1 0

0𝐽 = 1 . 𝑠1 𝑐1 0 . 0 1 . 𝑠1𝑐2 −𝑠1𝑠2 𝑐1 . 0 1 . 𝑠1𝑐23 −𝑠1𝑠23 𝑐1 . 0
𝑎𝑛𝑔𝑢𝑙𝑎𝑟
0 0 1 1 −𝑠2 −𝑐2 0 1 −𝑠23 −𝑐23 0 1

−𝑠1 𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23 𝑐1 −𝐿2𝑠2 − 𝐿3𝑠23 −𝐿3𝑠23𝑐1

𝑐1 𝐿1 + 𝐿2𝑐2 + 𝐿3𝑐23 𝑠1 −𝐿2𝑠2 − 𝐿3𝑠23 −𝐿3𝑠23𝑠1
0 −𝑠1 −𝑠1
0 −𝐿2𝑐2 − 𝐿3𝑐23 −𝐿3𝑐23
= 0 𝑐1 𝑐1 The general jacobian becomes: 0𝐽4 =
0 −𝑠1 −𝑠1
1 0 0
0 𝑐1 𝑐1
1 0 0

54
Jacobian using static forces/moments propagation method

a) Static forces
• Apply a static force at the end-effector:
𝑛𝐹 = 𝑛 𝑓
𝑥 𝑓𝑦 𝑓𝑧
𝑇

• To find the joint forces/ moments required to hold

the robot at static equilibrium, add forces relative to
the same frame:
෍ 𝐹 = 0 =⇒ 𝑖 𝑓𝑖 − 𝑖𝑓𝑖+1 = 0

• Pre – multiply by the rotation matrix:

𝑖𝑓 = 𝑖 𝑖 +1
𝑖 𝑖+1𝑅 ∙ 𝑓𝑖+1

55
Jacobian using static forces/moments propagation method

b) Static moments:
• Apply a static moments at the end – effector:
𝑛𝐹 = 𝑛 𝑛𝑥 𝑛𝑦 𝑛𝑧 𝑇
• To find the joint forces/ moments required to hold the robot at static equilibrium, add moments relative to
the same frame:
σ 𝑀𝑖 = 0 =⇒ 𝑖𝑛𝑖 − 𝑖𝑛𝑖+1 − 𝑖𝑃𝑖+1 × 𝑖𝑓𝑖+1 = 0
• pre – multiply by the rotation matrix:
𝑖𝑛 𝑖 +1
𝑖 = 𝑖+1𝑅 ∙𝑖 𝑛
𝑖+1 + 𝑖𝑃𝑖+1 × 𝑖𝑓𝑖

56
Jacobian using static forces/moments propagation method

c) The force/moments needed for the joints:

• The joint moment needed for each revolute joint is:
𝜏𝑖 = 𝑖𝑛𝑖 𝑇 ∙ 𝑖𝑍𝑖
• the joint forces needed for each prismatic joint is:
0
𝑖 𝑇 𝑖
𝜏𝑖 = 𝑓𝑖 ∙ 𝑍𝑖 𝑖
where 𝑍𝑖 = 0 always.
1
d) ) The jacobian, relative to the last frame {n}:
• Work done in Cartesian – space terms is the same as work done in joint – space terms.
• Virtual work is defined as the dot product of a force – torque vector and a displacement vector as follows:
𝐹. 𝛿𝑋 = 𝜏. 𝛿𝜃 , this equation can also be written as: 𝐹𝑇. 𝛿𝑋 = 𝜏𝑇 . 𝛿𝜃
• Since the jacobian is defined as 𝛿𝑋 = 𝐽. 𝛿𝜃 , then we can rewrite the above equation as:
𝐹𝑇. 𝐽. 𝛿𝜃 = 𝜏𝑇. 𝛿𝜃
• Dividing both sides by 𝛿𝜃 yields:
𝐹𝑇 . 𝐽 = 𝜏𝑇
• We can transpose both sides to get a direct r/ship b/n the Cartesian force – moment vector and the joint torques as follows:
𝜏 = 𝐽𝑇 ∙ 𝐹

57
Jacobian using static forces/moments propagation method

d) The Jacobian, relative to the last frame {n}:

𝜏1 𝑚×1
𝜏1 𝑛𝐽𝑇
𝑚𝑥1 3𝑥1
• For linear velocities jacobian: 𝜏 = ⋮ = 𝑙𝑖𝑛𝑒𝑎𝑟 . 𝑛𝐹
𝜏𝑚

𝜏1 𝑚×1
𝜏1 𝑛𝐽𝑇
𝑚𝑥1 3𝑥1
• For angular velocities jacobian: 𝜏 = ⋮ = 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 . 𝑛𝑁
𝜏𝑚

𝜏1 𝑚×1
𝜏1 𝑛𝐹 6𝑥1
• For general velocities jacobian:𝜏 = ⋮ = 𝑛𝐽𝑇 𝑚𝑥6 .
𝑛𝑁
𝜏𝑚

58
• E.g. for the 2R robot, calculate the forces and moments acting
on each joint, then calculate the joint torques and find the jacobian matrix
relative to frame {0} and {3}
if 3𝐹 = 𝑛 𝑓𝑥 𝑓𝑦 0 𝑇 and 3𝐹 = 3 0 0 𝑛𝑧 𝑇

𝑐1 −𝑠1 0 0 𝑐2 −𝑠2 0 𝑙1 1 0 0 𝑙2
0 𝑠1 𝑐1 0 0 1 𝑠2 𝑐2 0 0 0 0 1 0 0
1 𝑇 = 2 𝑇 = 1 𝑇 =
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

𝑐12 −𝑠12 0 𝐿1𝑐1 + 𝐿2𝑐12 i αi-1 ai-1 di θi

0 𝑠12 𝑐12 0 𝐿1𝑠1 + 𝑙2𝑠12
3𝑇 = 1 0 0 0 Θ1
0 0 1 0
0 0 0 1 2 0 L1 0 Θ2
3 0 L2 0 0

59
• Soln. now we start the force/moment propagation with the end-effector frame {3}, whose force/moments is given.
• Frame {3}:
𝑓𝑥 0
3𝑓 = 3𝐹 = 𝑓 3𝑛 = 3𝑁 = 0
3 𝑦 3
𝑓𝑧 𝑛𝑧
• Frame {2}:
1 0 0 𝑓𝑥 𝑓𝑥
2𝑓 2
2 = 3𝑅 ∙ 3𝑓3 = 0 1 0 𝑓𝑦 = 𝑓𝑦
0 0 1 0 0

1 0 0 0 𝐿2 𝑓𝑥 0
2 0
2𝑛
2 = 3𝑅 ∙ 3𝑛3 + 2𝑝3 × 2𝑓2 = 0 1 0 0 + 0 × 𝑓𝑦 =
0 0 1 𝑛𝑧 0 0 𝑛𝑧 + 𝐿2 ∙ 𝑓𝑦
Frame {1}:
𝑐2 −𝑠2 0 𝑓𝑥 𝑐2𝑓𝑥 − 𝑠2𝑓𝑦
1𝑓 1
1 = 2𝑅 ∙ 2𝑓2 = 𝑠2 𝑐2 0 𝑓𝑦 = 𝑠2𝑓𝑥 + 𝑐2𝑓𝑦
0 0 1 0 0

𝑐2 −𝑠2 0 0 𝐿1 𝑐2𝑓𝑥 − 𝑠2𝑓𝑦 0

1𝑛 = 1𝑅 ∙ 2𝑛 + 1𝑝 × 1𝑓 = 0 + 0 × 𝑠2𝑓𝑥 + 𝑐2𝑓𝑦 = 0
1 2 2 2 1 𝑠2 𝑐2 0
0 0 1 𝑛𝑧 + 𝐿2 ∙ 𝑓𝑦 0 0 𝑛𝑧 + 𝐿2 𝑓𝑦 + 𝐿1 𝑐2𝑓𝑦 + 𝐿1 𝑠2𝑓𝑦
• No need to proceed to frame {0}as there are no joints moving the ground frame.

60
• To find the joint torque (all joints are revolute)
Joint 1: revolute:
0
𝜏1 = 1𝑛𝑇 ∙ 1𝑍1 = 0 0 𝐿1 𝑠2𝑓𝑥 + 𝐿1 𝑐2𝑓𝑦 + 𝐿2 𝑓𝑦 + 𝑛𝑧
1 0 = 𝐿1 𝑠2𝑓𝑥 + 𝐿1 𝑐2𝑓𝑦 + 𝐿2 𝑓𝑦 + 𝑛𝑧
1
Joint 2: revolute:
0
𝜏2 = 2𝑛2𝑇 ∙ 2𝑍2 = 0 0 𝐿2 𝑓𝑦 + 𝑛𝑧 0 = 𝐿2 𝑓𝑦 + 𝑛𝑧
1
𝜏1 𝐿1 𝑠2𝑓𝑥 + 𝐿1 𝑐2𝑓𝑦 + 𝐿2 𝑓𝑦 + 𝑛𝑧
• The joint force/ torque matrix becomes: 𝜏 = 𝜏 =
2 𝐿2 𝑓𝑦 + 𝑛𝑧
𝑓
𝜏1 𝐿 𝑠2 𝐿1 𝑐2 + 𝐿2 0 𝑥
• For linear Jacobian: 𝜏 = 𝜏 = 1 𝑓
2 0 𝐿2 0 𝑦
0
3𝐽𝑇
𝑙𝑖𝑛𝑒𝑎𝑟
𝐿1 𝑠2 0
3𝐽 = 𝐿1 𝑐2 + 𝐿2
𝑙𝑖𝑛𝑒𝑎𝑟 𝐿2
0 0

61
• To find the linear jacobian relative to frame{0}:
𝑐12 −𝑠12 0 𝐿1 𝑠2 0 𝐿1 𝑠2𝑐12 − 𝐿1 𝑐2𝑠12 − 𝐿2 𝑠12 −𝐿2 𝑠12
0𝐽 0 3
𝑙𝑖𝑛𝑒𝑎𝑟 = 3𝑅 𝐽𝑙𝑖𝑛𝑒𝑎𝑟 = 𝑠12 𝑐12 0 𝐿1 𝑐2 + 𝐿2 𝐿2 = 𝐿1 𝑠2𝑠12 + 𝐿1 𝑐2𝑐12 + 𝐿2 𝑐12 𝐿2 𝑐12
0 0 1 0 0 0 0

−𝐿1 𝑠1 − −𝐿2 𝑠12 −𝐿2 𝑠12

= 𝐿1 𝑐1 + 𝐿2 𝑐12 𝐿2 𝑐12
0 0
𝜏1 𝐿1 𝑠2𝑓𝑥 + 𝐿1 𝑐2𝑓𝑦 + 𝐿2 𝑓𝑦 + 𝑛𝑧 0
0 0 1 0
• For the angular Jacobian: 𝜏 = 𝜏 = ≡
2 𝐿2 𝑓𝑦 + 𝑛𝑧 0 0 1 𝑛
𝑧
0 0
• We can extract the angular Jacobian as: 3𝐽
𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = 0 0
1 1
• To find the angular jacobian relative to frame {0}:
𝑐12 −𝑠12 0 0 0 0 0
• 0𝐽𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = 03𝑅 ∙ 3𝐽𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = 𝑠12 𝑐12 0 0 0 = 0 0
0 0 1 1 1 1 1

62
• Summery of the linear and angular jacobian in frames{0} and{3}
−𝐿1 𝑠1 − −𝐿2 𝑠12 −𝐿2 𝑠12 0 0 𝐿1 𝑠2 0 0 0
0𝐽 0𝐽 3𝐽 3𝐽
𝑙𝑖𝑛𝑒𝑎𝑟 = 𝐿1 𝑐1 + 𝐿2 𝑐12 𝐿2 𝑐12 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = 0 0 𝑙𝑖𝑛𝑒𝑎𝑟 = 𝐿1 𝑐2 + 𝐿2 𝐿2 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = 0 0
0 0 1 1 0 0 1 1
• To find the general jacobian relative to frames {3} and {0}:

𝐿1 𝑠2 0
𝐿1 𝑐2 + 𝐿2 𝐿2
3𝐽 𝐿1 𝑠2 0
3𝐽 𝑙𝑖𝑛𝑒𝑎𝑟 0 0
= 3𝐽 = = 𝐿1 𝑐2 + 𝐿2 𝐿2 and
𝑎𝑛𝑔𝑢𝑙𝑎𝑟 0 0
1 1
0 0
1 1 −𝐿1 𝑠1 − −𝐿2 𝑠12 −𝐿2 𝑠12
𝐿1 𝑐1 + 𝐿2 𝑐12 𝐿2 𝑐12
3𝐽 −𝐿1 𝑠1 − −𝐿2 𝑠12 −𝐿2 𝑠12
0𝐽 𝑙𝑖𝑛𝑒𝑎𝑟 0 0
= 3𝐽 = = 𝐿1 𝑐1 + 𝐿2 𝑐12 𝐿2 𝑐12
𝑎𝑛𝑔𝑢𝑙𝑎𝑟 0 0
1 1
0 0
1 1

63
singularity

• Singularities happen when the jacobian is not invertible, and it can be:
 Momentary singularity
 Permanente singularity
• For a square jacobian, 𝑉 = 𝐽𝜃ሶ ⇒ 𝜃ሶ = 𝐽−1 𝑉, 𝑤ℎ𝑖𝑐ℎ 𝑐𝑎𝑛 𝑏𝑒 used for inverse kinematics: given 𝑉 and find 𝜃.ሶ
 If det 𝐽 ≠ 0  𝐽 is investable.
 If det 𝐽 ≈ 0  𝐽 is investable, but dangerous joint velocities will occur
 If det 𝐽 = 0   𝐽 is not investable
a) Momentary singularities:
when the robot is at a configuration that is causing a momentary loss of one or more
degrees of freedom.
• Workspace – boundary singularity: when the arm is fully stretched out or full folded into it self (at or
near the boundary of the work space).
• Work space – interior singularity: when two or more joints of the same type are lined up.

64
• E.g. for the 2R planar robot, when does the robot becomes singular? Find the inverse of the jacobian for the
robot and the joint velocities (in terms of Cartesian velocities of the end – effector relative to it self) if:
Soln. for the robot to be singular det 𝐽 = 𝐿1 𝑠2. 𝐿2 − 𝐿1 𝑐2 + 𝐿2 . 0 = 𝐿1 𝐿2 𝑠2
For the above expression to be zero, s2 has to be zero  𝜃2 = 0 𝑜𝑟 180 𝑑𝑒𝑔𝑟𝑒𝑒.
The inverse of the jacobian is:
1 𝐿1 𝑠2 0 3𝐽
𝐿1 𝑠2 0
3𝐽−1 = 𝑙𝑖𝑛𝑒𝑎𝑟 =
𝐿1 𝐿2 𝑠2 𝐿1 𝑐2 + 𝐿2 𝐿1 𝐿2 𝑠2 𝐿1 𝑐2 + 𝐿2 𝐿2
Since:
ሶ
ሶ𝜃 = 𝐽−1 ∙ 3𝑉 ⇒ 𝜃1 = 1 𝐿1 𝑠2 0
∙
𝑥ሶ
𝜃ሶ2 𝐿1 𝐿2 𝑠2 𝐿1 𝑐2 + 𝐿2 𝐿1 𝐿2 𝑠2 𝑦ሶ

65
Singularities

b) Permanente singularities: when the number of joints in the robot are less than the number of Cartesian
degrees of freedom that need to be controlled at the end –effector (the jacobian is non – square matrix).
• to avoid a permanent singularity, choose the right Cartesian coordinates to be controlled such that a
representation of each joint velocity in a square jacobian is present to take advantage of all joints in the
robot.
• If any of the joint velocities has no representation in the jacobian, permanent singularity will occur

66
67
68
3.Trajectory planning and generation
• Trajectories for joint motion or end effector motion in space can be of many forms, including
linear trajectories and polynomial trajectories.

69
Linear trajectory for joints
a) Discrete trajectories (via points):
• Given ϴ ini, ϴ fin , and number of steps((n)
ϴ 𝑓𝑖𝑛 − ϴ 𝑖𝑛𝑖
• Trajectory step ϴ stp: ϴ 𝑠𝑡𝑝 =
𝑛
• Trajectory is : ϴ 𝑖𝑛𝑖 −>
ϴ𝑖𝑛𝑖 + ϴ𝑠𝑡𝑝 −> ϴ𝑖𝑛𝑖 + 2ϴ𝑠𝑡𝑝 −> ϴ𝑖𝑛𝑖 + 3ϴ𝑠𝑡𝑝 −> … .−> ϴ𝑓𝑖𝑛
• Do the same procedure for every joint to find the joint trajectories for the robot, if joint motion
control is used.
b) Continuous trajectories (lines):
• Given ϴ ini, ϴ fin , and total motion time (ttotal)
ϴ 𝑓𝑖𝑛 − ϴ 𝑖𝑛𝑖
• Joint velocity : ( ): and joint acceleration : 𝜃ሷ = 0.
𝑡 𝑡𝑜𝑡𝑎𝑙
• Trajectory equation is: ϴ = ϴ𝑖𝑛𝑖 + 𝜃ሶ ∙ 𝑡 (where ”t” is the time at any instance).

70
Linear trajectory for Cartesian coordinate of the end- effector
a) X-Y-Z coordinates’ discrete trajectories ( via points):
• Given Xini, Xfin, and number of steps (n)
X 𝑓𝑖𝑛 −X𝑖𝑛𝑖
• Trajectory step (Xstp ): 𝑋𝑠𝑡𝑝 =
𝑛
• Trajectory is : X 𝑖𝑛𝑖 →
𝑋𝑖𝑛𝑖 + X𝑠𝑡𝑝 −> X𝑖𝑛𝑖 + 2X𝑠𝑡𝑝 −> X𝑖𝑛𝑖 + 3X𝑠𝑡𝑝 −> … .−> X𝑓𝑖𝑛
• Do the same procedure for Y and Z coordinates
b) X-Y-Z coordinate trajectories (lines):
• Given Xini, Xfin, and total motion of time (ttotal)
X 𝑓𝑖𝑛 −X𝑖𝑛𝑖
• The velocity : 𝑋ሶ = and the acceleration: 𝑋ሷ = 0.
ttotal
• Trajectory equation is: X = X𝑖𝑛𝑖 + 𝑋ሶ ∙ 𝑡 (where ”t” is the time at any instance).
• Do the some procedure for Y and Z coordinate to find Cartesian trajectories.

71
Linear trajectory for Cartesian coordinate of the end- effector
c) α – β – ϒ orientation coordinates’ trajectory's:
• To find the trajectory for the end-effector orientation, the rotation matrix (orientation change b/n the initial and
desired orientation) should be translated to a single angle –axis equivalent rotation.
• Each trajectory step can then be translated back into a rotation matrix that represents the orientation change from
the initial orientation to the orientation of that step.
𝑇
i. Find the rotation matrix b/n the initial and final rotation matrices: R=𝑅𝑖𝑛𝑖 ∙ 𝑅𝑓𝑖𝑛
ii. Find the single angle representation of R:
ϕ = 𝐴𝑡𝑎𝑛2( (𝑟32 − 𝑟23 )2 +(𝑟13 − 𝑟31 )2 +(𝑟21 − 𝑟12 )2 , 𝑟11 + 𝑟22 + 𝑟33 − 1)
iii. Find the single axis representation of R: sϕ = 𝑠𝑖𝑛ϕ, 𝑐ϕ = 𝑐𝑜𝑠ϕ, 𝑣ϕ = 1 − 𝑐𝑜𝑠ϕ.

1 𝑟32 − 𝑟23
1
If : ϕ ≈ 0° → 𝑘 = 0 , 𝑖𝑓 ∶ 0° ≪ ϕ ≪ 90° → 𝑘 = 2sϕ 𝑟13 − 𝑟31 ,
0 𝑟21 − 𝑟12
𝑟21 +𝑟12 𝑟13 +𝑟31
𝑠𝑖𝑔𝑛(𝑟32 − 𝑟23) ∙ 𝑘𝑥 ≫ 𝑘𝑦 , 𝑘𝑧 → 𝑘𝑦 = , 𝑎𝑛𝑑 𝑘𝑧 =
𝑟11−𝑐ϕ 2𝑘𝑥 𝑣ϕ 2𝑘𝑥 𝑣ϕ
1 𝑟21 +𝑟12 𝑟32 +𝑟23
If :ϕ ≫ 90° → 𝑘 = 𝑠𝑖𝑔𝑛(𝑟13 − 𝑟31 ) ∙ 𝑟22−𝑐ϕ , 𝑘𝑦 ≫ 𝑘𝑥 , 𝑘𝑧 → 𝑘𝑥 = 2𝑘𝑦 𝑣ϕ
, 𝑎𝑛𝑑 𝑘𝑧 = 2𝑘𝑦 𝑣ϕ
2sϕ
𝑠𝑖𝑔𝑛(𝑟21 − 𝑟12 ) ∙ 𝑟33−𝑐ϕ 𝑘 ≫ 𝑘 , 𝑘 → 𝑘 = 𝑟13 +𝑟31
, 𝑎𝑛𝑑 𝑘𝑦 =
𝑟32 +𝑟23
𝑧 𝑥 𝑦 𝑥 2𝑘𝑧 𝑣ϕ 2𝑘𝑧 𝑣ϕ

72
Linear Trajectory for Cartesian coordinate of the end – effector
iv. Generate the orientation trajectory delta angles:
ϕ
• For a discrete trajectory with (n) steps, the step angle is: ϕ𝑠𝑡𝑝 =
𝑛
and the trajectory delta angles are : 0 → ϕ𝑠𝑡𝑝 → 2ϕ𝑠𝑡𝑝 → 3ϕ𝑠𝑡𝑝 → ⋯ ϕ
• For a continuous trajectory (line) with total motion time (𝑡𝑡𝑜𝑡𝑎𝑙 ):
ϕ
ϕሶ = 𝑎𝑛𝑑 ϕሷ = 0
𝑡𝑡𝑜𝑡𝑎𝑙
And the trajectory delta angle equation is:
ϕ = ϕሶ ∙ 𝑡
v. Convert each of the orientation trajectory delta angles back to delta rotation matrices for each step : sϕ =
𝑠𝑖𝑛ϕ, 𝑐ϕ = 𝑐𝑜𝑠ϕ, 𝑣ϕ = 1 − 𝑐𝑜𝑠ϕ.
𝑘𝑥 𝑘𝑥 𝑣ϕ + 𝑐ϕ 𝑘𝑦 𝑘𝑥 𝑣ϕ − 𝑘𝑧 𝑠ϕ 𝑘𝑧 𝑘𝑥 𝑣ϕ + 𝑘𝑦 𝑠ϕ
∆𝑅𝑗 (ϕ𝑗 ) = 𝑘𝑥 𝑘𝑦 𝑣ϕ + 𝑘𝑧 𝑠ϕ 𝑘𝑦 𝑘𝑦 𝑣ϕ + 𝑐ϕ 𝑘𝑧 𝑘𝑦 𝑣ϕ − 𝑘𝑥 𝑠ϕ ,
𝑘𝑥 𝑘𝑧 𝑣ϕ − 𝑘𝑦 𝑠ϕ 𝑘𝑦 𝑘𝑧 𝑣ϕ + 𝑘𝑥 𝑠ϕ 𝑘𝑧 𝑘𝑧 𝑣ϕ + 𝑐ϕ
(ϕ𝑗 𝑖𝑠 𝑡ℎ𝑒 𝑡𝑟𝑎𝑗𝑒𝑐𝑡𝑜𝑟𝑦 𝑑𝑒𝑙𝑡𝑎 𝑎𝑛𝑔𝑙𝑒).
• The above delta rotation matrix calculation should be repeated for each discrete trajectory via point to produce the
following delta rotations: ∆𝑅1 → ∆𝑅2 → ∆𝑅3 → ⋯ → ∆𝑅𝑛+1
vi. Construct the resulting orientation trajectory rotation matrices:
𝑅𝑡𝑟𝑎𝑗𝑒𝑐𝑡𝑜𝑟𝑦 = 𝑅𝑖𝑛𝑖 ∙ ∆𝑅1 → 𝑅𝑖𝑛𝑖 ∙ ∆𝑅2 → 𝑅𝑖𝑛𝑖 ∙ ∆𝑅3 → ⋯ → 𝑅𝑖𝑛𝑖 ∙ ∆𝑅𝑛+1
73
Examples on Trajectory Generation
• Eg1. Generate a linear trajectory equation on travel from Ti to Tf in 5 secs, where:

0.866 −0.5 0 2 0.866 −0.5 0 10

0.5 0.866 0 6 0.5 0.866 0 −2
Ti = , and Tf =
0 0 1 10 0 0 1 15
0 0 0 1 0 0 0 1

Soln. since the total time is given, we can generate a continuous Cartesian trajectory. Notice that the initial and final
transformations have the same rotation matrix, and only the translation portion changes.
• The velocities for the Cartesian coordinates are:
10−2 −2−6 15−10
𝑥ሶ = 5
= 1.6 𝑢𝑛𝑖𝑡𝑠/𝑠𝑒𝑐 , 𝑦ሶ = 5
= 1.6 𝑢𝑛𝑖𝑡𝑠/𝑠𝑒𝑐 , 𝑧ሶ = 5
= −1 𝑢𝑛𝑖𝑡𝑠/𝑠𝑒𝑐
• The trajectory equations for x,y,z coordinates are:
𝑥 = 2 + 1.6𝑡; 𝑦 = 6 − 1.6𝑡 ; 𝑧 = 10 + 𝑡.
• The trajectory equation in a matrix form is:

0.866 −0.5 0 2 + 1.6𝑡

0.5 0.866 0 6 − 1.6𝑡
𝑇𝑡𝑟𝑎𝑗𝑒𝑐𝑡𝑜𝑟𝑦 =
0 0 1 10 + 𝑡
0 0 0 1

74
Examples on Trajectory Generation

• Eg2: Generate a Cartesian trajectory to travel from Ti to Tf in 5 steps, where:

0.8324 −0.3448 0.4339 2 0.5721 −0.4156 0.7071 10

0.5318 0.7171 −0.4505 6 0.7893 0.0446 −0.6124 −2
Ti = and Tf =
−0.1558 0.6057 0.7803 10 0.2230 0.9084 0.3536 15
0 0 0 1 0 0 0 1
Soln: since the number of steps is given, we can generate a discrete trajectory for the Cartesian coordinate of
the end effector.
• For the Cartesian linear coordinates, the trajectory steps are:
10−2 −2−6 15−10
𝑥𝑠𝑡𝑝 = = 1.6 𝑢𝑛𝑖𝑡𝑠, 𝑦𝑠𝑡𝑝 = = −1.6 𝑢𝑛𝑖𝑡𝑠, z𝑠𝑡𝑝 = = 1 𝑢𝑛𝑖𝑡𝑠
5 5 5
The trajectory for X coordinate is : 2 -> 3.6 -> 5.2 -> 6.8 -> 8.4 -> 10.
The trajectory for Y coordinate is : 6 -> 4.4 -> 2.8 -> 1.2 -> -0.4 -> -2.
The trajectory for Z coordinate is : 10 -> 11 -> 12 -> 13 -> 14 -> 15.

75
• For the Cartesian orientation coordinates, the orientation coordinates, the rotation matrix from initial to final orientation is:

0.8324 −0.3448 0.4339 0.5721 −0.4156 0.7071

𝑅= 𝑅𝑖𝑇 ∙ 𝑅𝑓 = 0.5318 0.7171 −0.4505 ∙ 0.7893 0.0446 −0.6124
−0.1558 0.6057 0.7803 0.2230 0.9084 0.3536

0.8612 −0.4638 0.2078

= 0.5038 0.7255 −0.4688
0.066 0.5084 0.8585
 The single angle representation of R is:
ϕ = 𝐴𝑡𝑎𝑛2( (𝑟32 − 𝑟23 )2 +(𝑟13 − 𝑟31 )2 +(𝑟21 − 𝑟12 )2 , 𝑟11 + 𝑟22 + 𝑟33 − 1)

ϕ = 𝐴𝑡𝑎𝑛2( 0.5084 + 0.4688 2 + 0.2078 − 0.0666 2 + 0.5038 + 0.4638 2 , 0.8612 + 0.7255 + 0.8585 − 1)
Φ = 0.7632 radians or 43.73°
- Since sϕ = sin(0.7632) = 0.6912, cϕ = cos(0.7632) = 0.7226, vϕ = 1 − cos 0.7632 = 0.2774
𝑘1 𝑟32 − 𝑟23 0.5084 + 0.4688 0.7069
1 1
- And since 0 << ϕ << 90 => K = 𝑘2 = 2𝑠ϕ 𝑟13 − 𝑟31 = 2(0.6912) 0.2078 − 0.0666 = 0.1021
𝑘3 𝑟21 − 𝑟12 0.5038 + 0.4638 0.6999
ϕ 0.7632
- The step angle for the trajectory is ϕ𝑠𝑡𝑝 = 𝑛 = 5 = 0.1526 𝑟𝑎𝑑𝑖𝑎𝑛𝑠.
- The trajectory delta angles will then be:
- 0 -> 0.1526 -> 0.3053 -> 0.4579 -> 0.6106 -> 0.7632.
76
• Eg3 generate linear trajectories for a RRP robot to travel from qi to qf in 5 steps, where:

45 100
𝑞𝑖 = 22 , 𝑎𝑛𝑑 𝑞𝑓 = 9.5
210 70
Soln; since the number of steps is given, we can generate a discrete trajectory for the joint values. The trajectory steps for
the joints are:
ϴ 𝑓𝑖𝑛 − ϴ 𝑖𝑛𝑖 100 − 45 9.5 − 22 70 − 210
ϴ 1 − 𝑠𝑡𝑝 = = = 11 𝑑𝑒𝑔𝑟𝑒𝑒𝑠, ϴ 2 − 𝑠𝑡𝑝 = = −2.5 degrees, ϴ 3 − 𝑠𝑡𝑝 =
𝑛 5 5 5
- The trajectory for ϴ 1 is: 45 -> 56 -> 67 -> 78 -> 89 -> 100.
- The trajectory for ϴ 2 is: 22 -> 19.5 -> 17 -> 14.5 -> 12 -> 9.5.
- The trajectory for ϴ 3 is: 210 -> 182 -> 154 -> 126 -> 98 -> 70.
- The following is the trajectory in the form of joint vectors:
45 56 67 78 89 100
- 𝑞𝑡𝑟𝑎𝑗𝑒𝑐𝑡𝑜𝑟𝑦 = 22 → 19.5 → 17 → 14.5 → 12 → 9.5
210 182 154 126 98 70

77
• Eg4 : generate linear joint trajectories for a RRP robot to travel from qi to qf in 5 seconds, where

45 100
𝑞𝑖 = 22 , 𝑎𝑛𝑑 𝑞𝑓 = 9.5
210 70
Soln: since the total time is given, we can generate a continuous trajectories for the joint values. The velocities
for the joint are:
ϴ 𝑓𝑖𝑛 − ϴ 𝑖𝑛𝑖 100−45 𝑑𝑒𝑔 9.5−22 2.5𝑑𝑒𝑔 70−210 𝑢𝑛𝑖𝑡
𝜃1ሶ = = = 11 , 𝜃2ሶ = =− , 𝜃3ሶ = = −28 .
𝑡 𝑡𝑜𝑡𝑎𝑙 5 𝑠𝑒𝑐 5 𝑠𝑒𝑐 5 𝑠𝑒𝑐
- The trajectory equation for 𝜃1 is : 𝜃1 = 45 + 11𝑡
- The trajectory equation for 𝜃2 is : 𝜃2 = 22 − 2.5𝑡.
- The trajectory equation for 𝜃3 is : 𝜃3 = 210 − 28𝑡
- The following is the trajectory equation in the form of joint vectors:
-
45 + 11. 𝑡
𝑞𝑡𝑟𝑎𝑗𝑒𝑐𝑡𝑜𝑟𝑦 = 22 − 2.5. 𝑡
210 − 28. 𝑡

78
79