ECE 470Introduction to Robotics

Lab ManualOnline ver 1.3

Dan BlockJonathan K. Holm

Jifei XuYinai FanHang CuiYu Chen

Weihang Liang

University of Illinois at Urbana-Champaign

UR3 Python - ROS Interface

Contents

4 Inverse Kinematics 14.1 Important . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.2 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.3 Reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.4 Tasks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

4.4.1 Solution Derivation . . . . . . . . . . . . . . . . . . . . . . 14.4.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . 5

4.5 Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54.6 Report . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84.7 Demo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84.8 Grading . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84.9 Tentative Due Dates - Fall 2020 . . . . . . . . . . . . . . . . . . . 8

4.9.1 Group A . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.9.2 Group B . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

A ROS Programming with Python 10A.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10A.2 ROS Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10A.3 Before we start.. . . . . . . . . . . . . . . . . . . . . . . . . . . . 11A.4 Create your own workspace . . . . . . . . . . . . . . . . . . . . . 13A.5 Running a Node . . . . . . . . . . . . . . . . . . . . . . . . . . . 13A.6 More Publisher and Subscriber Tutorial . . . . . . . . . . . . . . 14

B VMware Installation and Simulator Guide 15B.1 System Requirements . . . . . . . . . . . . . . . . . . . . . . . . 15B.2 VMware Installation . . . . . . . . . . . . . . . . . . . . . . . . . 16B.3 VMware Image . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16B.4 Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

i

Preface

This is a set of laboratory assignments designed to complement the introductoryrobotics lecture taught in the College of Engineering at the University of Illi-nois at Urbana-Champaign. Together, the lecture and labs introduce studentsto robot manipulators and computer vision along with the Robot OperatingSystem (ROS) and serve as the foundation for more advanced courses on robotdynamics, control and computer vision. The course is cross-listed in three de-partments (Electrical & Computer Engineering, Aerospace Engineering, andMechanical Science & Engineering) and consequently includes students from avariety of academic backgrounds.

For success in the laboratory, each student should have completed a coursein linear algebra and be comfortable with three-dimensional geometry. In addi-tion, it is imperative that all students have completed a freshman-level course incomputer programming. MODERN ROBOTICS MECHANICS, PLANNING,AND CONTROL (Kevin M. Lynch and Frank C. Park, 2017) is required for thelectures and will be used as a reference for many of the lab assignments. Wewill hereafter refer to the textbook as MR in this lab manual.

These laboratories are simultaneously challenging, stimulating, and enjoyable.It is the author’s hope that you, the reader, share a similar experience.

Enjoy the course!

LAB 4

Inverse Kinematics

4.1 Important

Read the entire lab before starting and especially the “Grading” section so youare aware of all due dates and requirements associated with the lab. Hope-fully you are reading this well before your lab section meets as given the com-pressed schedule, it is very important that you arrive at lab well prepared. Thissemester, the more you do prior to your lab session, the more you will get outof the short time you have with the TA.

4.2 Objectives

The purpose of this lab is to derive and implement a solution to the inversekinematics problem for the UR3 robot. In this lab we will:

• Derive elbow-up inverse kinematic equations for the UR3

• Write a Python function that moves the UR3 to a point in space specifiedby the user.

4.3 Reference

Chapter 6 of Modern Robotics provides multiple examples of inverse kinematicssolutions.

4.4 Tasks

4.4.1 Solution Derivation

Make sure to read through this entire lab before you start derivingyour solution. There are some needed details not covered in this

1

4.4. TASKS

section.Given a desired end-effector position in space (xgrip, ygrip, zgrip) and orientation{θyaw, θpitch(fixed), θroll(fixed)}, write six mathematical expressions that yieldvalues for each of the joint angles. For the UR3 robot, there are many solutionsto the inverse kinematics problem. We will implement only one of the elbow-upsolutions.

• In the inverse kinematics problems you have examined in class (for 6 DOFarms with spherical wrists), usually the first step is to solve for the coor-dinates of the wrist center. The UR3 does not technically have a sphericalwrist center but we will define the wrist center as zcen which equals thesame desired z value of the suction cup and xcen, ycen are the coordinatesof θ6’s z axis. In addition, to make the derivation manageable, add thatθ5 will always be −90◦ and θ4 is set such that link 7 and link 9 are alwaysparallel to the world x,y plane.

• Solve the inverse kinematics problem in the following order:

