Robotics Lec3

‫رباتیک‬
‫ مقدمه‬: ‫فصل اول‬
AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 1

Trajectory Generation
 Assume the first & desired position of the end effector is given.
 Using IK, the first & desired position

of all joint can be determined.
 What is the time-based motion

from the first position to the desired one?
 Trajectory Generation:
 Compute the motion functions of each joint as a smooth function of
time, 𝜃(𝑡), to cause a manipulator to move in a smooth, controlled
fashion.
 Coordination:
 Each joint starts and ends its motion
at the same time.
 Q: if not?
Trajectory Generation
 A path is described not only by a desired destination but also by some
intermediate locations, or via points (Path Planning).
 Spline is sometimes used to refer to a smooth function that passes

through a set of via points.
 Cartesian Trajectory Generation:

 To follow a straight line through space, the desired motion must be
converted to an equivalent set of joint motions.
 Question ?

Position Control
 Compute the required torques (𝐹, 𝜏) that will cause the desired motion
(𝜃𝑑 , 𝑋𝑑 ).
 The problem of dynamics is central to the design of such algorithms, but

not a solution alone.
 The primary concern is to compensate for errors that tend to perturb the
system from the desired trajectory.
 Sources of errors:
 Knowledge of the parameters
 Disturbances
 Noises and …
 The manipulator dynamics are highly nonlinear.
 The nonlinear control techniques promise better performance than do

simpler linear schemes.
‫فصل دوم‬
Spatial Descriptions and Transformations

Outlines
 Descriptions: Positions, Orientations and Frames

 Mappings: Changing Description from Frame to Frame
 Operators
 Transformation Arithmetic
 More on Representation of Orientation
Euler Angles
Fixed Angles
Equivalent Angle-Axis
Euler Parameters
 Transformation of Free Vectors

Descriptions: Positions, Orientations and Frames
 To define position and orientation, we must define coordinate systems
and conventions for its representation.
 There is a universe coordinate system to which everything can be

referenced.
 A description is used to specify attributes of various objects.
 Objects are parts, tools, and the manipulator itself.
 Description:
 Positions
 Orientations
 An entity that contains both of these descriptions: the frame

← Outline
 Description of a Position
 Once a coordinate system is established, locate any point with a 3 × 1
position vector.
 Vectors must be tagged identifying which coordinate system they are

defined within, e,g, 𝐴𝑃 .
𝐴
 𝑃 have numerical values that indicate distances along the axes of {A}
(Projection).
𝐴
𝑝𝑥
𝐴
𝑃 = 𝐴𝑝𝑦
𝐴
𝑝𝑧
𝐴
 𝑝𝑥 = 𝐴𝑃 . 𝑋෠𝐴
𝐴
 𝑝𝑦 = 𝐴𝑃 . 𝑌෠𝐴
𝐴
 𝑝𝑧 = 𝐴𝑃 . 𝑍መ𝐴

← Outline
 Description of a Position
 Vector Norm:
𝐴 𝐴 𝐴 1/2 𝐴 2 𝐴 2 𝐴 2 1/2
𝑃 = 𝑃. 𝑃 = 𝑝𝑥 + 𝑝𝑦 + 𝑝𝑧
 It is invariant of the frame.
𝐴 𝐴 𝐴 1/2 𝐵 𝐵 1/2 𝐵
𝑃 = 𝑃. 𝑃 = 𝑃. 𝑃 = 𝑃
 Q: Condition ?

← Outline
 Description of an Orientation
 Positions of points are described with vectors and orientations of
bodies are described with an attached coordinate system.
 Attach a coordinate system to the body and then give a description of this
coordinate system relative to the reference system.
 One way to describe the coordinate system {B}:

 Write its unit vectors 𝑋෠𝐵 , 𝑌෠𝐵 , 𝑍መ𝐵 in terms of the coordinate system
{A}.
 They are called 𝐴𝑋෠𝐵 , 𝐴𝑌෠𝐵 , 𝐴𝑍መ𝐵 .

← Outline
 Description of an Orientation
 Each component is the dot product of a pair of unit vectors.
𝑋෠𝐵 . 𝑋෠𝐴 𝐶𝑜𝑠 𝑋෠𝐵 , 𝑋෠𝐴 𝑟11
𝐴෠
𝑋𝐵 = 𝑋෠𝐵 . 𝑌෠𝐴 = 𝐶𝑜𝑠 𝑋෠𝐵 , 𝑌෠𝐴 = 𝑟21
𝑋෠𝐵 . 𝑍መ𝐴 𝑟31
𝐶𝑜𝑠 𝑋෠𝐵 , 𝑍መ𝐴
𝑌෠𝐵 . 𝑋෠𝐴 𝐶𝑜𝑠 𝑌෠𝐵 , 𝑋෠𝐴 𝑟12
𝐴෠
𝑌𝐵 = 𝑌෠𝐵 . 𝑌෠𝐴 = 𝐶𝑜𝑠 𝑌෠𝐵 , 𝑌෠𝐴 = 𝑟22
𝑌෠𝐵 . 𝑍መ𝐴 𝑟32
𝐶𝑜𝑠 𝑌෠𝐵 , 𝑍መ𝐴
𝑍መ𝐵 . 𝑋෠𝐴 𝐶𝑜𝑠 𝑍መ𝐵 , 𝑋෠𝐴 𝑟13
𝐴መ
𝑍𝐵 = 𝑍መ𝐵 . 𝑌෠𝐴 = 𝐶𝑜𝑠 𝑍መ𝐵 , 𝑌෠𝐴 = 𝑟23
𝑍መ𝐵 . 𝑍መ𝐴 𝑟33
𝐶𝑜𝑠 𝑍መ𝐵 , 𝑍መ𝐴
 Stack these three unit vectors together as the columns of a 3 × 3 matrix.
