- May 24, 2020

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