1. xcen, ycen, zcen, given yaw desired in the world frame and the desiredx,y,z of the suction cup. The suction cup aluminum plate (link 9)has a length of 0.0535 meters from the center line of the suction cupto the center line of joint 6. Remember that this aluminum plateshould always be parallel to the world’s x,y plane. See Figure 4.2.

2. θ1, by drawing a top down picture of the UR3, Figure 4.1, and usingxcen, ycen, zcen that you just calculated.

3. θ6, which is a function of θ1 and yaw desired. Remember that whenθ6 is equal to zero the suction cup aluminum plate is parallel to link4 and link 6.

4. x3end, y3end, z3end is a point off of the UR3 but lies along the link 6axis, Figure 4.1. For example if θ1 = 0◦ then y3end = 0. If θ1 = 90◦

then x3end = 0. First use the top down view of the UR3 to findx3end, y3end. One way is to choose an appropriate coordinate frame atxcen, ycen and find the translation matrix that rotates and translatesthat coordinate frame to the base frame. Then find the vector inthe coordinate frame you chose at xcen, ycen that points from xcen,ycen to x3end, y3end. Simply multiply this vector by your translationmatrix to find the world coordinates at x3end, y3end. For z3end createa view of the UR3, Figure 4.2, that is a projection of the robot onto aplane perpendicular to the x,y world frame and rotated by θ1 aboutthe base frame. Call this the side view. Looking at this side viewyou will see that z3end is zcen offset by a constant.

5. θ2, θ3 and θ4, by using the same side view drawing just drawn aboveto find z3end, Figure 4.2. Now that x3end, y3end, z3end have beenfound use sine, cosine and the cosine rule to solve for partial anglesthat make up θ2, θ3 and θ4. Hint: In this side view, a parallel to

2

4.4. TASKS

Figure 4.1: Top View Stick Pictorial of UR3. Note that the coordinate framesare in the same direction as the World Frame but not at the World frame’sorigin. One origin is along the center of joint 1 and the second is along thecenter of joint 6.

3

4.4. TASKS

Figure 4.2: Side View Stick Pictorial of UR3.

4

4.5. PROCEDURE

the base construction line through joint 2 and a parallel to the baseconstruction line through joint 4 are helpful in finding the neededpartial angles.

4.4.2 Implementation

Implement the inverse kinematics solution by writing a Python function to re-ceive world frame coordinates (xWgrip, yWgrip, zWgrip, yawWgrip), compute thedesired joint variables {θ1, θ2, θ3, θ4, θ5, θ6}, and command the UR3 to move tothat pose using functions written in Lab4.

4.5 Procedure

• Download lab4Py.tar.gz from the course website and extract it in your“src” directory. You will notice that there are three .py files. lab4 exec.py,lab4 func.py and lab4 header.py. The lab4 func.py file again will becompiled into a library so that future labs can easily call the inverse kine-matic function. Like Lab 3, most of the needed code is given to youin lab4 exec.py. Your main job will be to add all the inverse kinematicequations to lab4 func.py. Please refer to the intermediate steps below toperform the inverse kinematic calculations. If you look at lab4 header.pyit includes lab4 header.py. This allows you to call the functions you cre-ated in lab4 func.py.

• Once your code is finished, run it using “rosrun lab4pkg py lab4 exec.py[x] [y] [z] [yaw(degrees)]” - e.g. “rosrun lab4pkg py lab4 exec.py0.1 0.1 0.15 90”. Remember that in another command prompt youshould have first run roscore and drivers using “roslaunch ur3 driverur3 gazebo.launch” or “roslaunch ur3 driver ur3 driver.launch”depending if you are using the simulator or real robot.

• For in-person students, you should measure the x,y,z position of the end-effector using the provided ruler and square. For students using the sim-ulation, it is not possible to measure this way, so we must use anothermethod. A simple way is to use some of the ROS commands we learnedbefore: “rostopic echo /gripper/position -n 1”. These values are be-ing calculated differently and so there will be small differences betweenthis value and your calculations.

• You should verify that your code works by selecting a variety of poses thatwill test the full range of motion. Your TA will not be providing you testpoints.

• In your code (This is repeating the derivation steps above):

1. Establish the world coordinate frame (frame w) centered at the cor-ner of the UR3’s base shown in Figure 4.3. The xw and yw plane

5

4.5. PROCEDURE

should correspond to the surface of the table, with the xw axis par-allel to the sides of the table and the yw axis parallel to the frontand back edges of the table. Axis zw should be normal to the tablesurface, with up being the positive zw direction and the surface ofthe table corresponding to zw = 0.We will solve the inverse kinematics problem in the base frame (frame0), so we will immediately convert the coordinates entered by theuser to base frame coordinates. Write three equations relating co-ordinates (xWgrip, yWgrip, zWgrip) in the world frame to coordinates(xgrip, ygrip, zgrip) in the base frame of the UR3.

