Problems Unit 2
Problems Unit 2
S pring 2 0 1 5
Departmen t of Mechanical Engineering
Uni v er sit y of Wisconsin , Madison
By
Nasser M. Abbasi
Student Version of MATLAB
Nov ember 2 8 , 2 0 1 9
Contents
0.1 Problem 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
0.1.1 problem description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
0.1.2 solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
0.2 Problem 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
0.3 Problem 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
0.4 Problem 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
0.5 Problem 5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
0.5.1 solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
0.6 Problem 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
0.6.1 problem description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
0.6.2 solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
List of Figures
1 problem 2 description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2 problem 3 description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
3 problem 4 description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
4 problem 5 description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
List of Tables
2
ME / ECE 739: Advanced Robotics Homework #1
Due: February 19th (Thursday)
Please submit your answers to the questions and all supporting work including your Matlab scripts, and, 3
where appropriate, program results (plots, explanations). Your Matlab scripts should be readable, with
comments, sensible variable names, indentation of code-block, etc. In addition to the hardcopy (pdf
format), you must also submit your Matlab scripts electronically to the Learn@UW course page dropbox
0.1 Problem 1
(e.g. Homework #1) using a zip archive file format. Please name your zip files using your last name and
hw# (e.g. zinn_hw1.zip)
0.1.1 problem description
(1) [Spong 2-15] Suppose that three coordinate frames o1x1y1z1, o2x2y2z2, and o3x3y3z3 are given, and
suppose
1 0 0 0 0 1
R21 0 1
2
3
2
, R1 0 1 0
3
0
1
1 0 0
3
2 2
0.1.2 solution
(2)the
Starting with [Spong 2-38] Consider the adjacent diagram.
relation Find the
homogeneous transformations T10 , T20 , T21 representing the
𝑅13 = 𝑅12 𝑅23
transformations among the three frames shown.
Pre-multiplying►Show
both that
sides T10(𝑅
T20 by T211 )−1 which exists since 𝑅 is a rotation matrix and hence invertible
2
results in
𝑅23 = (𝑅12 )−1 𝑅13
For a rotation matrix the following relation holds
(3) [Spong 2-39] Consider the diagram below. (𝑅12 )−1 A (𝑅12 )𝑇is set up 1 meter
= robot
from a table. The table top is 1 meter high and 1 meter square. A frame
Therefore o1x1y1z1 is fixed to the edge of the table as shown. A cube measuring 20
cm on a side is placed in the2 center1of𝑇 the table with the frame o2x2y2z2
established at the center of𝑅3the
= (𝑅 cube 𝑅13shown. A camera is situated
2 ) as
directly above the center of the ⎛block 2 meters ⎞above 𝑇 ⎛ the table ⎞top
⎜⎜1 0Find the 0 ⎟⎟homogeneous
⎟⎟ ⎜⎜⎜⎜base
0 0 −1⎟⎟
with frame o3x3y3z3 attached as shown.
transformations relating each of ⎜⎜⎜these frames ⎟
⎟⎟ ⎜⎜0 1 0 ⎟⎟⎟⎟
1 − to3 the
⎟ ⎜
frame o0x0y0z0. = ⎜⎜⎜0 √
⎟
⎜⎜ 2 2 ⎟ ⎜ ⎜ ⎟
► Find the homogeneous transformation ⎝ 1 ⎟
√3 relating ⎠ ⎝1 0 0 ⎟⎠
0
the frame o2x2y2z2 to the camera frame 2 o3x3y23z3.
⎛ ⎞⎛ ⎞
⎜⎜1 0 0 ⎟⎟ ⎜⎜0 0 −1⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟
= ⎜⎜⎜0
⎜ 1 √3 ⎟⎟⎟ ⎜⎜⎜0 1 0 ⎟⎟⎟⎟
⎜⎜ 2 2 ⎟ ⎟ ⎜⎜⎝ ⎟
⎝ −√3 1 ⎟ ⎠ 1 0 0 ⎟⎠
0 2 2
⎛ ⎞
⎜⎜ 0 0 −1⎟⎟
⎜⎜ ⎟⎟
⎜ 3 1Page 1 of ⎟⎟ 4
= ⎜⎜⎜ √ 0 ⎟⎟
⎜⎜ 2 2 ⎟⎟
⎝ 1 −√3 ⎠
2 2
0
(1) [Spong 2-15] Suppose that three coordinate frames o1x1y1z1, o2x2y2z2, and o3x3y3z3 are given, and
suppose 4
1 0 0 0 0 1
R2 0 1 2
1 3
1 1 0
2 , R3 0
0.2 Problem 2
0 3 2 1
2
1 0 0
►Find the matrix R 23
The goal is to determine 𝑇10 , 𝑇20 , 𝑇30 and 𝑇32 . 𝑇𝑖𝑖−1 is the homogeneous transformation from frame {𝑖 − 1}
to(3) [Spong
frame 2-39]by
{𝑖} given Consider the diagram below. A robot is set up 1 meter
from a table. The table top is 1 meter high and⎛ 1 meter⎞square. A frame
o1x1y1z1 is fixed to the edge of the table as𝑖−1shown. ⎜⎜𝑅𝑖−1A cube
𝑑⎟⎟⎟ measuring 20
𝑇𝑖 = ⎜⎜⎝ 𝑖 ⎟⎠
cm on a side is placed in the center of the table 0with 1the frame o2x2y2z2
established at the center of the cube as shown. A camera is situated
𝑑 is theabove
Wheredirectly the center
position vector of the the
from block 2 meters
origin above
of frame {𝑖 −the 1} table origin of frame {𝑖} expressed in
to thetop
𝑖−1
{𝑖 − 1}
framewith , ando𝑅
frame 3x𝑖3y3zis3 attached as shown.
the rotation matrix.Find the homogeneous
transformations relating each of these frames to the base
By direct inspection of the above diagram the following transformations are obtained
frame o0x0y0z0. ⎛ ⎞ ⎛ ⎞ ⎛ ⎞
► Find the homogeneous ⎜⎜ 0 1 0transformation
0⎟⎟ ⎜⎜ 0relating
0 −1 0⎟⎟ ⎜⎜0 −1 0 1 ⎟⎟
⎜
the frame o2x2y⎜⎜⎜2z2 to the camera ⎟ ⎜
⎟⎟ frame⎜⎜ o3x3y3z3. ⎟
⎟⎟ ⎜
⎜⎜ ⎟⎟
⎟ ⎜ ⎟ ⎜ ⎟⎟
⎜ 0 0 −1 0⎟⎟ 0 ⎜⎜−1 0 0 1⎟⎟ 1 ⎜⎜0 0 −1 0 ⎟⎟
𝑇10 = ⎜⎜⎜ ⎟,𝑇 = ⎜ ⎟ , 𝑇2 = ⎜⎜ ⎟
⎜⎜−1 0 0 1⎟⎟⎟ 2 ⎜⎜⎜ 0 1 0 0⎟⎟⎟⎟ ⎜⎜1 0 0 −1⎟⎟⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎠ ⎜⎜ ⎟⎠
⎝ ⎠ ⎝ ⎝
0 0 0 1 0 0 0 1 0 0 0 1
Given the above transformations 𝑇10 𝑇21 is found and checked to be the same as 𝑇20
⎛ ⎞⎛ ⎞
⎜⎜ 0 1 0 0⎟⎟ ⎜⎜0 −1 0 1 ⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ 0 0 −1 10⎟⎟⎟⎟of
Page ⎜⎜04 0 −1 0 ⎟⎟⎟⎟
⎜ ⎜
𝑇1 𝑇2 = ⎜⎜⎜
0 1
⎟ ⎜⎜⎜
⎟ ⎟
⎜⎜−1
⎜⎜ 0 0 1⎟⎟ ⎜⎜⎜1
⎟ 0 0 −1⎟⎟⎟⎟
⎝ ⎟⎠ ⎜⎝ ⎟⎠
0 0 0 1 0 0 0 1
⎛ ⎞
⎜⎜ 0 0 −1 0⎟⎟
⎜⎜ ⎟⎟
⎜⎜−1 0 0 1⎟⎟⎟⎟
⎜
= ⎜⎜⎜ ⎟
⎜⎜ 0
⎜⎜ 1 0 0⎟⎟⎟⎟
⎝ ⎟⎠
0 0 0 1
(3) [Spong 2-39] Consider the diagram below. A robot is set up 1 meter
from a table. The table top is 1 meter high and 1 meter square. A frame
o1x1y1z1 is fixed to the edge of the table as shown. A cube measuring 20
cm on a side is placed in the center of the table with the frame o2x2y2z2
established at the center of the cube as shown. A camera is situated
directly above the center of the block 2 meters above the table top
with frame o3x3y3z3 attached as shown. Find the homogeneous
transformations relating each of these frames to the base
frame o0x0y0z0.
► Find the homogeneous transformation relating
the frame o2x2y2z2 to the camera frame o3x3y3z3.
The distance from the table surface to the center of the small cube is 0.1 meter. The goal is to
determine 𝑇10 , 𝑇20 , 𝑇30 and 𝑇32 . By direct inspection of the given figure the following transformations
are obtained
⎛ ⎞ ⎛ ⎞ ⎛ ⎞
⎜⎜1 0 0 0⎟⎟ ⎜⎜1 0 0 −0.5⎟⎟ ⎜⎜0 1 0 −0.5⎟⎟
⎜⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜0 1 0 1⎟⎟⎟ 0 ⎜⎜⎜0 1 0 1.5 ⎟⎟⎟ 0 ⎜⎜⎜1 0 0 1.5 ⎟⎟
⎟⎟
𝑇1 = ⎜⎜⎜
0 ⎟⎟ , 𝑇 = ⎜⎜ ⎟⎟ , 𝑇 = ⎜⎜ ⎟⎟
⎜⎜0 0 1 1⎟⎟⎟ 2 ⎜⎜⎜0 0 1 1.1 ⎟⎟⎟ 3 ⎜⎜⎜0 0 −1 3 ⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎝ ⎠ ⎝ ⎠ ⎝ ⎠
0 0 0 1 0 0 0 1 0 0 0 1
In each of the above, the first column of 𝑇𝑖𝑖−1 is the projection of 𝑥̂ in frame 𝑖 into frame 𝑖 − 1 and
the second is the projection of 𝑦̂ in frame 𝑖 into frame 𝑖 − 1 and the third column the projection of
𝑧̂ in frame 𝑖 into frame 𝑖 − 1. The fourth column of 𝑇 is the position vector of the center of frame 𝑖
expressed in frame 𝑖 − 1. By inspection 𝑇32 is found to be
⎛ ⎞
⎜⎜0 1 0 0 ⎟⎟
⎜⎜ ⎟⎟
⎜⎜1 0 0 0 ⎟⎟
⎜ ⎟⎟
𝑇32 = ⎜⎜⎜ ⎟
⎜⎜0 0 −1 1.9⎟⎟⎟
⎜⎜ ⎟⎟
⎝ ⎠
0 0 0 1
6
(4) Coordinate frames {A} and {B} are fixed with respect to ground and are related by the homogeneous
transformation matrix
12 0 3
2
2
0 1 0 1
TBA
32 0 12 0
0 0 0 1
The velocity of a point expressed in frame {A} is given as
v A 2 4 2
T
PART 1:
Let 𝑑 be the(5)position
For the two manipulators
of the shown
originbelow derive the 𝐴 be
vector of frame {𝐵}forward kinematics
relative to frameequations using 𝑟the
{𝐴}. Let DHthe position
convention. 𝐵
vector of a point relative to frame {𝐴}, and 𝑟 be the position vector of the point relative to frame {𝐵}
as shown in the following diagram
► Assign frames per the DH convention and build the DH table for each manipulator
► Derive the homogeneous transforms that relate successive frames (i.e. frame {i} to {i-1}) as a
𝐴
function of𝑟the is
From the above diagram found
joint as and manipulator geometric parameters.
variables
𝐴 𝐴 𝐵
► Derive the homogeneous transform 𝑟that= 𝑅𝐵 𝑟the+position
relates 𝑑 and orientation of the end-effector to (1)
the base frame as a function of the joint variables and manipulator geometric parameters. Use the
⎛ operation point
base frame and end-effector ⎞ defined in the
⎛ figures.
⎞
⎜⎜ −1 0
−√3 ⎟
⎟ ⎜⎜−2⎟⎟
⎜⎜ 2 2 ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ ⎟⎟
From the problem statement, 𝑅𝐴
𝐵 =⎜ ⎜⎜ 0 1 0 ⎟⎟ and 𝑑 = ⎜⎜⎜⎜ 4 ⎟⎟⎟⎟
⎜⎝ √3 ⎟ ⎜⎝ ⎟⎠
−1 ⎟⎠
0 2
2 2
Page 2 of 4
Taking time derivative of (1) and using the chain rule results in
𝑑𝑅𝐴𝐵 𝐵 𝑑
𝑣𝐴 = 𝑟 + 𝑅𝐴 𝐵
𝐵𝑣 + 𝑑 (2)
𝑑𝑡 𝑑𝑡
𝑑𝑅𝐴
𝑅𝐴
𝐵 does not depend on time, therefore 𝑑𝑡
𝐵
= 0. Since frame {𝐵} does not move relative to frame {𝐴},
𝑑
therefore 𝑑𝑡
𝑑 = 0. Using these results (2) simplifies to
7
𝑣𝐴 = 𝑅 𝐴
𝐵𝑣
𝐵
−1 𝑇
Solving for 𝑣𝐵 from the above, and noting that �𝑅𝐴
𝐵� = �𝑅𝐴
𝐵 � since it is a rotation matrix gives
𝑇
𝑣𝐵 = �𝑅𝐴
𝐵� 𝑣
𝐴
⎛ −1 ⎞𝑇
−√3 ⎟ ⎛ ⎞
⎜⎜ 0 ⎟⎟ ⎜⎜−2⎟⎟
⎜⎜ 2 2 ⎟⎟ ⎜⎜ ⎟⎟
⎜ ⎜⎜ 4 ⎟⎟
𝑣 = ⎜⎜⎜ 0
𝐵
1 0 ⎟⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ ⎟ ⎜⎝ ⎟⎠
⎝ √3 −1 ⎟ ⎠
0 2
2 2
⎛ 1 1 ⎞⎛ ⎞
⎜⎜ − 0 √3⎟⎟⎟ ⎜⎜⎜−2⎟⎟⎟
⎜⎜ 2 2 ⎟⎜ ⎟
⎜
= ⎜⎜ 0 1 0 ⎟⎟⎟⎟ ⎜⎜⎜⎜ 4 ⎟⎟⎟⎟
⎜⎜
⎝ 1 1 ⎟ ⎠ ⎜⎝ ⎟⎠
− 2 √3 0 −2 2
Therefore
⎛ ⎞
⎜⎜√3 + 1⎟⎟
⎜⎜ ⎟⎟
𝑣𝐵 = ⎜⎜⎜ 4 ⎟⎟⎟
⎜⎜ ⎟⎟
⎝ ⎠
√3 − 1
PART 2:
The norm of the velocity vectors are
⎛ ⎞
⎜−2⎟
�⎜⎜⎜ ⎟⎟⎟�
�𝑣𝐴 � = ⎜⎜⎜ 4 ⎟⎟⎟ = 2√6
�⎜⎜⎝ ⎟⎟⎠�
2
⎛ ⎞
⎜⎜√3 + 1⎟⎟
⎜⎜ ⎟⎟
�𝑣𝐵 � = ⎜⎜⎜ 4 ⎟⎟⎟ = 2√6
⎜⎜ ⎟⎟
⎝ ⎠
√3 − 1
They have the same magnitude. The reason is that frame {𝐵} itself does not move nor rotate relative to
{𝐴}. Therefore frame 𝐵 is fixed relative from frame {𝐴}. Hence the velocity of a point relative to frame
{𝐴} will have the same magnitude relative to {𝐵}. The velocity vector has different representation
depending on the frame of reference, but has the same magnitude.
v A 2 4 2
T
0.5 Problem 5
(5) For the two manipulators shown below derive the forward kinematics equations using the DH
convention.
► Assign frames per the DH convention and build the DH table for each manipulator
► Derive the homogeneous transforms that relate successive frames (i.e. frame {i} to {i-1}) as a
function of the joint variables and manipulator geometric parameters.
► Derive the homogeneous transform that relates the position and orientation of the end-effector to
the base frame as a function of the joint variables and manipulator geometric parameters. Use the
base frame and end-effector operation point defined in the figures.
0.5.1 solution
PART (A)
The first step is to assign the 𝑧𝑖 axes for each link as follows
9
joint 2
link 1
joint 1
link 0
The following diagram shows the parameters for the first link
1
Textbook, page 80.
2
Text book, page 78.
10
The following diagram shows the parameters for the second link
The following diagram shows the parameters for the third link
Now the forward transformations using equation (3.10) on page 77 of the textbook is found
⎛ ⎞
⎜⎜𝐶𝜃 −𝑆𝜃 𝐶𝛼 𝑆𝜃 𝑆𝛼 𝑎𝐶𝜃 ⎟⎟
⎜⎜ ⎟⎟
⎜⎜ 𝑆 𝐶𝜃 𝐶𝛼 −𝐶𝜃 𝑆𝛼 𝑎𝑆𝜃 ⎟⎟⎟⎟
⎜ 𝜃
𝐴 = ⎜⎜⎜ ⎟
⎜⎜ 0
⎜⎜ 𝑆𝛼 𝐶𝛼 𝑑 ⎟⎟⎟⎟
⎝ ⎟⎠
0 0 0 1
Hence
11
⎛ 𝜋 ⎞ ⎛
𝜋 ⎞
⎜⎜cos 𝜃1 − sin 𝜃1 cos sin 𝜃1 sin
0 ⎟⎟ ⎜⎜cos 𝜃1 0 sin 𝜃1 0 ⎟⎟
⎜⎜ 2 ⎟2
⎟ ⎜ ⎜ ⎟⎟
⎜⎜ sin 𝜃 cos 𝜃1 cos 2 − cos 𝜃1 sin 2 0 ⎟⎟⎟⎟ ⎜⎜⎜⎜ sin 𝜃1 0 − cos 𝜃1 0 ⎟⎟⎟⎟
𝜋 𝜋
⎜ 1
𝐴1 = ⎜⎜⎜ ⎟⎟ = ⎜⎜ ⎟
𝐿1 ⎟⎟⎟⎟
𝜋 𝜋
⎜⎜ 0
⎜⎜ sin 2 cos 2 𝐿1 ⎟⎟⎟ ⎜⎜⎜ 0 1 0
⎝ ⎟⎠ ⎜⎝ ⎟⎠
0 0 0 1 0 0 0 1
⎛ ⎞ ⎛ ⎞
⎜⎜cos 𝜃2 − sin 𝜃2 cos 0 sin 𝜃2 sin 0 𝐿2 cos 𝜃2 ⎟⎟ ⎜⎜cos 𝜃2 − sin 𝜃2 0 𝐿2 cos 𝜃2 ⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ sin 𝜃 cos 𝜃2 cos 0 − cos 𝜃2 sin 0 𝐿2 sin 𝜃2 ⎟⎟⎟⎟ ⎜⎜⎜⎜ sin 𝜃2 cos 𝜃2 0 𝐿2 sin 𝜃2 ⎟⎟⎟⎟
⎜ 2
𝐴2 = ⎜⎜⎜ ⎟⎟ = ⎜⎜ ⎟⎟
⎜⎜ 0
⎜⎜ sin 0 cos 0 0 ⎟⎟ ⎜⎜ 0 0 1 0 ⎟⎟
⎝ ⎟⎟⎠ ⎜⎜⎝ ⎟⎟
⎠
0 0 0 1 0 0 0 1
⎛ ⎞ ⎛ ⎞
⎜⎜cos 𝜃3 − sin 𝜃3 cos 0 sin 𝜃3 sin 0 𝐿3 cos 𝜃3 ⎟⎟ ⎜⎜cos 𝜃3 − sin 𝜃3 0 𝐿3 cos 𝜃3 ⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ sin 𝜃 cos 𝜃3 cos 0 − cos 𝜃3 sin 0 𝐿3 sin 𝜃3 ⎟⎟⎟⎟ ⎜⎜⎜⎜ sin 𝜃3 cos 𝜃3 0 𝐿3 sin 𝜃3 ⎟⎟⎟⎟
⎜ 3
𝐴3 = ⎜⎜⎜ ⎟⎟ = ⎜⎜ ⎟⎟
⎜⎜ 0
⎜⎜ sin 0 cos 0 0 ⎟⎟ ⎜⎜ 0
⎟⎟ ⎜⎜ 0 1 0 ⎟⎟
⎟⎟
⎝ ⎠ ⎝ ⎠
0 0 0 1 0 0 0 1
𝑇30 = 𝐴1 𝐴2 𝐴3
⎛ ⎞⎛ ⎞⎛ ⎞
⎜⎜cos 𝜃1 0 sin 𝜃1 0 ⎟⎟ ⎜⎜cos 𝜃2 − sin 𝜃2 0 𝐿2 cos 𝜃2 ⎟⎟ ⎜⎜cos 𝜃3 − sin 𝜃3 0 𝐿3 cos 𝜃3 ⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ sin 𝜃 0 − cos 𝜃1 0 ⎟⎟⎟⎟ ⎜⎜⎜⎜ sin 𝜃2 cos 𝜃2 0 𝐿2 sin 𝜃2 ⎟⎟⎟⎟ ⎜⎜⎜⎜ sin 𝜃3 cos 𝜃3 0 𝐿3 sin 𝜃3 ⎟⎟⎟⎟
⎜ 1
= ⎜⎜⎜ ⎟⎜ ⎟⎜ ⎟⎟
⎜⎜ 0
⎜⎜ 1 0 𝐿1 ⎟⎟⎟⎟ ⎜⎜⎜⎜ 0 0 1 𝐿1 ⎟⎟⎟⎟ ⎜⎜⎜⎜ 0 0 1 0 ⎟⎟
⎟⎟
⎝ ⎟⎠ ⎜⎝ ⎟⎠ ⎜⎝ ⎠
0 0 0 1 0 0 0 1 0 0 0 1
For verification of the above, let 𝜃1 = 0, 𝜃2 = 0, 𝜃3 = 0 then the fourth column of 𝑇30 gives the position
vector of the end effector relative to the base when the manipulator is in the position in the problem.
Substituting these values for the angles gives
⎛ ⎞
⎜⎜1 0 0 𝐿2 + 𝐿3 ⎟⎟
⎜⎜⎜ ⎟⎟
0
⎜⎜0 0 −1
⎜ 0 ⎟⎟⎟⎟
𝑇3 = ⎜⎜ ⎟
⎜⎜0 1 0
⎜⎜ 𝐿1 ⎟⎟⎟⎟
⎝ ⎟⎠
0 0 0 1
The above says that the end effector is at position vector 𝑝0 = (𝐿2 + 𝐿3 , 0, 𝐿1 ) which means 𝑥3 =
𝐿2 + 𝐿3 , 𝑦3 = 0, 𝑧3 = 𝐿1 . From the diagram this result is correct.
PART (B) For the second manipulator, the same steps were repeated. The first step is to assign the
axes for each link as follows
12
3 joints, 4 links
L2
L1 x2 y3
link 3 x3
d2
y0 y2
x1 z1 3 L3
1
x0
o0 is base frame
link 0 o1 is link 1 frame (prismatic)
o2 is link 2 frame
o3 is link 3 frame
The forward transformations using equation (3.10) on page 77 of the textbook gives
⎛ ⎞
⎜⎜𝐶𝜃 −𝑆𝜃 𝐶𝛼 𝑆𝜃 𝑆𝛼 𝑎𝐶𝜃 ⎟⎟
⎜⎜⎜ ⎟⎟
⎜⎜ 𝑆𝜃 𝐶𝜃 𝐶𝛼 −𝐶𝜃 𝑆𝛼 𝑎𝑆𝜃 ⎟⎟⎟⎟
𝐴 = ⎜⎜⎜ ⎟
⎜⎜ 0
⎜⎜ 𝑆𝛼 𝐶𝛼 𝑑 ⎟⎟⎟⎟
⎝ ⎟⎠
0 0 0 1
Using the above, and noting that cos �𝑥 + 900 � = − sin 𝑥,sin �𝑥 + 900 � = cos 𝑥,cos �𝑥 − 900 � = sin 𝑥 and
sin �𝑥 − 900 � = − cos (𝑥) results in
13
⎛ 𝜋 𝜋 ⎞ ⎛ ⎞
⎜⎜cos (𝜃1 + 90) − sin (𝜃1 + 90) cos sin (𝜃1 + 90) sin 2 0⎟⎟ ⎜⎜− sin 𝜃1 0 cos 𝜃1 0⎟⎟
⎜⎜ 2 ⎟ ⎜ ⎟⎟
⎜⎜ sin (𝜃 + 90) cos (𝜃 + 90) cos 𝜋 − cos (𝜃 + 90) sin 𝜋 0⎟⎟⎟ ⎜⎜⎜ cos 𝜃 0 sin 𝜃 0 ⎟⎟
⎜ 1 1 1 ⎟⎟ ⎜⎜ 1 1 ⎟⎟
𝐴1 = ⎜⎜⎜ 𝜋
2
𝜋
2 ⎟ =
⎟⎟ ⎜⎜ 0 ⎜ ⎟⎟
⎜⎜
⎜⎜ 0 sin cos 0 ⎟⎟ ⎜⎜ 1 0 0 ⎟⎟
⎟⎟
⎝
2 2 ⎟⎠ ⎜⎝ ⎠
0 0 0 1 0 0 0 1
⎛ ⎞ ⎛ ⎞
⎜⎜cos 0 − sin 0 cos �−900 � sin 0 sin �−900 � 0 cos 0 ⎟⎟ ⎜⎜1 0 0 0 ⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ sin 0 cos 0 cos �−900 � − cos 0 sin �−900 � ⎟
0 sin 0 ⎟⎟⎟ ⎜⎜⎜0 0 1 ⎜ 0 ⎟⎟
⎜⎜ ⎟⎟
𝐴2 = ⎜⎜ ⎟
⎟ = ⎜
⎜ ⎟⎟
⎜⎜ 0 sin �−90 0 � cos �−90 0 � 𝐿 + 𝐿 + 𝑑 ⎟
⎟ ⎜
⎜0 −1 0 𝐿 + 𝐿 + 𝑑 ⎟⎟
⎜⎜⎝ ⎟⎟⎠ ⎜⎜⎝ ⎟⎟
1 2 2 1 2 2
⎠
0 0 0 1 0 0 0 1
⎛ ⎞
⎛ ⎞ ⎜⎜ sin 𝜃3 cos 𝜃3 0 𝐿3 sin 𝜃3 ⎟⎟
⎜⎜cos �𝜃3 − 900 � − sin �𝜃3 − 900 � cos 0 sin �𝜃3 − 900 � sin 0 𝐿3 cos �𝜃3 − 900 �⎟⎟ ⎜⎜ ⎟
⎜⎜ ⎟
0� ⎟
⎜⎜− cos 𝜃 sin 𝜃 0 −𝐿 cos 𝜃 ⎟⎟⎟
⎜⎜ sin �𝜃 − 900 � cos �𝜃 − 90 0 � cos 0 − cos �𝜃 − 90 0 � sin 0 𝐿 sin �𝜃 − 90 ⎟⎟
⎟⎟⎟ = ⎜
⎜ 3 3 3 3 ⎟⎟
𝐴2 = ⎜⎜⎜⎜ 3 3 3 3 3
⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ 0 sin 0 cos 0 0 ⎟⎟ ⎜
⎜⎜ 0 0 1 0 ⎟⎟
⎜⎝ ⎠ ⎜⎝ ⎟⎟
0 0 0 1 ⎠
0 0 0 1
𝑇30 = 𝐴1 𝐴2 𝐴3
⎛ ⎞⎛ ⎞⎛ ⎞
⎜⎜− sin 𝜃1 0 cos 𝜃1 0⎟⎟ ⎜⎜1 0 0 0 ⎟⎟ ⎜⎜ sin 𝜃3 cos 𝜃3 0 𝐿3 sin 𝜃3 ⎟⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟
⎜⎜ cos 𝜃 0 sin 𝜃1 ⎟ ⎜
0⎟⎟⎟ ⎜⎜⎜0 0 1 0 ⎟⎟ ⎜⎜− cos 𝜃 sin 𝜃3 0 −𝐿3 cos 𝜃3 ⎟⎟⎟⎟
⎜ 1 ⎟⎟ ⎜⎜ 3
= ⎜⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜ 0 1 0 ⎟ ⎜ ⎟
0⎟⎟ ⎜⎜0 −1 0 𝐿1 + 𝐿2 + 𝑑2 ⎟⎟ ⎜⎜ 0 ⎜ 0 1 0 ⎟⎟
⎜⎜ ⎟⎟
⎝ ⎠⎟ ⎝⎜ ⎟⎠ ⎜⎝ ⎠
0 0 0 1 0 0 0 1 0 0 0 1
⎛ ⎞
⎜⎜ cos 𝜃1 cos 𝜃3 − sin 𝜃1 sin 𝜃3 − cos 𝜃1 sin 𝜃3 − cos 𝜃3 sin 𝜃1 0 cos 𝜃1 (𝐿1 + 𝐿2 + 𝑑2 ) + 𝐿3 cos 𝜃1 cos 𝜃3 − 𝐿3 sin 𝜃1 sin 𝜃3 ⎟⎟
⎜⎜ ⎟⎟
⎜⎜cos 𝜃 sin 𝜃 + cos 𝜃 sin 𝜃 cos 𝜃1 cos 𝜃3 − sin 𝜃1 sin 𝜃3 0 sin 𝜃1 (𝐿1 + 𝐿2 + 𝑑2 ) + 𝐿3 cos 𝜃1 sin 𝜃3 + 𝐿3 cos 𝜃3 sin 𝜃1 ⎟⎟⎟
= ⎜⎜ 1 3 3 1
⎟⎟
⎜⎜ ⎟⎟
⎜⎜⎜ 0 0 1 0 ⎟⎟
⎝ ⎠
0 0 0 1
To verify the above, let 𝜃1 = 0, 𝜃3 = 3600 , 𝑑2 = 0 then the fourth column of 𝑇30 gives the position vector
of the end effector relative to the base when the manipulator is in a straight horizontal position
⎛ ⎞
⎜⎜1 0 0 𝐿1 + 𝐿2 + 𝐿3 ⎟⎟
⎜⎜ ⎟⎟
⎜⎜0 1 0 0 ⎟⎟
0 ⎜⎜ ⎟⎟
𝑇3 = ⎜⎜ ⎟⎟
⎜⎜0 0 1 0 ⎟⎟
⎜⎜ ⎟⎟
⎝ ⎠
0 0 0 1
The above results show that 𝑥3 = 𝐿1 + 𝐿2 + 𝐿3 which is the expected result.
14
0.6 Problem 6
ME / ECE 739: Advanced Robotics Homework #1
0.6.1 problem February 19th (Thursday)
Due: description
► Write Matlab code to plot the position (x, y, and z coordinates) of the end-effector (point E) as a
function of time. Your plots should match the plots shown below. The joint motion, as a
function of time, is given below. Make sure to include all supporting functions, including any
custom plotting routines, with your homework submission. Your homework submission must
provide clear, easy instructions to run you Matlab code
10 15 10
0 10 0
x
z
-10 5 -10
0 5 0 5 0 5
time time time
► Write Matlab code to animate the manipulator using the Matlab scripts provided on the
Learn@UW course page. The joint motion, as a function of time, is given above. Make sure to
include all supporting functions, including any custom plotting routines, with your homework
submission. Your homework submission must provide clear, easy instructions to run you Matlab
code. To maintain consistency, please use the following rendering window view parameters.
Page 4 of 4
ME / ECE 739: Advanced Robotics Homework #1 15
th
Due: February 19 (Thursday)
► Derive the homogeneous transforms that relate the successive frames as a function of the joint
0 1 2 3 4 5
variable and manipulator geometric parameters (i.e. find: T1 , T2 , T3 , T4 , T5 , T6 ). Use the
coordinate frames as defined in the figure. Note that the frames are fixed to the links at various
locations (e.g. proximal end, distal end). Assume that q1 equals zero when the manipulator lies in
the plane of the page. Attendee
► Write Matlab code to calculate the homogeneous transforms derived above as a function of the
joint variables q1, q2, q3, q4, and q5.
► Write Matlab code to calculate the homogeneous transformation matrices that describe the frame
0 0 0 0 0 0
displacements relative to the ground frame {0} (i.e. numerically evaluate T1 , T2 , T3 , T4 , T5 , T6 ).
(continued)
Page 3 of 4
ME / ECE 739: Advanced Robotics Homework #1 16
th
Due: February 19 (Thursday)
► Write Matlab code to plot the position (x, y, and z coordinates) of the end-effector (point E) as a
function of time. Your plots should match the plots shown below. The joint motion, as a
function of time, is given below. Make sure to include all supporting functions, including any
custom plotting routines, with your homework submission. Your homework submission must
provide clear, easy instructions to run you Matlab code
10 15 10
0 10 0
x
z
-10 5 -10
0 5 0 5 0 5
time time time
► Write Matlab code to animate the manipulator using the Matlab scripts provided on the
Learn@UW course page. The joint motion, as a function of time, is given above. Make sure to
include all supporting functions, including any custom plotting routines, with your homework
submission. Your homework submission must provide clear, easy instructions to run you Matlab
code. To maintain consistency, please use the following rendering window view parameters.
0.6.2 solution
Page 4 of 4
The homogeneous transformation 𝑇𝑖𝑖−1 was derived by inspection giving the following results
17
⎛ ⎞ ⎛ ⎞ ⎛ ⎞
⎜⎜0
⎜⎜
sin 𝑞1 cos 𝑞1 0 ⎟⎟
⎟⎟
⎜⎜ cos 𝑞2 0
⎜⎜
sin 𝑞2 0⎟⎟
⎟⎟
⎜⎜ cos 𝑞3 sin 𝑞3 0 0 ⎟⎟
⎜⎜ ⎟⎟
⎜⎜1 0 0 𝐿1 ⎟⎟⎟ 1 ⎜⎜⎜⎜ 0
⎟ 1 0 0⎟⎟⎟⎟ 2 ⎜⎜⎜⎜ 0 0 −1 0 ⎟⎟⎟⎟
⎜
𝑇10 = ⎜⎜⎜ ⎟⎟ , 𝑇2 = ⎜⎜ ⎟ , 𝑇3 = ⎜⎜ ⎟
⎜⎜⎜0 cos 𝑞1 − sin 𝑞1 0 ⎟⎟⎟ ⎜⎜− sin 𝑞2 0
⎜⎜ cos 𝑞2 0⎟⎟⎟⎟ ⎜⎜− sin 𝑞3 cos 𝑞3 0 𝐿2 ⎟⎟⎟
⎜ ⎟⎟
⎜⎝ ⎟⎠ ⎝ ⎟⎠ ⎜⎝ ⎠
0 0 0 1 0 0 0 1 0 0 0 1
⎛ ⎞ ⎛ ⎞ ⎛ ⎞
⎜⎜1 0 0 0 ⎟⎟ ⎜⎜ cos 𝑞5 sin 𝑞5 0 0⎟⎟ ⎜⎜− cos 𝑞6 sin 𝑞6 0 0 ⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟
⎜⎜0 1 0 𝐿3 + 𝑞4 ⎟⎟⎟ 4 ⎜⎜⎜⎜− sin 𝑞5 cos 𝑞5
⎟ 0 0⎟⎟⎟⎟ 5 ⎜⎜⎜⎜ 0 0 1 𝐿5 ⎟⎟⎟⎟
⎜
𝑇4 = ⎜⎜⎜
3
⎟ , 𝑇5 = ⎜⎜ ⎟ , 𝑇6 = ⎜⎜ ⎟
⎜⎜0
⎜⎜ 0 1 0 ⎟⎟⎟⎟ ⎜⎜ 0
⎜⎜ 0 1 0⎟⎟⎟⎟ ⎜⎜ sin 𝑞6 cos 𝑞6 0 0 ⎟⎟⎟
⎜ ⎟⎟
⎝ ⎟⎠ ⎝ ⎟⎠ ⎜⎝ ⎠
0 0 0 1 0 0 0 1 0 0 0 1
10 15 10
0 10
x
z
0
5
−10 −10
0
0 5 0 5 0 5
t (sec) t (sec) t (sec)
The following Matlab script problem_6_part_1.m calculates the homogeneous transformation 𝑇60
and plots the above figures
%This scripts plots the x,y,z coordinates of the end effector E
%for problem 5, HW1 , ME 739.
%to run, type this script name on the Matlab console
% problem_6_part_1
%The matlab path must include the ME 739 rendering software
%Nasser M. Abbasi 2/16/15
0 cos(q1) −sin(q1) 0;
0 0 0 1];
T34 = [1 0 0 0;
0 1 0 L3+q4;
0 0 1 0;
0 0 0 1];
subplot(1,3,2);
19
plot(timeScale,coords(:,2), 'LineWidth',1.5);
xlabel('t (sec)'); ylabel('y');
grid; axis square; xlim([0 2*pi]);ylim([0 18]);
%axis square; axis tight
set(gca, 'GridLineStyle', '−');
subplot(1,3,3);
plot(timeScale,coords(:,3),'LineWidth',1.5);
xlabel('t (sec)'); ylabel('z');
grid; axis square; xlim([0 2*pi]);ylim([−13 15]);
set(gca, 'GridLineStyle', '−');
%export_fig(gcf,'problem_6_part_1.pdf');
The following matlab script calculates 𝑇10 , 𝑇20 , 𝑇30 , 𝑇40 , 𝑇50 , 𝑇60 , where 𝑇20 = 𝑇10 𝑇21 , 𝑇30 = 𝑇20 𝑇32 , 𝑇40 = 𝑇30 𝑇43 , 𝑇50 =
𝑇40 𝑇54 , 𝑇60 = 𝑇50 𝑇65
For example, for 𝑇20 the result is
⎛ ⎞
⎜⎜0 sin 𝑞1 cos 𝑞1 0 ⎟⎟
⎜⎜ ⎟⎟
⎜⎜1 0 0 𝐿 ⎟⎟
⎜ 1 ⎟⎟
𝑇10 = ⎜⎜⎜ ⎟
⎜⎜0 cos 𝑞1 − sin 𝑞1 0 ⎟⎟⎟
⎜⎜ ⎟⎟
⎝ ⎠
0 0 0 1
⎛ ⎞⎛ ⎞ ⎛ ⎞
⎜⎜0 sin 𝑞1 cos 𝑞1 0 ⎟⎟ ⎜⎜ cos 𝑞2 0 sin 𝑞2 0⎟⎟ ⎜⎜− cos 𝑞1 sin 𝑞2 sin 𝑞1 cos 𝑞1 cos 𝑞2 0 ⎟⎟⎟
⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟
⎜⎜1 0 0 𝐿1 ⎟⎟⎟⎟ ⎜⎜⎜⎜ 0 1 0 0⎟⎟⎟⎟ ⎜⎜⎜⎜ cos 𝑞2 0 sin 𝑞2 𝐿1 ⎟⎟⎟⎟
⎜
𝑇20 = 𝑇10 𝑇21 = ⎜⎜⎜ ⎟⎜ ⎟⎟ = ⎜⎜ ⎟
⎜⎜⎜0 cos 𝑞1 − sin 𝑞1 0 ⎟⎟⎟⎟ ⎜⎜⎜⎜− sin 𝑞2 0 cos 𝑞2 0⎟⎟⎟ ⎜⎜⎜ sin 𝑞1 sin 𝑞2 cos 𝑞1 − cos 𝑞2 sin 𝑞1 0 ⎟⎟⎟⎟
⎜⎝ ⎟⎠ ⎜⎝ ⎟⎠ ⎜⎝ ⎟⎠
0 0 0 1 0 0 0 1 0 0 0 1
The complete calculation was done in the following Matlab script problem_6_part_2.m. The script
is run by typing its name on the Matlab console.
%This calculates T01,T02,T03,T04,T05,T06 numerically
%for problem 5, HW1 , ME 739.
%to run, just type this script name on the Matlab console
% problem_6_part_2
%
%The matlab path must include the ME 739 rendering software
%Nasser M. Abbasi 2/16/15
q6 = pi/4*sin(t);
T34 = [1 0 0 0;
0 1 0 L3+q4;
0 0 1 0;
0 0 0 1];
%now calculate all the T's at some specific time. The problem
%does not says what time instance to use, so we use t=0 for
%illustration
The above script calculates numerically all the transformation matrices using the joint variable inputs
given in the problem. At the end it prints each matrix. The problem did not indicate for which value
of 𝑡 to use to calculate the matrices, hence for illustration these are displayed for 𝑡 = 0 and 𝑡 = 1
second. A variable inside the script can be used to change the time instance. The following is the
output from running the above script for illustration
22
The manipulator was animated using the UW software. The following script problem_6_part_3.m
written for this purpose. Typing the name of the script in Matlab starts the animation.
This script assumes the Matlab path is set to include the UW rendering software.
Listing 1: problem_6_part_3.m
%This calculates T01,T02,T03,T04,T05,T06 numerically
%for problem 5, HW1 , ME 739.
%to run, just type this script name on the Matlab console
%
% problem_6_part_3
%The matlab path must include the ME 739 rendering software
%
%Nasser M. Abbasi 2/17/15
ADD_BASE = false; %set to TRUE to see base rendered, does not move.
DO_MOVIE = false; %set true to make frames for movie
ANIMATION_TIME = 10; %10 seconds to animation
DEL_T = 0.05; %time betweeb each animation loop. smaller time
%make it run slower but more accurate
T34 = [1 0 0 0;
0 1 0 L3+q4;
0 0 1 0;
0 0 0 1];
T = calcT(T02,timeScale(i));
UpdateLink(d2,T);
T = calcT(T03,timeScale(i));
UpdateLink(d3,T);
T = calcT(T04,timeScale(i));
UpdateLink(d4,T);
T = calcT(T05,timeScale(i));
UpdateLink(d5,T);
T = calcT(T06,timeScale(i));
UpdateLink(d6,T);
title(sprintf('time = %3.3f (sec)',timeScale(i)));
if DO_MOVIE
k = k+1;
I = getframe(gcf);
imwrite(I.cdata, sprintf('frame%d.png',k));
end
The following is a movie of the first few seconds of the Matlab run. (posted as animation gif in my
site).