𝑋෠𝐵 . 𝑋෠𝐴 𝑌෠𝐵 . 𝑋෠𝐴 𝑍መ𝐵 . 𝑋෠𝐴 𝑟11 𝑟12 𝑟13
𝐴 𝐴 𝐴෠ 𝐴
𝑅𝐵 = 𝑋෠𝐵 𝑌𝐵 𝑍መ𝐵 = 𝑋෠𝐵 . 𝑌෠𝐴 𝑌෠𝐵 . 𝑌෠𝐴 𝑍መ𝐵 . 𝑌෠𝐴 = 𝑟21 𝑟22 𝑟23
𝑋෠𝐵 . 𝑍መ𝐴 𝑌෠𝐵 . 𝑍መ𝐴 𝑍መ𝐵 . 𝑍መ𝐴 𝑟31 𝑟32 𝑟33
 It is called Rotation Matrix (R).

 Components of rotation matrices are direction cosines.
← Outline
 Properties of Rotation Matrix
1) Orthonormal
 The columns all have unit magnitude, and, these unit vectors are
orthogonal.
𝐴 𝐴෠ 𝐴
𝑋෠𝐵 = 𝑌𝐵 = 𝑍መ𝐵 = 1
𝐴
𝑋෠𝐵 . 𝐴𝑌෠𝐵 = 𝐴෠
𝑌𝐵 . 𝐴𝑍መ𝐵 = 𝐴
𝑋෠𝐵 . 𝐴𝑍መ𝐵 = 0
2) The rows of the matrix are the unit vectors of {A} expressed in {B}.
𝐵 ෠𝑇
𝑋෠𝐵 . 𝑋෠𝐴 𝑌෠𝐵 . 𝑋෠𝐴 𝑍መ𝐵 . 𝑋෠𝐴 𝑋𝐴
𝐴 𝐴෠ 𝐴෠ 𝐴 𝐵 ෠𝑇
𝑅𝐵 = 𝑋𝐵 𝑌𝐵 𝑍መ𝐵 = 𝑋෠𝐵 . 𝑌෠𝐴 𝑌෠𝐵 . 𝑌෠𝐴 𝑍መ𝐵 . 𝑌෠𝐴 = 𝑌𝐴
𝑋෠𝐵 . 𝑍መ𝐴 𝑌෠𝐵 . 𝑍መ𝐴 𝑍መ𝐵 . 𝑍መ𝐴 𝐵 መ𝑇
𝑍𝐴
𝑇
= 𝐵
𝑋෠𝐴 𝐵෠
𝑌𝐴 𝐵
𝑍መ𝐴 = 𝐵𝑅𝐴𝑇
 The description of frame {A} relative to {B} ( 𝐵𝑅𝐴 ) is the transpose of

𝐴
𝑅𝐵 .
𝐵
𝑅𝐴 = 𝐴𝑅𝐵𝑇
← Outline
 Properties of Rotation Matrix
3) From linear algebra, the inverse of a orthonormal matrix is equal to its
transpose.
𝐴 𝑇
𝑅𝐵 = 𝐴𝑅𝐵−1
 To demonstrate:
𝐴 ෠𝑇
𝑋𝐵
𝐴 𝑇 𝐴 𝐴 ෠𝑇 𝐴 𝐴෠ 𝐴
𝑅𝐵 𝑅𝐵 = 𝑌𝐵 𝑋෠𝐵 𝑌𝐵 𝑍መ𝐵 = 𝐼3
𝐴 መ𝑇
𝑍𝐵
 where 𝐼3 is the 3 × 3 identity matrix.
4)
𝐴
𝑑𝑒𝑡 𝑅𝐵 = 1
 Q: Why?
5) It can be expressed by Only 3 independent numbers.
 Q: Why
← Outline
 Description of a Frame
 For a frame, both position and orientation should be determined.
 For convenience, the point whose position described is chosen as the

origin of the body-attached frame.
 The description of a frame: a position vector and a rotation matrix.
 Frame {B} is described by 𝐴𝑅𝐵 and 𝐴𝑃𝐵𝑂𝑅𝐺 .

𝐵 = { 𝐴𝑅𝐵 , 𝐴𝑃𝐵𝑂𝑅𝐺 }
𝐴
 𝑃𝐵𝑂𝑅𝐺 is the vector that locates the origin of the frame {B}.
 A frame can be used as a description

of one coordinate system relative to another.

← Outline
 Description of a Frame
 Frames {A} and {B} are known relative to the universe frame.
 Frame {C} is known relative to frame {A}.
 Position and orientation can be represented as frames:

Position: a frame with identity rotation-matrix and position-vector
which locates the point
Orientation: a frame whose position-vector was the zero vector.

← Outline
Mappings: Changing Description from Frame to Frame
 Mapping between frames can be done by:
Translation
Rotation
General Transformation
 Translation
 A position defined by the vector 𝐵𝑃 .
 {A} has the same orientation as {B}
 {A} differs only by a translation, i.e. 𝐴𝑃𝐵𝑂𝑅𝐺 .
 Express this point in space in terms of frame {A}, i.e. 𝐴𝑃 .
𝐴
𝑃 = 𝐵𝑃 + 𝐴𝑃𝐵𝑂𝑅𝐺

← Outline
 Rotation
 The orientation of {B} is known relative to {A} i.e. 𝐴𝑅𝐵 .
 The origins of the two frames are coincident.
 The components of 𝐴𝑃 may be calculated by the projection as:
𝐴
𝑝𝑥
𝐴
𝑃 = 𝐴𝑝𝑦
𝐴
𝑝𝑧
𝐴
 𝑝𝑥 = 𝐵𝑋෠𝐴 . 𝐵𝑃
𝐴
 𝑝𝑦 = 𝐵𝑌෠𝐴 . 𝐵𝑃
𝐴
 𝑝𝑧 = 𝐵𝑍መ𝐴 . 𝐵𝑃
 Q: Why?

← Outline
Mappings: Frame to Frame
 Rotation