xgrip(xWgrip, yWgrip, zWgrip) =

ygrip(xWgrip, yWgrip, zWgrip) =

zgrip(xWgrip, yWgrip, zWgrip) =

Figure 4.3: Correct location and orientation of the world frame.

2. Given the desired position of the gripper (xgrip, ygrip, zgrip) (in the

6

4.5. PROCEDURE

base frame) and the yaw angle, find wrist’s center point (xcen, ycen, zcen).

xcen(xgrip, ygrip, zgrip, yaw) =

ycen(xgrip, ygrip, zgrip, yaw) =

zcen(xgrip, ygrip, zgrip, yaw) =

3. Given the wrist’s center point (xcen, ycen, zcen), write an expressionfor the waist angle θ1. Make sure to use the atan2() function insteadof atan() because atan2() takes care of the four quadrants the x,ycoordinates could be in.

θ1(xcen, ycen, zcen) = (4.1)

4. Solve for the value of θ6, given yaw and θ1.

θ6(θ1, yaw) = (4.2)

5. Find the projected end point (x3end, y3end, z3end) using (xcen, ycen, zcen)and θ1.

x3end(xcen, ycen, zcen, θ1) =

y3end(xcen, ycen, zcen, θ1) =

z3end(xcen, ycen, zcen, θ1) =

6. Write expressions for θ2, θ3 and θ4 in terms of the end point. Youprobably will want to define some intermediate variables to help youwith these calculations.

θ2(x3end, y3end, z3end) =

θ3(x3end, y3end, z3end) =

θ4(x3end, y3end, z3end) =

7. Now that your code solves for all the joint variables (rememberthat θ5 is always −90◦) send these six values to the Lab 3 func-tion lab fk(). You will need to copy you Lab 3 solution into thesefunctions. Do this simply to check that your inverse kinematic cal-culations are correct. Double check that the x,y,z point that youasked the robot to go to is the same value displayed by the forwardkinematic equations.

7

4.6. REPORT

4.6 Report

You should submit a lab report using the guidelines given in the ECE 470: Howto Write a Lab Report document. Please be aware of the following:

• Lab reports are due with your demo for lab 4!

• Lab reports will be submitted online at GradeScope.

Your lab report should include the following:

• A clearly written derivation of the inverse kinematics solution for each jointvariable (θ1, θ2, θ3, θ4, θ5, θ6). You must include figures in your derivation.Diagrams should be your own creation and clear and easily read. Do notuse hand drawn figures or annotations.

• For each test point include:

– The given {(xwgrip

, ywgrip, zwgrip

), θyaw}

– The measured position

– The scalar error

• Include a brief discussion of sources of error.

As appendices to your report, include the following:

• Your lab4 func.py code and lab4 exec.py if it was edited.

4.7 Demo

Your TA will require you to run your program twice, each time with a differentset of desired position and orientation. Your program should reach the desiredposition and orientation with almost no error. You will be required to be ableto demo on the simulator, even if you choose to demo on the real robot.

4.8 Grading

• 80 points, successful demonstration.

• 20 points, individual report.

4.9 Tentative Due Dates - Fall 2020

The exact date and time will depend on your lab section and can be found onGradeScope.

8

4.9. TENTATIVE DUE DATES - FALL 2020

4.9.1 Group A

• Demonstration - Week of November 16

• Report - Week of November 16

4.9.2 Group B

• Demonstration - Week of November 30

• Report - Week of November 30

9

Appendix A

ROS Programming withPython

A.1 Overview

ROS is an open-source, meta-operating system for your robot. It provides theservices you would expect from an operating system, including hardware ab-straction, low-level device control, implementation of commonly-used function-ality, message-passing between processes, and package management. It alsoprovides tools and libraries for obtaining, building, writing, and running codeacross multiple computers.

• The ROS runtime “graph” is a peer-to-peer network of processes (poten-tially distributed across machines) that are loosely coupled using the ROScommunication infrastructure. ROS implements several different stylesof communication, including synchronous RPC-style communication overservices, asynchronous streaming of data over topics, and storage of dataon a Parameter Server.

• For more details about ROS: http://wiki.ros.org/

• How to install on your own Ubuntu: http://wiki.ros.org/ROS/Insta

llation

• For detailed tutorials: http://wiki.ros.org/ROS/Tutorials

A.2 ROS Concepts

