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.

10

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.

11