𝐴
𝑝𝑥
𝐴
𝑃 = 𝐴𝑝𝑦
𝐴
𝑝𝑧
𝐴
 𝑝𝑥 = 𝐵𝑋෠𝐴 . 𝐵𝑃
𝐴
 𝑝𝑦 = 𝐵𝑌෠𝐴 . 𝐵𝑃
𝐴
 𝑝𝑧 = 𝐵𝑍መ𝐴 . 𝐵𝑃
 Expressing in the matrix form:

𝐴 𝐵 ෠𝑇
𝑝𝑥 𝑋𝐴
𝐴 𝐴 𝐵 ෠𝑇 𝐵
𝑃= 𝑝𝑦 = 𝑌𝐴 𝑃
𝐴 𝐵 መ𝑇
𝑝𝑧 𝑍𝐴
 Note that the rows of rotation matrix 𝐴𝑅𝐵 are 𝐵𝑋෠𝐴𝑇 , 𝐵𝑌෠𝐴𝑇 and 𝐵𝑍መ𝐴𝑇 .
𝐴
𝑃 = 𝐴𝑅𝐵 𝐵𝑃

← Outline
 Rotation
 Example:
 Frame {B} that is rotated relative to frame {A} about 𝑍መ by 𝜃 degrees.
𝐵
 𝑃 is given.
 Find 𝐴𝑃?

← Outline
 General Transformation
 The orientation of {B} is known relative to {A} i.e. 𝐴𝑅𝐵 .
 The vector that locates {B}'s origin is called 𝐴𝑃𝐵𝑂𝑅𝐺 .
 Assume an intermediate frame {C}:

 the same origin of {B} and the same orientation of {A}.
 Describe 𝐵𝑃 in the intermediate frame.

𝐶
𝑃 = 𝐶𝑅𝐵 𝐵𝑃 = 𝐴𝑅𝐵 𝐵𝑃
 Then account the translation between origins.

𝐴
 𝑃 = 𝐴𝑅𝐵 𝐵𝑃 + 𝐴𝑃𝐵𝑂𝑅𝐺

← Outline
 General transform into a single matrix form.
𝐴
𝑃 = 𝐴𝑇𝐵 𝐵𝑃
 Define a 4 × 4 matrix operator and use 4 × 1 position vectors.
𝐴 𝐴
𝐴 𝑅𝐵 𝑃𝐵𝑂𝑅𝐺 𝐵
𝑃 = 𝑃
1 1
0 0 0 1
 So,
𝐴
𝑃 = 𝐴𝑅𝐵 𝐵𝑃 + 𝐴𝑃𝐵𝑂𝑅𝐺
1=1
𝐴
 𝑇𝐵 is called a Homogeneous Transformation Matrix.
 The description of frame {B} relative to (A} is 𝐴𝑇𝐵 .

← Outline
 Example:
 Frame {B} is rotated relative to frame {A} about 𝑍መ by 30 degrees,
translated 10 units in 𝑋෠𝐴 and 5 units in 𝑌෠𝐴 .
 where 𝐵𝑃 = 3 7 0 𝑇 .
 Find 𝐴𝑃 ?

← Outline
 Special Transformations
 Translation:
𝐼3×3 𝐴
𝑃𝐵𝑂𝑅𝐺
𝐴
𝑇𝐵 = 𝑇𝑇𝑟𝑎𝑛𝑠 =
0 0 0 1
 Rotation:
0
𝐴
𝐴 𝑅𝐵 0
𝑇𝐵 = 𝑇𝑅𝑜𝑡 =
0
0 0 0 1

← Outline
Operators
 Mapping concept can be used as operators.
Translation
Rotation
Transformation
 Translation
𝐴
 Moving a point 𝑃1 in space a finite distance along a given vector
direction 𝐴𝑄.
𝐴
𝑃2 = 𝐴𝑃1 + 𝐴𝑄
 Frame is invariant.
 Translational Operator:
𝐴 𝐴
𝑃2 = 𝑇𝑇𝑟𝑎𝑛𝑠 𝑄 𝑃1
 Q: 𝑇𝑇𝑟𝑎𝑛𝑠 𝑄 = ?

← Outline
Operators
 Translation
 It is accomplished with the same mathematics as mapping the point to a
second frame.
 When a vector is moved "forward" relative to a frame = the frame is

moved "backward“.
1 0 0 𝑞𝑥
𝑇𝑇𝑟𝑎𝑛𝑠 𝑄 = 0 1 0 𝑞𝑦
0 0 1 𝑞𝑧
0 0 0 1
 where 𝑞𝑥 , 𝑞𝑦 and 𝑞𝑧 are the components of the translation vector 𝑄 .
 Q: Sign of 𝑞𝑖 ?

← Outline
Operators
 Rotation
 Rotating a vector 𝐴𝑃1 to a new vector 𝐴𝑃2 , by means of a rotation, 𝑅.
𝐴
𝑃2 = 𝑅 𝐴𝑃1
 The rotation matrix that rotates vectors through some rotation, R = the
rotation matrix that describes a frame rotated by 𝑅 relative to the
reference frame.
𝐴 𝐴
𝑃2 = 𝑅𝐾 𝜃 𝑃1
 "𝑅𝐾 𝜃 " performs a rotation about the axis direction 𝐾 by 𝜃 degrees.
መ what is 𝑅𝐾 𝜃 ?
 Q: Assume 𝐾 = 𝑍,

← Outline
Operators
 Rotation
 Rotational Operator:
𝐴 𝐴
𝑃2 = 𝑇𝑅𝑜𝑡 𝜃 𝑃1
0
𝑅𝐾 𝜃 0
 𝑇𝑅𝑜𝑡 𝑅 =
0
0 0 0 1
𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃 0 0
 𝑇𝑅𝑜𝑡 𝑅 = 𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃 0 0
0 0 1 0
0 0 0 1

← Outline
Operators
 Transformation
 Rotating and translating a vector 𝐴𝑃1 to compute a new vector 𝐴𝑃2 :
