Homework 2: ME 570 - Prof. Tron
Homework 2: ME 570 - Prof. Tron
Homework 2: ME 570 - Prof. Tron
The goal of this homework is to practice the notions about configuration spaces seen
in class. In the first part of the homework you will see how the same configuration spaces
applies to a “physical” two-link manipulator. In the second part of the homework you will go
through a couple of examples of viewing a configuration space as a differential manifold and
the corresponding implications (charts, embeddings, transfer of curves and tangents). On the
way, you will learn more about rotation matrices and function handles in Matlab.
General instructions
Programming For your convenience, together with this document, you will find a zip
archive containing Matlab files with stubs for each of the questions in this assignment; each
stub contains an automatically generated description of the function and the function header.
You will have to complete these files with the body of each function requested. The goal
of this files is to save you a little bit of time, and to avoid misspellings in the function or
argument names. The functions from the parts marked as provided (see also the Grading
paragraph below) contain already the body of the function.
Please refer to the guidelines on Blackboard under Information/Programming Tips &
Tricks/Coding Practices.
Homework report Along the programming of the requested functions, prepare a small PDF
report containing one or two sentences of comments for each question marked as report ,
and including:
• Embedded figures and outputs that are representative of those generated by your code.
• All analytical derivations if required.
You can include comments also on the questions marked as code , if you feel it necessary
(e.g., to explain any difficulty you might have encountered in the programming), but these
will not be graded. In general, do not insert the listing of the functions in the report (I will
have access to the source files); however, you can insert short snippets of code if you want to
specifically discuss them.
A small amount of beauty points are dedicated to reward reports that present their content
in a professional way (see the Grading criteria section in the syllabus).
Analytical derivations To include the analytical derivations in your report you can type
them in LATEX(preferred method), any equation editor or clearly write them on paper and
use a scanner (least preferred method).
1
Submission
The submission will be done on Gradescope through two separate assignments, one for the
questions marked as code , and one for those marked as report . See below for details. You
can submit as many times as you would like, up to the assignment deadline. Each question is
worth 1 point unless otherwise noted. Please refer to the Syllabus on Blackboard for late
homework policies.
Report Upload the PDF of you report, and then indicate, for each question marked as
report , on which page it is answered (just follow the Gradescope interface). Note that some
of the questions marked as report might include a coding component, which however will
be evaluated from the output figures you include in the report, not through automated tests.
Many of these questions are intended as checkpoints for you to visually check the results of
your functions.
Code questions Upload all the necessary .m files, both those written by you, and those
provided with the assignment. The questions marked as code will be graded using automated
tests. Shorty after submission, you will be able to see the results for some of the tests: green
and red mean that the test has, respectively, passed or not; if a test did not pass, check
for clues in the name of the test, the message provided on Gradescope, and the text of this
assignment. You can post questions on Blackboard at any time.
Note 1: The final grade will depend also on additional tests that will become visible only
after all the grades have been reviewed as a whole. It is therefore important that you examine
the results in the figures that you will generate for the report.
Note 2: The automated tests use Octave, a open-source clone of Matlab. For the purposes
of this class, you should not encounter any specific compatibility problem. If, however, you
suspect that some tests fail due to this incompatibility, please contact the instructor.
Optional and provided questions. Questions marked as optional are provided just to
further your understanding of the subject, and not for credit. Questions marked as provided
have already a solution provided in the file accompanying this assignment, which you can
directly use unless you want to answer it by yourself. If you submit an answer for optional or
provided questions I will correct it, but it will not count toward your grade.
Maximum possible score. The total points available for this assignment are 29.0 ( 28.0
from questions, plus 1.0 that you can only obtain with beauty points).
Hints
Some hints are available for some questions, and can be found at the end of the assignment
(you are encouraged to try to solve the questions without looking at the hints first). If you
use these hints, please state so in your report (your grading will not change based on this
information, but it is a useful feedback for me).
Use of external libraries and toolboxes All the problems can be solved using Matlab’s
standard features. You are not allowed to use functions or scripts from external libraries or
toolboxes (e.g., mapping toolbox), unless specifically instructed to do so (e.g., CVX).
2
Problem 1: Rotations
A 2-D rotation R ∈ R2×2 rotating points in a counterclockwise manner can be expressed as:
cos(θ) − sin(θ)
R(θ) = . (1)
sin(θ) cos(θ)
Question provided 1.1. Write a Matlab function that implements (1); this simple function
will be used in subsequent problems.
[ Rtheta ]= rot2d ( theta )
Description: Create a 2-D rotation matrix from the angle theta according to (1).
Input arguments
• theta (dim. [1 × 1]): An angle θ.
Output arguments
• Rtheta (dim. [2 × 2]): A matrix containing R(θ).
Question report 1.1 (0.5 points). Eq. 1 can also be used to build 3-D rotations for some
particular
axes of rotation.
Let rij (θ) represent the individual entries of the matrix R(θ), i.e.
r (θ) r12 (θ)
R(θ) = 11 . Explain the geometrical meaning (i.e., the rotation axis and the
r21 (θ) r22 (θ)
direction of rotation) of the following 3-D rotations:
1 0 0 1 0 0
R1 (θ) = 0 r11 (θ) r12 (θ) = 0 cos(θ) − sin(θ) (2)
0 r21 (θ) r22 (θ) 0 sin(θ) cos(θ)
r11 (θ) 0 r12 (θ) cos(θ) 0 − sin(θ)
R2 (θ) = 0 1 0 = 0 1 0 (3)
r21 (θ) 0 r22 (θ) sin(θ) 0 cos(θ)
r11 (θ) r12 (θ) 0 cos(θ) − sin(θ) 0
R3 (θ) = r21 (θ) r22 (θ) 0 = sin(θ) cos(θ) 0 (4)
0 0 1 0 0 1
−r11 (θ) −r12 (θ) 0 − cos(θ) sin(θ) 0
R4 (θ) = −r21 (θ) −r22 (θ) 0 = − sin(θ) − cos(θ) 0 (5)
0 0 1 0 0 1
3
We consider three reference frames: W, which is fixed to the world, B1 , which rotates
with the first link, and B2 , which rotates with the second link.
The translation of the first link in the world reference frame is zero, i.e., W T B1 = 0, while
the rotation WRB1 has angle θ1 ; the translation of the second link in the first link’s reference
5
is B1 T B2 = , while the rotation B1 RB2 has angle θ2 (see also the annotations in Figure 1).
0
Question report 2.1 (1.5 points). Derive two separate expressions that compute the
coordinates of a point p expressed in W, W p, for each one of these inputs, respectively:
1) The coordinates of the point given in B1 , denoted as B1 p;
2) The coordinates of the point given in B2 , denoted as B2 p.
Please make sure that the use of rotation matrices is clear, otherwise you might not receive
full credit for your answer; although we will use these computations for 2-D points, the same
expressions should hold for 3-D points.
Question code 2.1. Create a function to compute the coordinates of the end effector and
of the vertices of the
manipulator for a given configuration. The end effector is defined to be
5
the point B2 peff = .
0
6 θ2
5
4
B2
3
1 B1
θ1
0
W
-3 -2 -1 0 1 2 3 4 5 6
Figure 1: The two-link manipulator for this exercise. The angle of the first link θ1 is measured
from the horizontal x axis, the angle of the second link θ2 is measured with respect to the
axis of the first link.
4
Output arguments
• vertexEffectorTransf (dim. [2 × 1]): The coordinates of the point B2 peff
transformed according to θ1 , θ2 (i.e., the coordinates of the points in the world
frame); this point represents the transformed end effector of the manipulator.
vertices1Transf (dim. [2 × 4]), vertices2Transf (dim. [2 × 12]): the coor-
dinates contained in the arrays vertexEffector , vertices1 and vertices2 ,
generated by twolink_polygons ( ), transformed according to θ1 , θ2 .
Requirements: Use the results from Question report 2.1 to guide your implementation.
This function must use twolink_polygons ( ) to obtain the vertices of the polygons of
the matrix, and it must use rot2d ( ) from Question provided 1.1. Note that here we
are simply computing the vertices of the transformed polygons, without plotting them.
The next function will be used to plot the transformed vertices.
Question provided 2.1. Create a function that plots the manipulator in a given configuration
according to the following.
twolink_plot ( theta,color,thetaDot )
Description: This function should use twolink_kinematicMap ( ) from the previous
question together with the function polygon_plot ( ) from Homework 1 to plot the
manipulator.
Input arguments
• theta (dim. [2 × 1]): A vector containing the configuration angles as used by
twolink_kinematicMap ( ).
• color (dim. [1 × 1], type string ): Color specification (e.g., ’r’ , ’g’ , ’b’ ).
• thetaDot (dim. [2 × 1]) optional : A vector containing the time derivatives of
the joint angles. These are visualized as black arrows perperndicular to the links,
each with length proportional to the respective component of the velocities. If
this argument is omitted, the arrows are not included in the plot.
Note that, by default, this plotting function does not enforce the same proportions for
the horizontal and vertical axes (i.e., the manipulator might appear skewed). To enforce this,
use the command axis equal .
Question code 2.2. Create a function that checks for collisions between the manipulator
and a sparse set of points (representing obstacles).
[ flagTheta ]= twolink_isCollision ( theta,points )
Description: For each specified configuration, returns true if any of the links of the
manipulator collides with any of the points, and false otherwise. Use the function
polygon_isCollision ( ) to check if each link of the manipulator is in collision.
Input arguments
• theta (dim. [2 × NTheta]): An array of configurations where each column repre-
sents a different configuration according to the convention used by twolink_kinematicMap ( ).
5
• points (dim. [2 × NPoints]): An array of coordinate of points (obstacles). Each
column represents a different point.
Output arguments
• flagTheta (dim. [1 × NTheta], type logical ): each element of this array
should be true if the configuration specified in the corresponding column of
theta causes a collision, and false otherwise.
Requirements: For this question, do not consider self-collision (i.e., if the two polygons
overlap but they do not cover any of the points, then it is not a collision).
Question report 2.2. Make a function that combines the functions from the previous two
questions.
twolink_plotCollision ( theta,points )
Description: This function should:
1) Plot the points specified by points as black asterisks.
2) Use twolink_isCollision ( ) for determining if each configuration is a collision
or not.
3) Use twolink_plot ( ) to plot the manipulator for all configurations, using a red
color when the manipulator is in collision, and green otherwise.
Input arguments
• theta (dim. [2 × NTheta]), points (dim. [2 × NPoints]): As used by
twolink_isCollision ( )
All the drawings from the different configurations should overlap on the same figure (it is fine
if many configurations overlap). You should use the command axis equal if you want to
avoid seeing the manipulator “distorted” (e.g., to see right angles as real right angles on the
screen). In your report, include the output of the function twolink_plotCollision_test ( )
provided in the next question.
Question provided 2.2. This function can be used to test and see if the other functions
from this problem are working correctly.
twolink_plotCollision_test ( )
Description: This function generates 30 random configurations, loads the points
variable from the file twolink_testData.mat (provided with the homework), and then
display the results using twolink_plotCollision to plot the manipulator in red if it
is in collision, and green otherwise.
Question optional 2.1. Write a function twolink_freeSpace ( ) that loads points from
twolink_testData.mat , calls [ flag ]= twolink_isCollision ( ) for θ1 , θ2 sampled along
a regular fine grid, and stores the results in a matrix of logical values freeSpaceMap . Display
freeSpaceMap as an image. This is an approximate representation of the free configuration
space. You can use imagesc ( ) to plot the map as an image. Using the functions above,
generating the map should be straightforward, but it might take a very long time.
6
Problem 3: Grids, function handles, and evaluating functions on grids
In this homework and in future assignments, we will need to evaluate different functions
on discretized domains (i.e., on regular grids). To aid in this representation, we will use a
structure grid with the following fields:
• xx (dim. [1 × NGrid]) and yy (dim. [1 × NGrid]): arrays containing, respectively,
the x and y values corresponding to the horizontal and vertical lines of the grid.
• F (dim. [ NGrid × NGrid × k ]): an array to store the values of the function. The
dimension k is the dimension of the values of the function (i.e., the function evaluated
might return a vector for each point). The first and second index in the array F
correspond, respectively, to xx and yy (see Figure 2 for a visualization).
Function handles If you have a function implemented in a Matlab file, e.g. myfun.m ,
you can create a handle to the function by prefixing the @ sign to the function name, i.e.,
@myfun . An handle can be stored in a variable, e.g., F=@myfun . You can then use the
variable to call the function as if you were calling it from the original name: in our example,
F(x) and myfun(x) would then produce the same results. The advantage is that the
variable an handle can be passed as an argument to another function, as it will be done in
the next question. This is an advanced but very useful feature of Matlab; please search for
“Create Function Handle” in the Matlab help to learn more.
Question provided 3.1. Implement a function that evaluates another function, passed as
an handle, on a grid.
[ grid ]= grid_eval ( grid,fun )
Description: This function evaluates the function fun (which should be a function
handle) on each point defined by the grid.
Input arguments
• grid (, type struct ): a struct representing a grid as detailed before. Only the
fields xx and yy need to be present (the field F will be added by this function).
• fun (, type function handle ): an handle to a function that takes a vector x
of dimension [2 × 1] as a single argument, and returns a scalar or a vector. An
example of this function could be the function norm ( ).
Output arguments
• grid (, type struct ): a struct which is the same as the input one, but where
the field F contains the evaluation of the function fun on each point on the grid.
We will use this function any time we will need to evaluate a map on a rectangular region.
Problem 4: Charts for the circle using rotations
In this question you will consider the circle S1 as a manifold, and consider charts based on
rotation matrices. For this problem, most questions are optional, but understanding them
will help you with the answers to Problem 5.
7
y
F(
F( NG
NG ri
yy(NGrid) ri d,
d, NG
1) ri
d)
.. .
. ..
F( F( F(
yy(3) 1, 2, 3,
3) 3) 3)
F( F( F(
yy(2) 1, 2, 3,
2) 2) 2)
···
F(
F( F( F( 1,
yy(1) 1, 2, 3, NG x
1) 1) 1) ri
d)
xx(1) xx(2) xx(3) xx(NGrid)
8
From angles to circles to the torus using rotations. Consider the following maps
1
φcircle (θ) = R(θ) . (6)
0
Equation (1) defines a 2-D rotation with angle θ ∈ R, while (6) maps an angle θ to a point on
the circle S1 . By choosing different intervals for θ, we can cover different parts of the circle.
Question report 4.1. Show that R(θ) is a rotation (i.e., R(θ) ∈ SO(2)) and that
φcircle (θ) ∈ S1 for all θ ∈ R.
Question optional 4.1. Two charts are necessary and sufficient to cover the circle, for
instance {U1 , φcircle } and {U2 , φcircle }, where the intervals U1 , U2 are U1 = (− 34 π; 34 π) and
U2 = ( 41 π; 74 π). Why do we need two charts to cover the circle?
Question optional 4.2. Generate a vector theta1 and theta2 of points that uniformly
cover the intervals U1 , U2 , respectively. Plot the results of applying φcircle to theta1 and
theta2 on the same plot, but in different colors. Explain what you see.
Problem 5: Charts for the torus using rotations
In this question you will consider the torus T = S1 × S1 as a manifold, and use the material
from the previous question to make charts for the torus. You will then check that these
charts define an embedding, and use this embedding to visualize the torus along with curves
in the configuration space and their tangents.
From angle pairs to the torus using. A point on the torus can be identified with two
θ
angles, which we collect in a single vector θ~ = 1 . Let r = 3, and consider the map:
θ2
1 0 r
~ = diag R(θ2 ), 1 0 0 φcircle (θ1 ) + 0 ,
φtorus (θ) (7)
0 1 0
where diag is an operator that creates a block diagonal matrix with its arguments (in Matlab,
this is implemented by the function blkdiag ( )), and R and φ are the same as in (1), (6).
This map might look a little intimdating at first, but, intuitively, it boils down to setting
the circle from Problem 4 onto the x − z plane, translating it along the x axis, and then
revolving it around the z axis. As you will see later, the map φtorus can be used to embed a
torus in R3 , and use it to represent the configuration space of the two-link manipulator.
Question code 5.1. Implement the following function, which will be used to embed the
torus plot in the Euclidean space R3 .
[ xTorus ]= torus_phi ( theta )
Description: Implements equation (7).
Input arguments
• theta (dim. [2 × NPoints]): An array of values for θ~ (each column corresponds
~
to a different θ).
Output arguments
9
~ corresponding to all the
• xTorus (dim. [3 × NPoints]): The values of φtorus (θ)
angles in theta .
Question report 5.1. Choose N square regions Ui ⊂ R2 , i ∈ {1, . . . , N }, such that these
regions together with the map φtorus define an atlas for the torus; what is the minimum
sufficient and necessary number of charts N ? This question is best answered while working on it
in parallel with the next. You should not use charts that start and end at the same coordinates
(e.g., U1 = (0, 2π) × (0, 2π)), but leave small “gaps” (e.g., U1 = (0, 2π − 0.1) × (0, 2π − 0.1));
this typically does not fundamentally change the number of charts that you use, and it will
make it easier to spot problems. In the report, include a picture illustrating the charts on the
flat representation of the torus seen in class (the square with opposite sides glued together).
Question report 5.2. Make a function that shows the embedding of the torus given by
(7).
torus_plotCharts ( )
Description: For each one of the chart domains Ui from the previous question:
1) Fill a grid structure with fields xx and yy that define a grid of regular point
in Ui . Use NGrid=33 .
2) Call the function grid_eval ( ) with argument grid and torus_phi ( ).
3) Plots the surface described by the values in grid.F using the Matlab function
surf ( ) in a separate figure.
Plot a final additional figure showing all the charts at the same time (using the commands
hold on and hold off ). To better show the overlap between the charts, you can use
different colors each one of them, and the ’FaceAlpha’,0.75 option for surf ( ).
Requirements:
Use the same charts that you gave for Question report 5.1. The output of the function should
show a torus (i.e., a doughnut) with different sections in different colors. To give you an idea,
Figure 3 gives an example of how the final figure would look like for a sphere (that, as seen
in class, requires N = 2 charts).
Question report 5.3. Using what seen in class, explain why, in the previous question
(Question report 5.2), you should not see the same colored part overlap on itself, and why
you should not see parts of the surface of the torus that are not covered.
Question provided 5.1. The following function can be used to generate points on a curve
θ(t) that is a straight line.
[ thetaPoints ]= line_linspace ( aLine,bLine,tMin,tMax,NPoints )
~ =
Description: Generates a discrete number of NPoints points along the curve θ(t)
2
( a(1) · t + b(1) , a(2) · t + b(2) ) ∈ R for t ranging from tMin to tMax .
Input arguments
• aLine (dim. [2 × 1]), bLine (dim. [2 × 1]): First and second parameter of the
curve.
10
Figure 3: Example of visualization of a minimal choice of charts for the sphere.
Question report 5.4 (0.5 points). Compute the tangent θ̇(t) of the curve θ(t) defined in
the previous question.
Question code 5.2. This question is based on the idea of generating a curve θ(t) ~ ∈ R2
(in fact, a straight line) in the configuration space, “push it through” the embedding φtorus to
~
3
generate the curve x(t) = φtorus θ(t) ∈ R .
[ xPoints ]= torus_phiPushCurve ( a,b )
~
Description: This function evaluates the curve x(t) = φtorus θ(t) ∈ R3 at NPoints=31
~ using line_linspace ( ) with tMin = 0 and
points generated along the curve θ(t)
tMax = 1, and a, b as given in the input arguments.
Input arguments
• a (dim. [2 × 1]), b (dim. [2 × 1]): Parameters of the curve to be passed to
11
line_linspace ( ).
Output arguments
• xPoints (dim. [3 × NPoints]): Array of points generated by evaluating φtorus
on thetaPoints (i.e., array of points on the curve x(t)).
Question provided 5.2. The following function combine all the functions from this problem
to visualize the embedding of the torus, together with four curves.
torus_plotChartsCurves ( )
Description: The function should iterate over the following four curves:
• a=[3/4*pi;0] ,
• a=[3/4*pi;3/4*pi] ,
• a=[-3/4*pi;3/4*pi] ,
• a=[0;-3/4*pi] ,
and b=[-1;-1] . The function should show an overlay containing:
• The output of torus_plotCharts ( );
• The output of the functions torus_pushCurve ( ) for each one of the curves.
Requirements: This function needs to use plot3 ( ) to show the output of torus_pushCurve ( ).
You should see that all the curves start at the same point on the torus.
optional Use different colors to display the results of the different curves.
12
[ vertexEffectorDot ]= twolink_jacobian ( theta,thetaDot )
Description: Implement the map for the Jacobian of the position of the end effector with
respect to the joint angles as derived in Question report 6.1.
Input arguments
θ
• theta (dim. [2 × NTheta]) An array where each column contains 1 , two joint
θ2
angles for the two-link manipulator.
• thetaDot (dim. [2 × NTheta]): An array where each column contains the time
derivatives of the joint angles, θ̇, for each one of the corresponding columns in
theta .
Output arguments
• vertexEffectorDot (dim. [2 × NTheta]): An array where each column contains
the time derivative of the end effector position, expressed in the world’s frame,
d W
dt
( peff ), for the corresponding columns in the input arguments.
Question report 6.2. Assume θ(t) follow a line as defined in Questions report 5.3 and re-
d W
port
5.4.
What
isthe corresponding value of dt ( peff )for the following values of θ,
0 0 π π 1 0
, π , π , , and the following values of a , , with b = 0; in to-
0 2 2
π 0 1
d W
tal, you need to compute eight values for dt ( peff ). You can do the calculations by hand,
or use the function twolink_jacobian ( ) from the previous question. In addition, Use
the function twolink_plot ( ) from Question provided 2.1 to plot the eight configurations
together with their corresponding tangents. Make comments on anything interesting that
you might notice from the results.
Question report 6.3. In the following question we analyze the correspondence between
the torus and the two-link manipulator from Problem 2.
torus_twolink_plotJacobian ( )
Description: For each one of the curves used in Question provided 5.2, do the following:
• Use line_linspace ( ) to compute the array thetaPoints for the curve;
• For each one of the configurations given by the columns of thetaPoints :
The function should produce a total of four windows (or, alternatively, a single window
with four subplots), each window (or subplot) showing all the configurations of the
manipulator superimposed on each other. You can insert a pause command in the
loop for drawing the manipulator, in order to obtain a “movie-like” presentation of the
motion.
13
Requirements:
optional For each window (or subplot), use the color of the corresponding curve as
used in Question provided 5.2.
Question report 6.4 (0.5 points). Comment on the relation between the results of
Questions report 6.3 and provided 5.2. Instead of focusing of the individual curves, comment
on how the two sets of results can be conceptually linked together.
Problem 7: Homework feedback
Question report 7.1. Indicate an estimate of the number of hours you spent on this
homework (possibly broken down by problem). Explain what you found hard/easy about
this homework, and include any suggestion you might have for improving it.
14
Hint for question report 1.1: Consider what happens
when you
apply each rotation to
1 0 0
the standard basis, i.e., to the vectors ex = 0, ey = 1, ez = 0.
0 0 1
Hint for question report 1.1: Remember that a rotation, when applied to the vector v ∈ R3
representing its own rotation axis, it does not change it (i.e., Rv = v).
Hint for question optional 4.1: Think about the properties that the sets Ui must have.
Hint for question report 5.1: The answer to this question can be seen as a generalization
of Question optional 4.1 from the 1-D to 2-D case.
Hint for question report 6.1: To derive the map, imagine that θ is a function of time,
i.e. θ(t), and then compute the time derivative of B peff . The simplest way to derive this
expression is by using the derivative of rotation matrices seen in class.
15