The basic concepts of ROS are nodes, Master, messages, topics, ParameterServer, services, and bags. However, in this course, we will only be encountering

10

A.3. BEFORE WE START..

the first four.

• Nodes “programs” or ”processes” in ROS that perform computation. Forexample, one node controls a laser range-finder, one node controls he wheelmotors, one node performs localization ...

• Master Enable nodes to locate one another, provides parameter server,tracks publishers and subscribers to topics, services. In order to startROS, open a terminal and type:

$ r o s c o r e

roscore can also be started automatically when using roslaunch in termi-nal, for example:

$ ros launch <package name> <launch f i l e name>. launch# the launch f i l e f o r a l l our l a b s :$ ros launch u r 3 d r i v e r u r 3 d r i v e r . launch

• Messages Nodes communicate with each other via messages. A messageis simply a data structure, comprising typed fields.

• Topics Each node publish/subscribe message topics via send/receive mes-sages. A node sends out a message by publishing it to a given topic. Theremay be multiple concurrent publishers and subscribers for a single topic,and a single node may publish and/or subscribe to multiple topics.In gen-eral, publishers and subscribers are not aware of each others’ existence.

Figure A.1: source: http://wiki.ros.org/ROS/Concepts

A.3 Before we start..

Here are some useful Linux/ROS commands

• The command “ls” stands for (List Directory Contents), List the contentsof the folder, be it file or folder, from which it runs.

$ l s

11

A.3. BEFORE WE START..

• The “mkdir” (Make directory) command create a new directory with namepath. However is the directory already exists, it will return an error mes-sage “cannot create folder, folder already exists”.

$ mkdir <new directory name>

• The command “pwd” (print working directory), prints the current workingdirectory with full path name from terminal

$ pwd

• The frequently used “cd” command stands for change directory.

$ cd /home/ user /Desktop

return to previous directory

$ cd . .

Change to home directory

$ cd ˜

• The hot key “ctrl+c” in command line terminates current running exe-cutable. If “ctrl+c” does not work, closing your terminal as that will alsoend the running Python program. DO NOT USE “ctrl+z” as it canleave some unknown applications running in the background.

• If you want to know the location of any specific ROS package/executablefrom in your system, you can use “rospack” find “package name” com-mand. For example, if you would like to find ‘lab2pkg py’ package, youcan type in your console

$ rospack f i n d lab2pkg py

• To move directly to the directory of a ROS package, use roscd. For exam-ple, go to lab2pkg py package directory

$ roscd lab2pkg py

• Display Message data structure definitions with rosmsg

$ rosmsg show <message type> #Disp lay the f i e l d s in the msg

• rostopic, A tool for displaying debug information about ROS topics, in-cluding publishers, subscribers, publishing rate, and messages.

$ r o s t o p i c echo / topic name #Print messages to screen$ r o s t o p i c l i s t #Li s t a l l the t o p i c s a v a i l a b l e$ r o s t o p i c pub <top ic−name> <top ic−type> [ data . . . ]#Pub l i sh data to t o p i c

12

A.4. CREATE YOUR OWN WORKSPACE

A.4 Create your own workspace

Since other groups will be working on your same computer, you should backupyour code to a USB drive or cloud drive everytime you come to lab. This wayif your code is tampered with (probably by accident) you will have a backup.

• Log on to the computer as ‘ur3’ with the password ‘ur3’. If you log on as‘guest’, you will not be able to use ROS.

• First create a folder in the home directory, mkdir catkin (yourNETID). Itis not required to have ”catkin” in the folder name but it is recommended.

$ mkdir −p c a t k i n (yourNETID)/ s r c$ cd c a t k i n (yourNETID)/ s r c$ c a t k i n i n i t w o r k s p a c e

• Even though the workspace is empty (there are no packages in the ’src’folder, just a single CMakeLists.txt link) you can still ”build” the workspace.Just for practice, build the workspace.

$ cd ˜/ c a t k i n (yourNETID)/$ catkin make

• VERY IMPORTANT: Remember to ALWAYS source when you opena new command prompt, so you can utilize the full convenience of Tabcompletion in ROS. Under workspace root directory:

$ cd c a t k i n (yourNETID)$ source deve l / setup . bash

A.5 Running a Node

• Once you have your catkin folder initialized, add the UR3 driver and labstarter files. The compressed file lab2andDanDriver.tar.gz, found at theclass website contains the driver code you will need for all the ECE 470 labsalong with the starter code for LAB 2. Future lab compressed files will onlycontain the new starter code for that lab. Copy lab2andDriverPy.tar.gzto your catkin directories “src” directory. Change directory to your “src”folder and uncompress by typing “tar -xvf lab2andDriver.tar.gz”. Youcan also do this via the GUI by double clicking on the compressed file anddragging the folders into the new location.

