MCEN90028 Robotics Systems
Assignment 4 (34 marks)
Due date: Friday 29 May 2020 5pm (Week 11)
5 June 2020 5pm (Week 12)
1 General Instructions
1. This is an individual assignment.
2. You are expected to construct you own Matlab codes in this assignment and
not call Peter Corke’s toolbox anywhere in your codes.
3. Please read the submission instructions carefully before submitting your as-
signment.
2 Assignment Description and Tasks
Task 1. Construction of a Robot Dynamic Simulator (8
marks)
In the first task, construct a Matlab code that represents a 2 link RR (revolute-
revolute) robot as shown in Figure 1.
Kinematically,
• q1 = 0 when link 1 is aligned with the inertial X axis.
• q2 = 0 when the robot is straight.
• the axes of rotation for both joints (i.e. zˆ1 and zˆ2 are defined to point out of
the page.
• q1 and q2 are not bounded by any joint limits in Tasks 1 and 2. Joint limits
are introduced in Task 3 onwards.
The robot has link lengths of L1, L2 as indicated, with masses m1,m2 as shown and
angular moments of inertia:
I1 =
Ixx1 0 00 Iyy1 0
0 0 Izz1
1
I2 =
Ixx2 0 00 Iyy2 0
0 0 Izz2
about their centres of mass.
The values for the dynamics parameters are:
• m1 = 2kg, m2 = 1kg.
• Izz1 = 0.5kgm2, Izz2 = 0.3kgm2. Ixx1 = Ixx2 = Iyy1 = Iyy2 = 0.
• L1 = 1m, L2 = 0.6m, rC1 = 0.5m, rC2 = 0.3m
• gravity acts in the negative Y direction.
Figure 1: Planar Robot to be simulated. Note that gravity is acting in the negative
Y direction.
Download the Matlab script files provided for the Assignment:
• Example RP robot.m
• runrobot.m
2
Run the main script file Example RP robot.m. It will call runrobot.m. The runrobot.m
file contains the robot dynamics (the equation of motion). It takes the input of the
current robot state y = [q, q˙]T and outputs y˙ = [q˙, q¨]T . This gradient is used by the
numerical integrator ode45 in the main file, that takes the joint torque and the robot
state, performs the numerical integration to produce the motion of the robot [q, q˙]T
at the next time instance.
The Example RP robot.m will produce a plot of the movement of the robot vs time.
Note that the duration of runtime tf has been set to 5s in the example and the
sampling period dt has been set to 0.1s.
For Task 1, perform the necessary calculations to obtain the equation of motion for
the robot described in Figure 1, using the provided sample code Example RP robot.m
and runrobot.m as a starting point.
For the report, for Task 1:
1. produce the derivation of the equation of motion of the robot in this assignment
(from DH frame assignments, DH parameter, Jacobian matrices to the centres of
mass, and finally the matrices A, b, G). (This essentially reviews the kinematics
content for the entire subject to date.)
2. produce the plots q1 vs t and q2 vs t for tf = 5s, from the initial displacement
of q1 = q2 = 0 at rest, with sampling period dt = 0.01s and joint torque
τ1 = τ2 = 0 throughout. Set friction coefficient to friction coeff = 0.1. Please
take note that the friction coefficient in the example has been set to zero. (Go
ahead and play around with different settings to familiarise yourself with the
code and the robotic system, but please submit only the plot requested to
assist with marking).
3. produce the plots q1 vs t and q2 vs t for tf = 5s, exactly the same as the
previous point, but instead of setting τ1 = τ2 = 0, get the robot to compensate
for the gravity forces acting on the robot. Explain how this is achieved in
your report. Your result should show that the robot is balanced perfectly
against gravity and therefore is not moving. Go ahead and play with the
resulting code, for example, intentionally mis-estimate the mass, for example,
underestimate the masses and watch as the robot floats like a balloon. Discuss
the performance of the proposed method based on your observation.
For your own enjoyment, produce an XY robot animation plot, i.e. the plot of link
1 and link 2 (and the end-effector) of the robot movement in space (like in Figure
1) using Matlab plot function, such that way you can watch the movement of the
robot that you produce visually. This animation can be produced by combining the
plot function and the pause function. Dictate the values of the axes so Matlab
does not adaptively re-adjust the axes.
For example:
3
1 for t=0:dt:tf
2 plot(.......)
3 axis([-5 5 -5 5]) %this sets the axes to avoid re-sizing of ...
the view of the plot
4 pause(.1) %this sets the display speed to 0.1 to correspond ...
to real time dt
5 end
Listing 1: Example of avoiding re-sizing in Matlab plots
For the Matlab files to be submitted for Task 1:
• Name your main file as Task1 main.m.
• Name your robot dynamics file as Task1 dynamics.m.
• Label the figures produced in the code according to the subtasks.
• Use Code Sections in Matlab to structure the subtasks.
Task 2. PID Controller on robot in joint space (6 marks)
After Task 1, the robot, represented by the ode45 function, takes the input of joint
torque [τ1, τ2]
T . The robot is therefore in “torque-controlled mode”. In Task 2, we
wish to send joint velocity commands to the robot. This means, in the main file, we
would send a joint velocity command at each sampling instance to a PID loop, which
implements a closed loop control at sampling period dtPIDloop = 0.001s to regulate
the torque sent to the robot joints so that the robot joint velocity would track the
joint velocity command given. Note that dtPIDloop is different from dt, which is the
main robot control loop. Tune your PID controller to your best efforts – feel free to
go back to earlier subjects (such as ELEN90055 Control Systems) if you forgot how
to do so. A keyword reminder of Zielger-Nichols may help job your memory, though
there are many ways. Your tuning does not need to be perfect or optimised, as long
as it performs reasonably well. A rough guide would be a settling time for 5%− 10%
tolerance band (aim for 5%) of ≤ 0.01s for a δq˙ = 1°/s commanded step velocity for
individual joints.
The outcome of Task 2 is equivalent in practice to having servo motors for your
robot, which accept velocity command.
For the report, for Task 2:
1. produce the plots q1 vs t, q2 vs t, q˙1 vs t, and q˙2 vs t; for t = 0 to tf = 5s, from
the initial displacement of q1 = 30° and q2 = 75° at rest at t = 0, to arrive at
the final joint displacement of q1 = −15° and q2 = 45°, with q˙1(tf ) = q˙2(tf ) = 0
at tf = 5s. Reference trajectory is to be cubic polynomial of time in joint space.
4
Sampling period dt = 0.01s (note again, dt is the sampling period of the main
control loop, not the PID loop). Set friction coefficient to friction coeff = 0.1.
2. produce the plots of the corresponding joint torque τ1 vs t and τ2 vs t (corre-
sponding to the resulting robot motion in the first point of Task 2).
3. produce the XY plot of the robot (like in Figure 1) which consist of link 1 and
link 2 (and the end-effector, e.g. marked with a circle of triangle), every 0.5s.
(meaning at t = [0, 0.5, 1, ..., 5]s) for the resulting robot motion in Task 2.
For the Matlab files to be submitted for Task 2:
• Name your main file as Task2 main.m.
• Name your robot dynamics file as Task2 dynamics.m.
• Label the figures produced in the code according to the subtasks.
• Use Code Sections in Matlab to structure the subtasks.
Hint
Added on 20 May 2020.
During the workshop today, it appeared that a lot of students were not able
to get past Task 2, which was to construct a PID closed loop controller to
allow the robot to receive a joint velocity command q˙C instead of the torque
command. This is equivalent (in real life) to having a velocity servo drive for
your motors, which accepts velocity command from your main robot loop.
This velocity control loop is to run 10 times faster than the main robot
control loop. This essentially allows the joint velocity controller 10 sampling
instances to produce the velocity step response on the robot joints for every
joint velocity command issued by the main loop.
How is it done?
This is a closed-loop system with q˙C as the reference input, a PID as the
controller (that you are meant to design and tune), the robot as the plant (the
robot as a plant takes in joint torque τ as the input and robot joint motion
q and q˙C as outputs) and the output q˙ is used as a feedback to this velocity
control loop. Therefore:
evc = q˙C − q˙;
τ = Kpe+Kde˙+KI
∫ t
0
edτ ;
(1)
where vc denotes “velocity control”, and Kp, Kd, KI are diagonal matrices
containing the gains assigned to each joint, of size n × n, where n is the
number of joints in the robot.
Hope that helps. Have fun!
5
Task 3. Joint space control of the velocity commanded robot
(8 marks)
Continuing from where we left off in Task 2, this time, we are going to get the
end-effector of the robot to move in “task space”, while still utilising joint space
control. That is, you are to generate a reference trajectory (using cubic polynomial)
to describe the reference trajectory in (xref , yref ) for the end-effector, from an initial
coordinate (xi, yi) to the final coordinate (xf , yf ) in a straight line. The reference tra-
jectory is converted (using inverse kinematics) within at each robot sampling instance
dt into a reference joint space trajectory, that is then used to command the veloc-
ity commanded robot (i.e. as the input to the velocity PID control of the robot joints).
NOTE: In Task 3, construct a joint limit of −60° ≤ q1 ≤ 60° and −90° ≤ q2 ≤ 90°
in your code.
Two reference trajectories for the end-effector are to be generated and tracked by
the robot:
• Trajectory A: initial end-effector position: (xi, yi) = (0.71, 1.08)m at (x˙i, y˙i) =
(0, 0)m/s, final end-effector position: (xf , yf) = (1.485, 0.041)m and velocity
at the final position (x˙f , y˙f ) = (0, 0)m/s. Trajectory duration: tf = 5s.
• Trajectory B: initial end-effector position: (xi, yi) = (0.71, 1.08)m at rest
and comes to a complete stop at final end-effector position: (xf , yf) =
(1.36,−0.59)m. Trajectory duration: tf = 5s.
For the report, for Task 3:
1. produce the plots of the end-effector position x vs t and y vs t for Trajectory
A. Superimpose the reference trajectory xref and yref on the plot.
2. produce the XY plot of the robot (like in Figure 1) which consist of link 1 and
link 2 (and the end-effector, e.g. marked with a circle or triangle), every 0.5s,
(meaning at t = [0, 0.5, 1, ..., 5]s) for the resulting robot motion in Trajectory
A. Comment on the difference between the resulting motion and that produced
in Task 2.
3. produce the XY plot of the robot (like in Figure 1) which consist of link 1 and
link 2 (and the end-effector, e.g. marked with a circle or triangle), every 0.5s,
(meaning at t = [0, 0.5, 1, ..., 5]s) for the resulting robot motion in Trajectory
B.
Note:
• Trajectory B is designed so that it passes outside the reachable workspace
dictated by the joint limits of the robot.
6
• Superimpose the rough plot of the reachable workspace to aid your think-
ing.
• Discuss your results and observations.
For the Matlab files to be submitted for Task 3:
• Name your main file as Task3 main.m.
• Name your robot dynamics file as Task3 dynamics.m.
• Label the figures produced in the code according to the subtasks.
• Use Code Sections in Matlab to structure the subtasks.
Task 4. Task space control of the velocity commanded robot
(6 marks)
In Task 4, we would like to implement Task Space control, which means: our error
term in the main loop ought to be in the task space. Using end-effector Trajectory
A in Task 3, construct the error signal ex = xref − xr and ey = yref − yr at every
sampling instace dt, where xref and yref are the reference position of the end-effector
and xr and yr are the actual end-effector of the robot, obtained by performing
forward kinematics of the output of the robot simulator (q, q˙). Generate task space
velocity command (x˙C , y˙C) through a P-controller of the error (or PI controller – state
what you chose to implement in your report). Use the inverse of the Jacobian ma-
trix to obtain the joint velocity command to be sent to the velocity commanded robot.
In Task 4, the joint limit of −60° ≤ q1 ≤ 60° and −90° ≤ q1 ≤ 90° introduced in
Task 3 is retained.
For the report, for Task 4:
1. produce the plots of the end-effector position x vs t and y vs t for Trajectory
A. Superimpose the reference trajectory xref and yref on the plot.
2. produce the XY plot of the robot (like in Figure 1) which consist of link 1 and
link 2 (and the end-effector, e.g. marked with a circle of triangle), every 0.5s,
(meaning at t = [0, 0.5, 1, ..., 5]s) for the resulting robot motion in Trajectory
A. Comment on any observation you had between the resulting motion and
that produced in Task 3.
For the Matlab files to be submitted for Task 4:
• Name your main file as Task4 main.m.
• Name your robot dynamics file as Task4 dynamics.m.
7
• Label the figures produced in the code according to the subtasks.
• Use Code Sections in Matlab to structure the subtasks.
Task 5. Task space control of the velocity commanded robot
with reactive obstacle avoidance (6 marks)
Task 5 continues from Task 4, where the robot is to execute Trajectory A with Task
Space control of the velocity commanded robot. However, a circular obstacle appears
in the path. The robot does not know where it is beforehand (it is not a known
obstacle from the start as the one you encountered in Assignment 3). However,
you have been given a wonderful sensor, mounted at the end-effector. This sensor
provides one set of measurement at each time instance. It can be called at each time
instance by:
1 [d, theta] = sensor t5(obs init, xr, yr)
Inputs:
obs init A variable to handle the generation of a new obstacle. Set it to 1 if a
new obstacle needs to be generated, 0 if not initialising the obstacle. An
example code is provided below shown in Listing 2.
xr, yr The robots end-effector coordinate in the inertial frame at the time the
function is called.
Outputs:
d The distance (in metre) of the end-effector from the nearest detected point
(of the obstacle) (therefore it is a scalar value, not a vector).
theta The angle (in radians) to the same point from the end-effector, measure
from the inertial positive X axis.
1 newObs=1; % create a new obstacle in the environment.
2 for ...
3 ...
4 [d, th]=sensor t5(newObs,x,y);
5 newobs=0; % avoid changing the obstacle during the motion.
6 ...
7 end
Listing 2: Example of initialising the obstacle
The function sensor t5.p is provided for this assignment. The sensor output variables
d and θ are shown in Figure 2. It is assumed that there is only ONE (circular)
obstacle in the scenario.
The sensor has a maximum range of 0.35m, beyond which it will not detect the
obstacle and will output the value d = 1000 and theta = 1000. This means that you
will not see the obstacle until it is within 0.35m of the end-effector. The strategy you
8
Figure 2: The sensor outputs: d and θ shown in this diagram. The obstacle (Obs)
is assumed to be always a circle. Its location is recorded inside the function of the
sensor.
need to come up with is therefore described as being “reactive”, or in the literature,
it is often called “online obstacle avoidance”.
You therefore initialise Task 5 driving the robot end-effector to track Trajectory A.
In the effort to avoid the obstacle, you are to get your robot to track the desired
trajectory as much as possible and only deviate from it where it is necessary to avoid
the obstacle. In the main loop, you will call the sensor t5(obs init, xr, yr) function at
each sampling instance and decide what to do. This has not been explicitly covered
in our lectures, but it is part of the open-ended brain teaser to the class. There is
plenty in the literature in the 80s - 90s on these type of problems if you wish to read
up. Focus on the reactive strategies.
In Task 5, the joint limit of −60° ≤ q1 ≤ 60° and −90° ≤ q1 ≤ 90° introduced in
Task 3 is retained.
For the report, for Task 5:
• In less than 500 words, explain the outline of your strategy in guiding the
robot end-effector through Trajectory A while avoiding the obstacle. Please do
not use ready made functions, such as those in the Matlab Robotics Toolbox.
Include references if you use material from the literature.
IMPORTANT
9
If you utilised any methods taken from the literature (from a paper, textbook,
etc), provide a reference to the source. Add a section at the end of the re-
port titled ”references”. Learn how to cite literature source appropriately in
your own time at: https://students.unimelb.edu.au/academic-skills/explore-
our-resources/referencing
At this stage of your master degree, ignorance in how to appropriately ac-
knowledge your source is not a valid excuse if you were found to plagiarise
external material. If you have not already done so, please educate yourself as
to the boundary of what constitute acceptable and non-acceptable practice.
Those boundaries include the cases of how to appropriately report material
that you may share with your peers in class. Know the difference between
group discussion and helping each other to learn material and copying someone
else’s work.
• As part of your explanation above, produce the necessary plots to demonstrate
your result implementing a Task Space control to the velocity commanded robot
to traverse Trajectory A, with the adjustments made to avoid the obstacle.
Include in the results relevant variables over t such as: the reference trajectory,
the actual trajectory traversed by the robot to avoid the obstacle, sensor
readings during the operation, and other variables relevant to demonstrating
the effectiveness or in explaining the inner working of the strategy of your
choice. Refer to these plot when explaining your answer and explain the plots.
3 Submission
In your report, detail the steps and understanding in producing your answers. Do not
include the Matlab codes in the body of the report. Include only the pseudo-code
if necessary and explain the logic.
You need to submit one report per person plus the necessary Matlab codes.
The report should be in an appropriate engineering report format and submitted as
a PDF. The report should be no more than 30 pages (everything included) with
12pt font size.
Compress your report and all your relevant Matlab files as a .zip file with folder
name “Assignment4 [#]”, where you replace [#] with your student ID (eg. As-
signment4 123456) and submit to LMS.
Note: only the last submission will be assessed.
Submission checklist:
Report is no more than 30 pages
Report has 12pt font size
Report saved as PDF
Titles of files are in the right format
Compressed all files as a single .zip file
4 Academic Integrity
We take academic integrity seriously. Please note that while students may discuss
and help each other in the thinking process, you should work on your assignments
separately.
Details about academic integrity can be found on MCEN90028 Canvas page (under
Subject Overview) or at http://academicintegrity.unimelb.edu.au/. Please
check with the tutors or the lecturer if you are in doubt. Ignorance is not a valid
reason for academic misconducts.