𝐴
𝑃2 = 𝑇 𝐴𝑃1
 The transform that rotates by 𝑅 and translates by 𝑄

= the transform that describes a frame rotated by 𝑅 and translated by 𝑄
relative to the reference frame.
𝑞𝑥
𝑅𝐾 𝜃 𝑞𝑦
𝑇= 𝑞𝑧
0 0 0 1
 Q: Is its sequence important?

← Outline
Transformation Arithmetic
 Compound Transformations
 Frame {C} is known relative to frame {B}, and frame {B} is known
relative to frame {A}
𝐴 𝐶
𝑃 = 𝐴𝑇𝐵 𝐵𝑇𝐶 𝑃
𝐴
𝑇𝐶 = 𝐴𝑇𝐵 𝐵𝑇𝐶
𝐴 𝐴
𝑅𝐶 𝑃𝐶𝑂𝑅𝐺
𝐴
𝑇𝐶 =
0 0 0 1
 We have 𝐴𝑅𝐵 , 𝐵𝑅𝐶 , 𝐴𝑃𝐵𝑂𝑅𝐺 , 𝐵𝑃𝐶𝑂𝑅𝐺 , So
? ?
𝐴
 𝑇𝐶 =
0 0 0 1

← Outline
 Compound Transformations
𝐴
𝑅𝐵 𝐵𝑅𝐶 𝐴
𝑅𝐵 𝐵𝑃𝐶𝑂𝑅𝐺 + 𝐴𝑃𝐵𝑂𝑅𝐺
𝐴
𝑇𝐶 =
0 0 0 1
 Computation:
𝐴 𝐶
𝑃 = 𝐴𝑇𝐵 𝐵𝑇𝐶 𝑃 32 𝑀𝑢𝑙𝑡𝑖𝑝𝑙𝑖𝑐𝑎𝑡𝑖𝑜𝑛 + 24 𝐴𝑑𝑑𝑖𝑡𝑖𝑜𝑛
𝐴 𝐶
𝑃 = 𝐴𝑇𝐵 𝐵𝑇𝐶 𝑃 80 𝑀𝑢𝑙𝑡𝑖𝑝𝑙𝑖𝑐𝑎𝑡𝑖𝑜𝑛 + 60 𝐴𝑑𝑑𝑖𝑡𝑖𝑜𝑛
 However, the second one is better when you want to do the

transformation of many vectors many times.

← Outline
 Inverting a Transform
 Frame {B} with respect to a frame {A}, 𝐴𝑇𝐵 , is known ( 𝐴𝑅𝐵 & 𝐴𝑃𝐵𝑂𝑅𝐺 ).
 Invert this transform to get a description of {A} relative to {B},

𝐵
𝑇𝐴 = ?
𝐴

← Outline
 Inverting a Transform
 Frame {B} with respect to a frame {A}, 𝐴𝑇𝐵 , is known ( 𝐴𝑅𝐵 & 𝐴𝑃𝐵𝑂𝑅𝐺 )
 Invert this transform to get a description of {A} relative to {B},

𝐵
𝑇𝐴 = ?
𝐴
𝐴
𝑃 = 𝐴𝑅𝐵 𝐵𝑃 + 𝐴𝑃𝐵𝑂𝑅𝐺
𝐴
𝑃 − 𝐴𝑃𝐵𝑂𝑅𝐺 = 𝐴𝑅𝐵 𝐵𝑃
𝐴 −1 𝐴
𝑅𝐵 𝑃 − 𝐴𝑃𝐵𝑂𝑅𝐺 = 𝐵
𝑃
𝐴 𝑇𝐴 𝑇𝐴
𝑅𝐵 𝑃 − 𝐴𝑅𝐵 𝑃𝐵𝑂𝑅𝐺 = 𝐵
𝑃
𝐴 𝑇 𝑇𝐴
𝐵
𝑅𝐵 − 𝐴𝑅𝐵 𝑃𝐵𝑂𝑅𝐺
𝑇𝐴 =
0 0 0 1
 Q: Geometrical Description = ?
 Note that
𝐵 −1
𝑇𝐴 = 𝐴𝑇𝐵
← Outline
More on Representation of Orientation
 Remember rotation matrix:
𝑟11 𝑟12 𝑟13
𝐴
𝑅𝐵 = 𝑟21 𝑟22 𝑟23
𝑟31 𝑟32 𝑟33
 The nine elements are not all independent (six dependencies).
 Imagine 𝑅 as three columns, as originally introduced:

𝐴 𝐴 𝐴෠ 𝐴
𝑅𝐵 = 𝑋෠𝐵 𝑌𝐵 𝑍መ𝐵
 These three vectors are the unit axes of some frame written in terms of
the reference frame.
 Each is a unit vector, and all three must be mutually perpendicular (six
constraints).
𝐴
𝑋෠𝐵 = 𝐴𝑌෠𝐵 = 𝐴
𝑍መ𝐵 = 1
𝐴෠
𝑋𝐵 . 𝐴𝑌෠𝐵 = 𝐴𝑌෠𝐵 . 𝐴𝑍መ𝐵 = 𝐴෠
𝑋𝐵 . 𝐴𝑍መ𝐵 = 0
 The representation is conveniently specified with three parameter.

← Outline
 Rotation matrix is also called proper orthonormal matrix, ("proper"
refers to 𝒅𝒆𝒕 𝑨𝑹𝑩 = +𝟏 )
 Cayley's formula for orthonormal matrices:

For any proper orthonormal matrix 𝑅, there exists a skew-symmetric
matrix 𝑺 such that
𝑅 = 𝐼3 − 𝑆 −1 (𝐼3 + 𝑆)
 A skew-symmetric matrix (i.e., 𝑆 = −𝑆 𝑇 ) is specified by three

parameters (𝑠𝑥 , 𝑠𝑦 , 𝑠𝑧 ).
0 −𝑠𝑥 𝑠𝑦
𝑆 = 𝑠𝑥 0 −𝑠𝑧
−𝑠𝑦 𝑠𝑧 0
 Consequently, any 3 × 3 rotation matrix can be specified by just three