“cd ..” back to your catkin (yourNETID) folder and build the code with“catkin make”

• After compilation is complete, we can start running our own nodes. Forexample our lab2node node. However, before running any nodes, we musthave roscore running. This is taken care of by running a launch file.

13

A.6. MORE PUBLISHER AND SUBSCRIBER TUTORIAL

$ ros launch u r 3 d r i v e r ur3 gazebo . launch

is used for the simulator.

$ ros launch u r 3 d r i v e r u r 3 d r i v e r . launch

is used on the real robot.This command runs both roscore and the UR3 driver that acts as a sub-scriber waiting for a command message that controls the UR3’s motors.

• Open a new command prompt with “ctrl+shift+N”, cd to your rootworkspace directory, and source it “source devel/setup.bash”.

• We may also need to make lab2 exec.py executable.

$ chmod +x lab2 exec . py

• Run your node with the command rosrun in the new command prompt.Example of running lab2node node in lab2pkg package:

$ rosrun lab2pkg py lab2 exec . py −−s imu la tor True

Note that the “--simulator True” tells it you are running on the simulator, while“--simulator False” tells the program you are running on real hardware. Thisflag is currently only applicable to Lab 2.

A.6 More Publisher and Subscriber Tutorial

Please refer to the webpage: http://wiki.ros.org/ROS/Tutorials/Writing

PublisherSubscriber(c%2B%2B)

14

Appendix B

VMware Installation andSimulator Guide

B.1 System Requirements

• CPU: i3 desktop or equivalent/ i5 laptop or equivalent

• GPU: Integrated Graphics Unit/ Dedicated GPU

• Memory: 6G or above

• Disk Space: 20G or above

Reference Laptop:

• CPU: i5 8th Gen

• GPU: Intel UHD Graphics 620

• Memory: 8G

• Disk Space: 20G

Note: Simulators are demanding software to run. If you have problems runningthe virtual machine and/or the simulator, please let the TAs know about yoursituation. If you believe that it is due to hardware limitations, please checkout the technology loan program https://answers.uillinois.edu/illinois

/99680.

15

B.2. VMWARE INSTALLATION

B.2 VMware Installation

The UIUC WebStore offers free VMware products under VMware AcademicProgram. https://e5.onthehub.com/WebStore/Welcome.aspx?ws=6c313875-25d6-e311-93fd-b8ca3a5db7a3

Follow the instructions and download VMware Workstation 15.x Player on Win-dows/Linux or VMware Fusion 11.x Pro for Mac. Note that the guide is onlywritten for VMware Workstation 15.x but not on VMware Fusion 11.x Pro.Please let the instructors know if you experience issues with Mac.

B.3 VMware Image

VMware uses VM image files to run the operating system as well as store files.Please unzip the ECE470VM.zip file and extract it to a working directory. InVMware, click “Open a Virtual Machine” and navigate to your working direc-tory.

Figure B.1: Open a Virtual Machine

Double click the “ECE470VM.vmx” and you will see the “ECE470VM” optionon the left side.

16

B.3. VMWARE IMAGE

Figure B.2: Open vmx file

For memory, you need at least 4 GB to run the simulation. Because the hostsystem also requires memory to run, the requirement for memory is at least 6GB. If your computer has less than 6 GB memory, you need to contact the TAsand use the technology loan program.

Now you are ready to boot the system. Click “Play virtual machine” to bootthe system. In the new box that asks about whether you moved or copied thevirtual machine, choose ”I Copied It”.

Figure B.3: Click ”I Copied It”

17

B.4. TEST

B.4 Test

In the lab, we are mainly going to use two software. To test their functionalities,open a terminal by the shortcut ”Ctrl + Alt + T” or other way you prefer. Inthe terminal window, type in the following command:

$ r o s c o r e

The terminal should print out something similar to this:

Figure B.4: Result of Running ”roscore”

Now you’ve tested basic functionalities of ROS, close the ROS core by ”Ctrl +C”. After the process ends, type in the following command:

$ gazebo

A new window should pop up and display something like this. If it somehow fails,try running it again. Let the TA know if Gazebo fails consistently. Similarly,after testing, close Gazebo by ”Ctrl + C” in the terminal (not the new simulatorwindow).

18

B.4. TEST

Figure B.5: Result of Running ”gazebo”

19