parameters.

← Outline
 5 Methods for Representation of Orientation
 Direction Cosines (9 Dependent Parameters)
 Euler angles (3 Parameters)
 Fixed angles (3 Parameters)
 Equivalent angle-axis (4 Dependent Parameters)
 Euler parameters (4 Dependent Parameters)

← Outline
 Z-Y-X Euler Angles
 Start with the frame {A}
Rotate {A} about 𝑍መ𝐴 by an angle 𝛼 to get frame {𝐵′}
Rotate {𝐵′} about 𝑌෠ 𝐵′ by an angle 𝛽 to get frame {𝐵"}
Rotate {𝐵"} about 𝑋෠ 𝐵" by an angle 𝛾 to get frame {𝐵}
 Each rotation is performed about an axis of the moving frames.
 Q: 𝐴𝑅𝐵 =?

← Outline
 Using the intermediate frames {𝐵′} and {𝐵"} in order to give an

expression for 𝐴𝑅𝐵_𝑍𝑌𝑋 𝛼, 𝛽, 𝛾 .
𝐴 𝐵′ 𝐵 ′′
𝑅𝐵_𝑍𝑌𝑋 𝛼, 𝛽, 𝛾 = 𝐴𝑅𝐵′ 𝑅𝐵′′ 𝑅𝐵
𝑐𝛼 −𝑠𝛼 0 𝑐𝛽 0 𝑠𝛽 1 0 0
𝐴
𝑅𝐵𝑍𝑌𝑋 = 𝑅𝑍 𝛼 𝑅𝑌 𝛽 𝑅𝑋 𝛾 = 𝑠𝛼 𝑐𝛼 0 0 1 0 0 𝑐𝛾 −𝑠𝛾
0 0 1 −𝑠𝛽 0 𝑐𝛽 0 𝑠𝛾 𝑐𝛾
 where 𝑐𝛼 = 𝑐𝑜𝑠 𝛼, 𝑠𝛼 = 𝑠𝑖𝑛 𝛼
𝑐𝛼 𝑐𝛽 𝑐𝛼 𝑠𝛽 𝑠𝛾 − 𝑠𝛼 𝑐𝛾 𝑐𝛼 𝑠𝛽 𝑐𝛾 + 𝑠𝛼 𝑠𝛾
𝐴
𝑅𝐵_𝑍𝑌𝑋 𝛼, 𝛽, 𝛾 = 𝑠𝛼 𝑐𝛽 𝑠𝛼 𝑠𝛽 𝑠𝛾 + 𝑐𝛼 𝑐𝛾 𝑠𝛼 𝑠𝛽 𝑐𝛾 − 𝑐𝛼 𝑠𝛾
−𝑠𝛽 𝑐𝛽 𝑠𝛾 𝑐𝛽 𝑐𝛾
← Outline
 The Inverse Problem
 Extracting equivalent X-Y-Z Euler angles from a given rotation matrix.
 If 𝐴𝑅𝐵_𝑍𝑌𝑋 𝛼, 𝛽, 𝛾 is equated to the given rotation matrix:
𝑐𝛼 𝑐𝛽 𝑐𝛼 𝑠𝛽 𝑠𝛾 − 𝑠𝛼 𝑐𝛾 𝑐𝛼 𝑠𝛽 𝑐𝛾 + 𝑠𝛼 𝑠𝛾 𝑟11 𝑟12 𝑟13

𝑠𝛼 𝑐𝛽 𝑠𝛼 𝑠𝛽 𝑠𝛾 + 𝑐𝛼 𝑐𝛾 𝑠𝛼 𝑠𝛽 𝑐𝛾 − 𝑐𝛼 𝑠𝛾 = 𝑟21 𝑟22 𝑟23
−𝑠𝛽 𝑐𝛽 𝑠𝛾 𝑐𝛽 𝑐𝛾 𝑟31 𝑟32 𝑟33
 9 equations & 3 unknowns.

 Due to six dependencies, 3 equations & 3 unknowns.
2 2
𝛽 = 𝐴𝑡𝑎𝑛2 −𝑟31 , 𝑟11 + 𝑟21
 𝐴𝑡𝑎𝑛2(𝑦, 𝑥) is a two-argument arc tangent function.
 Although a second solution exists for 𝛽, we always compute the single

solution for which −90.0° ≤ 𝛽 ≤ 90.0° to have a one-to-one mapping.
← Outline

2 2
𝛽 = 𝐴𝑡𝑎𝑛2 −𝑟31 , 𝑟11 + 𝑟21
 As long as 𝑐𝛽 ≠ 0,
𝛼 = 𝐴𝑡𝑎𝑛2 𝑟21 /𝑐𝛽, 𝑟11 /𝑐𝛽
𝛾 = 𝐴𝑡𝑎𝑛2 𝑟32 /𝑐𝛽, 𝑟33 /𝑐𝛽
 Q: What about for 𝛽 = ±90.0° (𝑐𝛽 = 0) ?!!!

← Outline
 Singularity of the Inverse Problem:
𝛼 = 𝐴𝑡𝑎𝑛2 𝑟21 /𝑐𝛽, 𝑟11 /𝑐𝛽
𝛾 = 𝐴𝑡𝑎𝑛2 𝑟32 /𝑐𝛽, 𝑟33 /𝑐𝛽
 If 𝛽 = ±90.0° (𝑐𝛽 = 0), the solution degenerates.
 For 𝛽 = +90.0° :
0 𝑠𝑖𝑛 𝛾 − 𝛼 𝑐𝑜𝑠 𝛾 − 𝛼 𝑟11 𝑟12 𝑟13
0 𝑐𝑜𝑠 𝛾 − 𝛼 − 𝑠𝑖𝑛 𝛾 − 𝛼 = 𝑟21 𝑟22 𝑟23
−1 0 0 𝑟31 𝑟32 𝑟33
 In those cases, only the 𝛼 ± 𝛾 can be computed.

← Outline
 For 𝛽 = +90.0° :
0 𝑠𝑖𝑛 𝛾 − 𝛼 𝑐𝑜𝑠 𝛾 − 𝛼 𝑟11 𝑟12 𝑟13
0 𝑐𝑜𝑠 𝛾 − 𝛼 − 𝑠𝑖𝑛 𝛾 − 𝛼 = 𝑟21 𝑟22 𝑟23 .
−1 0 0 𝑟31 𝑟32 𝑟33
 One possible convention is to choose 𝛼 = 0.0 in these cases and compute

𝛾.
 If 𝛽 = ±90.0°, then a solution can be calculated to be
𝛽 = +90.0° 𝛽 = −90.0°
𝛼 = 0.0 𝛼 = 0.0
𝛾 = 𝐴𝑡𝑎𝑛2 (𝑟12 , 𝑟22 ) 𝛾 = −𝐴𝑡𝑎𝑛2 (𝑟12 , 𝑟22 )

← Outline
 Q: What is the physical interpretation of the IK singularity?

← Outline
 X-Y-Z Fixed Angles
 Rotate {A} about 𝑋෠𝐴 by an angle 𝛾 to get frame {𝐵′}
 Rotate {𝐵′} about 𝑌෠𝐴 by an angle 𝛽 to get frame {𝐵"}
 Rotate {𝐵"} about 𝑍መ𝐴 by an angle 𝛼 to get frame {𝐵}
 Each of the three rotations takes place about an axis in the fixed
reference frame {A}.
 This convention is referred to as roll, pitch, yaw angles.
 Q: 𝐴𝑅𝐵 =?
← Outline
 The composition rule cannot be applied here, similarity transformation
can be used instead.
 Similarity Transformation
 A rotation matrix (as a coordinate transformation) may be viewed as
changing basis from one frame to another.
 A general linear transformation is transformed from one frame to another

using similarity transformation.
 𝑀 is a linear transformation in frame {0} and 𝑁 is the representation of

𝑀 in frame {1}.
0 −1
𝑁= 𝑅1 𝑀 0𝑅1

← Outline
 Using the intermediate frames {𝐵′} and {𝐵"} in order to give an
expression for 𝐴𝑅𝐵_𝑋𝑌𝑍 𝛾, 𝛽, 𝛼 .
𝐴 𝐴 𝐵′ 𝐵 ′′
𝑅𝐵 = 𝑅𝐵 ′ 𝑅𝐵′′ 𝑅𝐵
𝐴
𝑅𝐵′ = 𝑅𝑋 𝛾
𝐵′ 𝐴 −1 𝐴
𝑅𝐵′′ = 𝑅𝐵 ′ 𝑅𝑌 𝛽 𝑅𝐵 ′
𝐵 ′′ 𝐴 −1 𝐴
𝑅𝐵 = 𝑅𝐵′′ 𝑅𝑍 𝛼 𝑅𝐵′′
 Therefore,
𝐴
𝑅𝐵′ = 𝑅𝑋 𝛾
𝐴 𝐵′ −1
𝑅𝐵′′ = 𝐴𝑅𝐵′ 𝑅𝐵′′ = 𝐴𝑅𝐵′ 𝐴
𝑅𝐵 ′ 𝑅𝑌 𝛽 𝐴
𝑅𝐵′ = 𝑅𝑌 𝛽 𝑅𝑋 𝛾
𝐴 𝐵 ′′ −1
𝑅𝐵 = 𝐴𝑅𝐵′′ 𝑅𝐵 = 𝐴𝑅𝐵′′ 𝐴
𝑅𝐵′′ 𝑅𝑍 𝛼 𝐴
𝑅𝐵′′ = 𝑅𝑍 𝛼 𝑅𝑌 𝛽 𝑅𝑋 𝛾

← Outline
𝐴
𝑅𝐵_𝑋𝑌𝑍 𝛾, 𝛽, 𝛼 = 𝑅𝑍 𝛼 𝑅𝑌 𝛽 𝑅𝑋 𝛾
𝑐𝛼 −𝑠𝛼 0 𝑐𝛽 0 𝑠𝛽 1 0 0
= 𝑠𝛼 𝑐𝛼 0 0 1 0 0 𝑐𝛾 −𝑠𝛾
0 0 1 −𝑠𝛽 0 𝑐𝛽 0 𝑠𝛾 𝑐𝛾
 Therefore,
𝑐𝛼 𝑐𝛽 𝑐𝛼 𝑠𝛽 𝑠𝛾 − 𝑠𝛼 𝑐𝛾 𝑐𝛼 𝑠𝛽 𝑐𝛾 + 𝑠𝛼 𝑠𝛾
𝐴
𝑅𝐵_𝑋𝑌𝑍 𝛾, 𝛽, 𝛼 = 𝑠𝛼 𝑐𝛽 𝑠𝛼 𝑠𝛽 𝑠𝛾 + 𝑐𝛼 𝑐𝛾 𝑠𝛼 𝑠𝛽 𝑐𝛾 − 𝑐𝛼 𝑠𝛾
−𝑠𝛽 𝑐𝛽 𝑠𝛾 𝑐𝛽 𝑐𝛾
 Note: Three rotations taken about fixed axes 𝑒. 𝑔. 𝐴𝑅𝐵_𝑋𝑌𝑍 𝛾, 𝛽, 𝛼

yield the same final orientation as the same three rotations taken in
opposite order about the axes of the moving frame
𝑒. 𝑔. 𝐴𝑅𝐵_𝑍𝑌𝑋 𝛼, 𝛽, 𝛾 .
 24 representations: 12 Euler angles series + 12 Fixed angles series.

← Outline
 Equivalent Angle-Axis
 Rotate about the unit vector 𝐴𝐾 ෡ by an angle 𝜃 according to the right-
hand rule (Based on the Euler Theorem).
𝐴෡
 𝐾 : Equivalent axis of a finite rotation.
 It may be written as 𝐴𝑅𝐵 𝐾,

෡ 𝜃 or 𝑅𝐾 (𝜃).
𝐴෡
 𝐾 requires only two parameters caused
by its unit length.
 For the general axis of rotation:

𝑘𝑥 𝑘𝑥 𝑣𝜃 + 𝑐𝜃 𝑘𝑥 𝑘𝑦 𝑣𝜃 − 𝑘𝑧 𝑠𝜃 𝑘𝑥 𝑘𝑧 𝑣𝜃 + 𝑘𝑦 𝑠𝜃
𝑅𝐾 𝜃 = 𝑘𝑥 𝑘𝑦 𝑣𝜃 + 𝑘𝑧 𝑠𝜃 𝑘𝑦 𝑘𝑦 𝑣𝜃 + 𝑐𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 − 𝑘𝑥 𝑠𝜃
𝑘𝑥 𝑘𝑧 𝑣𝜃 − 𝑘𝑦 𝑠𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 + 𝑘𝑥 𝑠𝜃 𝑘𝑧 𝑘𝑧 𝑣𝜃 + 𝑐𝜃
𝑇
 where 𝑐𝜃 = 𝑐𝑜𝑠 𝜃, 𝑠𝜃 = 𝑠𝑖𝑛 𝜃, 𝑣𝜃 = 1 − 𝑐𝑜𝑠 𝜃 and 𝐴𝐾
෡ = 𝑘𝑥 , 𝑘𝑦 , 𝑘𝑧
← Outline
𝑘𝑥 𝑘𝑥 𝑣𝜃 + 𝑐𝜃 𝑘𝑥 𝑘𝑦 𝑣𝜃 − 𝑘𝑧 𝑠𝜃 𝑘𝑥 𝑘𝑧 𝑣𝜃 + 𝑘𝑦 𝑠𝜃
𝑅𝐾 𝜃 = 𝑘𝑥 𝑘𝑦 𝑣𝜃 + 𝑘𝑧 𝑠𝜃 𝑘𝑦 𝑘𝑦 𝑣𝜃 + 𝑐𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 − 𝑘𝑥 𝑠𝜃
𝑘𝑥 𝑘𝑧 𝑣𝜃 − 𝑘𝑦 𝑠𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 + 𝑘𝑥 𝑠𝜃 𝑘𝑧 𝑘𝑧 𝑣𝜃 + 𝑐𝜃
 Example:
 ෡ as principal axes (e.g. 𝐾෠ = 𝑋෠ = 1 0 0
For 𝐾 𝑇
or 𝑌෠ or 𝑍መ )
1 0 0
𝑅𝑋 𝜃 = 0 𝑐𝜃 − 𝑠𝜃
0 𝑠𝜃 𝑐𝜃
𝑐𝜃 0 𝑠𝜃
𝑅𝑌 𝜃 = 0 1 0
− 𝑠𝜃 0 𝑐𝜃
𝑐𝜃 −𝑠𝜃 0
𝑅𝑍 𝜃 = 𝑠𝜃 𝑐𝜃 0
0 0 1
← Outline
 Computing 𝐾෡ and 𝜃 from a given rotation matrix.
𝑘𝑥 𝑘𝑥 𝑣𝜃 + 𝑐𝜃 𝑘𝑥 𝑘𝑦 𝑣𝜃 − 𝑘𝑧 𝑠𝜃 𝑘𝑥 𝑘𝑧 𝑣𝜃 + 𝑘𝑦 𝑠𝜃 𝑟11 𝑟12 𝑟13

𝑘𝑥 𝑘𝑦 𝑣𝜃 + 𝑘𝑧 𝑠𝜃 𝑘𝑦 𝑘𝑦 𝑣𝜃 + 𝑐𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 − 𝑘𝑥 𝑠𝜃 = 𝑟21 𝑟22 𝑟23
𝑘𝑥 𝑘𝑧 𝑣𝜃 − 𝑘𝑦 𝑠𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 + 𝑘𝑥 𝑠𝜃 𝑘𝑧 𝑘𝑧 𝑣𝜃 + 𝑐𝜃 𝑟31 𝑟32 𝑟33
𝑟32 − 𝑟23
𝑟11 +𝑟22 +𝑟33 −1 1
𝜃 = 𝐴𝑐𝑜𝑠 ෡=
, 𝐾 𝑟13 − 𝑟31
2 2 sin𝜃
𝑟21 − 𝑟12
 This solution always computes a value of 𝜃 between [0-180°].
𝐴෡
 For any 𝐾, 𝜃 , − 𝐴𝐾,
෡ −𝜃 results in the same orientation in space.
 For small angular rotations, the axis becomes ill-defined.
 If 𝜃 → 0°, the axis becomes completely undefined. (𝜃 = 0° or 𝜃 = 180°)

← Outline
 Euler Parameters
 Another representation is by means of four numbers called the Euler
parameters.
𝑇
 In terms of the equivalent axis 𝐾 ෡ = 𝑘𝑥 , 𝑘𝑦 , 𝑘𝑧 and the equivalent
angle 𝜃, the Euler parameters are given by
𝜖1 𝑘𝑥
𝜃 𝜃
𝜖 𝑘 ෡
𝜖 = 2 = 𝑦 𝑠𝑖𝑛 = 𝐾 𝑠𝑖𝑛
𝜖3 2 2
𝑘𝑧
𝜃
𝜖4 = 𝑐𝑜𝑠
2
 These four quantities are not independent (Unit Quaternion)

𝜖12 + 𝜖22 + 𝜖32 + 𝜖42 =1
 An orientation might be visualized as a point on a unit hypersphere in

four-dimensional space.
← Outline
 The rotation matrix
1 − 2𝜖22 − 2𝜖32 2 𝜖1 𝜖2 − 𝜖3 𝜖4 2 𝜖1 𝜖3 + 𝜖2 𝜖4
𝑅𝜖 = 2 𝜖1 𝜖2 + 𝜖3 𝜖4 1 − 2𝜖12 − 2𝜖32 2 𝜖2 𝜖3 − 𝜖1 𝜖4
2 𝜖1 𝜖3 − 𝜖2 𝜖4 2 𝜖2 𝜖3 + 𝜖1 𝜖4 1 − 2𝜖22 − 2𝜖22
 The Inverse Problem:

 Given a rotation matrix (𝐴), the equivalent Euler parameters are
1 − 2𝜖22 − 2𝜖32 2 𝜖1 𝜖2 − 𝜖3 𝜖4 2 𝜖1 𝜖3 + 𝜖2 𝜖4 𝑟11 𝑟12 𝑟13
2 𝜖1 𝜖2 + 𝜖3 𝜖4 1 − 2𝜖12 − 2𝜖32 2 𝜖2 𝜖3 − 𝜖1 𝜖4 = 𝑟21 𝑟22 𝑟23 = 𝐴
2 𝜖1 𝜖3 − 𝜖2 𝜖4 2 𝜖2 𝜖3 + 𝜖1 𝜖4 1 − 2𝜖12 − 2𝜖22 𝑟31 𝑟32 𝑟33
1
𝜖4 = 1 + 𝑟11 + 𝑟22 + 𝑟33
2
𝑟 − 𝑟23
1 32
𝜖= 𝑟13 − 𝑟31
4𝜖4 𝑟 − 𝑟
21 12
 For a rotation of 180 degrees about some axis, 𝜖4 → 0 (ill defined).
← Outline
 The Inverse Problem:
 Remember:
𝜃
෡ 𝑠𝑖𝑛
𝜖=𝐾
2
𝜃
𝜖4 = 𝑐𝑜𝑠
2
 By definition, if 𝜖4 = 0, then 𝜃 = 180° and 𝜖 is equal to rotation axis,
෡
i.e. 𝜖 = 𝐾
1 + 2𝑟11 − 𝑡𝑟 𝐴
𝜖12 =
4
1 + 2𝑟22 − 𝑡𝑟 𝐴
𝜖22 =
4
2
1 + 2𝑟33 − 𝑡𝑟 𝐴
𝜖3 =
4
 Note: There is No Singularity associated with these parameters.

 As long as the direction cosines are known, we can find the
corresponding Euler parameters [1].
← Outline
Transformation of Free Vectors
 Line vector: a vector that is dependent on its line of action, along with
direction and magnitude. (Position & Force)
 Free vector: a vector that may be positioned anywhere in space,

provided that magnitude and direction are preserved (Velocity &
Moments)
 For free vectors, only the rotation matrix relating the two systems is used
in transforming.
 Position Transformation
𝐴
 Velocity Transformation
𝐴
𝑉 = 𝐴𝑅𝐵 𝐵𝑉
𝐴
 𝑃𝐵𝑂𝑅𝐺 which would appear in
a position-vector transformation,
does not appear in a velocity transform.
← Outline
Acknowledgment
References:
1-Advanced Robotics , Dr. H. Ghafarirad, Department Of Mech. Eng. Amirkabir University of
Technology
.‫تقریباً تمامی اسالیدها متعلق به جناب آقای دکتر غفاری است از لطف ایشان سپاسگزارم‬
2-Advanced Robotics , Dr. A. M. Shafei, Department Of Mech. Eng. Shahid Bahonar University of
Kerman

Robotics Lec3

Uploaded by

Copyright:

Available Formats

Robotics Lec3

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotics Lec3

Uploaded by

Copyright:

Available Formats

‫رباتیک‬

‫ مقدمه‬: ‫فصل اول‬

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 1

 Using IK, the first & desired position

 What is the time-based motion

 Spline is sometimes used to refer to a smooth function that passes

 Cartesian Trajectory Generation:

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 3

 The problem of dynamics is central to the design of such algorithms, but

 The manipulator dynamics are highly nonlinear.

 The nonlinear control techniques promise better performance than do

Spatial Descriptions and Transformations

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 5

 Descriptions: Positions, Orientations and Frames

 Transformation of Free Vectors

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 6

 There is a universe coordinate system to which everything can be

 A description is used to specify attributes of various objects.

 Objects are parts, tools, and the manipulator itself.

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 7

 Vectors must be tagged identifying which coordinate system they are

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 8

 It is invariant of the frame.

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 9

 One way to describe the coordinate system {B}:

 They are called 𝐴𝑋෠𝐵 , 𝐴𝑌෠𝐵 , 𝐴𝑍መ𝐵 .

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 10

 It is called Rotation Matrix (R).

 The description of frame {A} relative to {B} ( 𝐵𝑅𝐴 ) is the transpose of

5) It can be expressed by Only 3 independent numbers.

 For convenience, the point whose position described is chosen as the

 The description of a frame: a position vector and a rotation matrix.

 Frame {B} is described by 𝐴𝑅𝐵 and 𝐴𝑃𝐵𝑂𝑅𝐺 .

 A frame can be used as a description

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 15

 Position and orientation can be represented as frames:

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 16

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 17

 The components of 𝐴𝑃 may be calculated by the projection as:

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 18

 Expressing in the matrix form:

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 19

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 20

 Assume an intermediate frame {C}:

 Describe 𝐵𝑃 in the intermediate frame.

 Then account the translation between origins.

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 21

 Define a 4 × 4 matrix operator and use 4 × 1 position vectors.

 The description of frame {B} relative to (A} is 𝐴𝑇𝐵 .

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 22

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 23

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 24

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 25

 When a vector is moved "forward" relative to a frame = the frame is

 where 𝑞𝑥 , 𝑞𝑦 and 𝑞𝑧 are the components of the translation vector 𝑄 .

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 26

 "𝑅𝐾 𝜃 " performs a rotation about the axis direction 𝐾 by 𝜃 degrees.

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 27

AMIRKABIR UNIVERSITY OF TECHNOLOGY, M. ZAREINEJAD 28