Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revisionNext revisionBoth sides next revision | ||
tutorials:demo:fetch_and_place [2019/08/16 11:57] – Edited the first part and simplified it. amar | tutorials:demo:fetch_and_place [2019/10/07 18:01] – gkazhoya | ||
---|---|---|---|
Line 1: | Line 1: | ||
====== Zero prerequisites demo tutorial: Simple fetch and place ====== | ====== Zero prerequisites demo tutorial: Simple fetch and place ====== | ||
- | This tutorial is from the "Demo Tutorials" | + | This tutorial is from the "Demo Tutorials" |
+ | |||
+ | |||
+ | |||
===== Setting Up ===== | ===== Setting Up ===== | ||
- | You will need a virtual machine to run this tutorial. We use VirtualBox to run the VM that contains all the preinstalled | + | You will need a virtual machine to run this tutorial. We use VirtualBox |
+ | This tutorial comes with a virtual machine (VM) image, which contains all the prerequisites required to run the system already | ||
+ | You just need to install VirtualBox, download | ||
+ | |||
+ | ==== Technical Requirements ==== | ||
+ | |||
+ | The VM image with VirtualBox | ||
+ | |||
+ | The machine on which you will be running the system needs a graphics card, it can be a built in laptop GPU as well. | ||
+ | |||
+ | We recommend to run the VM on a machine with 8 GB of RAM or more. | ||
+ | (The VM itself needs 4 GB of RAM, so the host machine should have at least 5 GB but 8 GB or more would be optimal.) | ||
+ | |||
+ | |||
+ | ==== Install ==== | ||
- | ====Install==== | + | First, download and install VirtualBox on your computer. |
- | Here are some hints on how to install VirtualBox | + | Here are some hints on how to do that on Ubuntu, Arch, Windows and MacOS |
- | **Ubuntu | + | == Ubuntu |
- | You can install VirtualBox via the Ubuntu Software | + | You can install VirtualBox via the Ubuntu Software |
<code bash> | <code bash> | ||
- | sudo add-apt-repository multiverse | ||
sudo apt update | sudo apt update | ||
sudo apt install virtualbox | sudo apt install virtualbox | ||
</ | </ | ||
- | **Arch-Linux** | + | == Arch-Linux |
Follow [[https:// | Follow [[https:// | ||
- | **Windows** | + | == Windows |
Download [[https:// | Download [[https:// | ||
Line 29: | Line 46: | ||
The CPU needs to support virtualization to run a VM on Windows. The virtualization can be enabled in the BIOS. To get into the BIOS settings restart your machine, hit the settings button while booting (try F2, F8 and F10) and go into the CPU settings. There is something like 'Intel Virtual Technology' | The CPU needs to support virtualization to run a VM on Windows. The virtualization can be enabled in the BIOS. To get into the BIOS settings restart your machine, hit the settings button while booting (try F2, F8 and F10) and go into the CPU settings. There is something like 'Intel Virtual Technology' | ||
- | **MacOS** | + | == MacOS == |
Download [[https:// | Download [[https:// | ||
+ | Hint: If the VirtualBox installer fails with The **installation failed** where it encountered an //error that caused the installation to fail.// Go to System Preferences > Security & Privacy. Click the **‘Allow’** button at the bottom and re-run the installer. | ||
- | ====VirtualBox Setup==== | + | ==== VirtualBox Setup ==== |
- | * Get the Virtual Disk Image [[https:// | + | First of all, download |
- | * Launch VirtualBox. | + | |
- | First we want to create a new virtual machine. Click on the ' | + | Then, launch VirtualBox, which you installed in the previous step. |
- | {{ : | + | |
+ | Inside VirtualBox, you first want to create a new virtual machine. Click on the ' | ||
+ | {{ : | ||
Usually the simplified menu will open. Change to expert mode, it won't get too complicated. | Usually the simplified menu will open. Change to expert mode, it won't get too complicated. | ||
- | {{ : | + | {{ : |
In this view the VM gets | In this view the VM gets | ||
Line 49: | Line 68: | ||
* the OS as ' | * the OS as ' | ||
* the version as ' | * the version as ' | ||
- | Choose the VMs memory size (RAM), depending on your machines | + | Choose the VM' |
+ | We suggest 4096 MB (or you can also give it 8192 MB as shown in the screenshot): | ||
- | {{ : | + | {{ : |
- | The Virtual Disc Image downloaded before can now be used. Instead of creating a new one, the VM is set to 'Use an existing virtual hard disk file'. Over the button at the right the program opens a file explorer, where the cram-vm.vdi needs to be found (usually in Downloads) and selected. Finally ' | + | The Virtual Disc Image downloaded before can now be used. Instead of creating a new one, the VM is set to 'Use an existing virtual hard disk file'. Over the button at the right the program opens a file explorer, where the '' |
+ | |||
+ | === Settings (optional) === | ||
- | ===Settings (optional)=== | ||
For better performance the VM could use some of the CPU's power. Choose the newly created VM and hit ' | For better performance the VM could use some of the CPU's power. Choose the newly created VM and hit ' | ||
- | {{ : | + | {{ : |
Everything is set up. The VM can now be started. The Ubuntu 16.04 system should start and automatically log into the only account, with the username and password ' | Everything is set up. The VM can now be started. The Ubuntu 16.04 system should start and automatically log into the only account, with the username and password ' | ||
- | ===== Crash Course on Ubuntu, ROS, Emacs, Lisp and Robotics :D ===== | ||
- | ==== Launching | + | |
- | To launch | + | |
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | ===== Understanding | ||
+ | |||
+ | ==== Ubuntu Linux ==== | ||
+ | |||
+ | The operating system installed in our virtual machine is an Ubuntu Linux, which is perhaps the most mainstream Linux version. | ||
+ | |||
+ | === Launching a terminal === | ||
+ | |||
+ | A terminal is a command line of the operating system. | ||
+ | We will use terminals to start our programs. | ||
+ | |||
+ | To launch | ||
+ | Alternatively, | ||
+ | |||
+ | ==== Emacs ==== | ||
+ | |||
+ | Emacs is one of the oldest text editors and one of the two most popular text editors in the Linux world. | ||
+ | |||
+ | It is older than the Windows operating system, so its shortcuts are the old Unix shortcuts. | ||
+ | For example, instead of " | ||
+ | In a Linux terminal, " | ||
+ | |||
+ | In addition to being a text editor, Emacs has a powerful plugin engine for running programs inside it. | ||
+ | For example, with an email plugin you can read and write emails with Emacs, with a chat plugin you can talk to your friends over any common chat protocol, with a music plugin you can listen to radio through Emacs, with a C++ development plugin you can program in C++ with Emacs, etc. | ||
+ | |||
+ | We are going to use Emacs as an Interactive Development Environment (IDE) for programming our robot, so we will be using the Common Lisp programming plugin of Emacs (it is called Slime). | ||
+ | |||
+ | === Launching Emacs === | ||
+ | |||
+ | To launch Emacs with the IDE plugin, type into a terminal | ||
+ | < | ||
+ | $ roslisp_repl & | ||
+ | </ | ||
+ | |||
+ | To see useful key shortcuts for your Emacs IDE, [[http:// | ||
+ | |||
+ | |||
+ | ==== ROS ==== | ||
+ | |||
+ | ROS stands for Robot Operating System. | ||
+ | It is software that makes a robot programmer' | ||
+ | For example, it allows programs written in different programming languages to talk to each other. | ||
+ | |||
+ | |||
+ | ==== Lisp ==== | ||
+ | |||
+ | We are going to program our robot in the Lisp programming language. | ||
+ | |||
+ | This language has a convenient command line. | ||
+ | If you are curious why Lisp, [[http:// | ||
===== Simple Fetch and Place ===== | ===== Simple Fetch and Place ===== | ||
- | ==== Environment setup ==== | + | ==== Robot Environment setup ==== |
- | For this tutorial we need to set up the environment similar to how it is done for the real system: we will need an occupancy map which helps with navigation, an Inverse Kinematics solver | + | |
+ | For this tutorial, we need to set up the robot environment similar to how it is done for the real system, so we will need: | ||
+ | * a so-called //occupancy map//, which is the floor plan of the room that helps with navigation, | ||
+ | * an Inverse Kinematics solver, which deals with the mathematics related to arm movements, | ||
+ | * a 3D description | ||
+ | * as well as a 3D description of the robot itself. | ||
- | Let's load all these into memory and running | + | Let's load all these by running a script called the launch file. |
+ | Open a new terminal and run the following command: | ||
< | < | ||
$ roslaunch cram_pick_place_tutorial world.launch | $ roslaunch cram_pick_place_tutorial world.launch | ||
Line 77: | Line 160: | ||
Do not close this terminal instance throughout this tutorial. | Do not close this terminal instance throughout this tutorial. | ||
- | ==== REPL Setup ==== | + | ==== Preparing the Lisp Command Line ==== |
- | Create | + | Open a new terminal |
+ | For that type in the terminal: | ||
< | < | ||
- | $ roslisp_repl | + | $ roslisp_repl |
</ | </ | ||
- | An emacs window with the REPL will have launched. | + | An Emacs window with a Lisp command line will have launched. |
+ | At this point we can execute some simple Lisp code. | ||
+ | Try: | ||
+ | |||
+ | <code lisp> | ||
+ | CL-USER> (print "Hello World" | ||
+ | </ | ||
'' | '' | ||
+ | |||
+ | Now let's load our pick and place tutorial package. | ||
+ | Type in the following two commands into the Lisp command line: | ||
<code lisp> | <code lisp> | ||
Line 93: | Line 186: | ||
</ | </ | ||
- | Your prompt will now change to '' | + | To paste something from, e.g., a browser, into Emacs, press " |
+ | Alternatively, | ||
- | ==== Launching | + | Your prompt will now change to '' |
- | Now to launch a simulation | + | ==== Launching the simulator ==== |
+ | |||
+ | Now to launch a simulation | ||
<code lisp> | <code lisp> | ||
PP-TUT> (roslisp-utilities: | PP-TUT> (roslisp-utilities: | ||
</ | </ | ||
- | You will get a pop-up window | + | You will get a pop-up window with the environment and the robot inside. |
+ | We call this environment | ||
+ | The robot we have there is a simulation of the real-life | ||
+ | and we're going to use it to perform various tasks inside the Bullet World. | ||
+ | **Note**:// | ||
+ | {{ : | ||
- | At any point during the tutorial, if you want to clean/reset the bullet world, enter the following command into the REPL: | + | If you get an error saying "'' |
+ | Once you've done that, press " | ||
+ | |||
+ | At any point during the tutorial, if you want to clean/reset the Bullet World, enter the following command into the Lisp command line: | ||
<code lisp> | <code lisp> | ||
PP-TUT> (init-projection) | PP-TUT> (init-projection) | ||
</ | </ | ||
+ | You can change the viewpoint in the window by pressing and dragging the left mouse button. | ||
+ | Pressing and dragging the right mouse button translates the camera. | ||
+ | Pressing and dragging the middle click doesn' | ||
==== Moving Around ==== | ==== Moving Around ==== | ||
- | First, | + | |
+ | First, | ||
<code lisp> | <code lisp> | ||
PP-TUT> (make-pose " | PP-TUT> (make-pose " | ||
</ | </ | ||
- | The '' | + | The '' |
+ | |||
+ | The screenshot below shows axes where the " | ||
+ | {{ : | ||
+ | |||
+ | Please note that in our Emacs IDE the result of executing something is colored as red. It is not an error but simply a resulting object. You can always inspect the result object by right-clicking on it and saying " | ||
+ | Press " | ||
Let's use this to move around our robot: | Let's use this to move around our robot: | ||
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
(let ((? | (let ((? | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
</ | </ | ||
- | You can see that the robot has moved to a new location which is 1 meter from the origin along the y axis in the " | + | To go onto a new line without executing, press " |
- | '' | + | |
+ | You can see that the robot has moved to a new location which is 1 meter from the origin along the y-axis in the " | ||
+ | '' | ||
+ | |||
+ | Note:- Notice how we're using '' | ||
- | Note:- The '' | + | Note: |
<code lisp> | <code lisp> | ||
- | CL-USER> (let ((a 1) | + | PP-TUT> (let ((a 1) |
(b 2)) | (b 2)) | ||
(print a) | (print a) | ||
Line 139: | Line 257: | ||
1 | 1 | ||
2 | 2 | ||
- | CL-USER> (print b) | + | PP-TUT> (print b) |
The variable B is unbound. | The variable B is unbound. | ||
</ | </ | ||
- | The variables a and b have been declared with values under the scope of let and as soon as the body of '' | + | //To exit the debugger, press " |
+ | //The variables a and b have been declared with values under the scope of let and as soon as the body of '' | ||
- | Now let's move the robot next to the table on the right-hand side in the kitchen. Since we are going to use this pose a lot, we'll save this in a variable | + | Now let's move the robot next to the table on the right-hand side of the kitchen. Since we are going to use this pose a lot, we'll save this in a global variable. In Lisp, a parameter is a kind of a global |
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
(defparameter *base-pose-near-table* | (defparameter *base-pose-near-table* | ||
(make-pose " | (make-pose " | ||
- | ;; The orientation in this pose corresponds to a rotation of -(pi/2) around the z-axis. | ||
- | ;; To check it for yourself or to create other orientations you can use the following functions | ||
- | ;; (cl-transforms: | ||
- | ;; or | ||
- | ;; (cl-transforms: | ||
- | PP-TUT> | + | (with-simulated-robot |
- | (pr2-proj:with-simulated-robot | + | |
(let ((? | (let ((? | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
</ | </ | ||
- | Note:- You'll notice that we're using ''pr2-proj: | + | Don't be surprised if you occasionally see " |
+ | {{ : | ||
+ | |||
+ | To go up in the history of your Lisp command line, press " | ||
+ | |||
+ | To clean all the input that is currently sitting in the prompt, instead of pressing " | ||
==== Spawning Objects ==== | ==== Spawning Objects ==== | ||
- | We have written and provided you with a method | + | |
+ | Now let's try to spawn a new object | ||
<code lisp> | <code lisp> | ||
PP-TUT> (spawn-object ' | PP-TUT> (spawn-object ' | ||
</ | </ | ||
- | You will see that a bottle has been created on the table in front of the PR2. By default, this method is written to help you to always create a bottle. But if you want to spawn other supported | + | {{ : |
+ | |||
+ | You will see that a red bottle has been created on the table in front of the PR2. By default, this method is written to help you to always create a bottle. But if you want to spawn other objects | ||
Eg: | Eg: | ||
<code lisp> | <code lisp> | ||
PP-TUT> (spawn-object ' | PP-TUT> (spawn-object ' | ||
</ | </ | ||
+ | Here, '': | ||
- | ==== Detecting | + | To see a list of all available objects, call: |
- | The objective of this tutorial would be to enable you to write methods to pick an object (for us, the bottle) from one place and transport and place it on another. But before the robot can pick up anything, it has to know where the object is, which is enabled by the cameras placed on the robot, which are not always trying to detect the objects in its view. We have to explicitly command the robot to detect | + | <code lisp> |
+ | PP-TUT> (list-available-objects) | ||
+ | </ | ||
+ | |||
+ | You can also give a different color to the object in RGB encoding: | ||
+ | <code lisp> | ||
+ | PP-TUT> (spawn-object '((0 0 0.1) (0 0 0 1)) :weisswurst ' | ||
+ | </ | ||
+ | This should have spawned a green sausage on the floor near the origin of the " | ||
+ | {{ : | ||
+ | |||
+ | ==== Perceiving | ||
+ | |||
+ | The objective of this tutorial would be to enable you to write methods to pick an object (for us, the bottle) from one place and transport and place it somewhere else. But before the robot can pick up anything, it has to know where the object is, which is enabled by the cameras placed on the robot. We have to explicitly command the robot to perceive a bottle to find it. | ||
The first step to enable this is to bring the object that has to be detected under the field of view of the robot' | The first step to enable this is to bring the object that has to be detected under the field of view of the robot' | ||
* Position the cameras a bit higher so that the robot can see things easily. | * Position the cameras a bit higher so that the robot can see things easily. | ||
* Move the arms away from the field of view to a standard position - also called parking the arms. | * Move the arms away from the field of view to a standard position - also called parking the arms. | ||
- | * Look towards | + | * Move the neck such that the camera is directed towards |
- | Let's do the first two steps in this first. The torso of the robot has a prismatic joint which has limits from 0 to 30cm. So we will set that to the maximum and we'll also call the method to park arms. Since these are two actions that are independent and can be done parallelly, we can bundle them together under a clause called '' | + | Let's do the first two steps in this first. The torso of the robot has a prismatic joint which has limits from 0 to 30cm. So we will set that to the maximum and we'll also call the method to park arms. Since these are two actions that are independent and can be done parallelly, we can bundle them together under a clause called '' |
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
- | | + | (with-simulated-robot |
- | (let ((? | + | (let ((? |
- | (cpl:par | + | (cpl:par |
- | ;; Increasing the height of the torso by setting the joint angle to 0.3 meters | + | ;; Moving the robot near the table. |
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type moving-torso) | + | (type going) |
- | (joint-angle 0.3))) | + | |
- | ;; Parking the arms | + | (pose ? |
- | (park-arms) | + | |
- | ;; Moving the robot near the table. | + | (perform (a motion |
- | (exe: | + | (type moving-torso) |
- | (type going) | + | (joint-angle 0.3))) |
- | (target (desig:a location | + | ;; Parking the arms |
- | (pose ? | + | (park-arms)))) |
</ | </ | ||
- | The robot will do all the said actions | + | The robot will do all the said actions |
+ | |||
+ | In Lisp, "'';''" | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | Note how until now we were performing '' | ||
+ | The distinction is that motions are very low-level, and actions are more high-level. | ||
+ | We will not go into detail here. For now, just remember that '' | ||
+ | |||
+ | If you get a navigation pose unreachable error, it might be that there are objects on the floor. | ||
+ | To clean up the environment, | ||
- | Now the next thing to do is to look towards | + | Now, the robot is standing next to the table, the torso is up and the arms are out of the field of view. |
+ | The next thing to do is to look in the direction where the bottle | ||
+ | We have found the corresponding | ||
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
Line 212: | Line 360: | ||
(make-pose " | (make-pose " | ||
;; This coordinate frame has base_footprint as reference, which is the reference | ;; This coordinate frame has base_footprint as reference, which is the reference | ||
- | ;; frame of PR2. | + | ;; frame of PR2's base. |
- | PP-TUT> | + | |
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
(let ((? | (let ((? | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type looking) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
</ | </ | ||
- | Notice that the robot now tilts it's head downwards, somewhat in the direction of where the bottle is at. | + | Notice that the robot now tilts it's head downwards, somewhat in the direction of where the bottle is. |
+ | {{ : | ||
- | Now the only thing remaining is to detect, or " | + | ''" |
+ | Here is a visualization of " | ||
+ | Note, how the coordinate frame is located on the floor. | ||
+ | |||
+ | {{ : | ||
+ | |||
+ | |||
+ | Now the only thing remaining is to detect, or " | ||
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
- | (setf *perceived-bottle* (exe:perform | + | (perform (an action |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
</ | </ | ||
+ | |||
+ | If you get an error that an object could not be perceived, the robot is probably not looking at an object of type '' | ||
+ | So press " | ||
+ | |||
+ | After the object has been perceived successfully, | ||
+ | Let's save this in a variable, as this will be an argument to the methods we will call below. | ||
+ | |||
+ | <code lisp> | ||
+ | PP-TUT> | ||
+ | (defparameter *perceived-bottle* nil) | ||
+ | (with-simulated-robot | ||
+ | (setf *perceived-bottle* (perform (an action | ||
+ | (type detecting) | ||
+ | (object (an object | ||
+ | (type bottle))))))) | ||
+ | </ | ||
+ | |||
+ | '' | ||
==== Picking up Objects ==== | ==== Picking up Objects ==== | ||
- | Once the object has been found picking up is very straightforward: | + | Once the object has been found, picking up is very straightforward: |
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
- | (let ((? | + | (let ((? |
- | (exe:perform (desig:an action | + | (perform (an action |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
;; Parking the right arm after grasping, bringing the bottle close to the robot | ;; Parking the right arm after grasping, bringing the bottle close to the robot | ||
;; and freeing up the field of view. | ;; and freeing up the field of view. | ||
| | ||
- | |||
</ | </ | ||
- | You will notice that we have provided | + | You will notice that we have provided |
- | The robot has successfully picked up the bottle with its right arm. | + | |
+ | By now, the robot should have successfully picked up the bottle with its right arm. | ||
+ | {{ : | ||
==== Placing the object ==== | ==== Placing the object ==== | ||
- | Now that the robot has the object in its gripper, the next task is to place the object in the destination. Let us use the dining area counter on the left side as our destination. So, to place the bottle the robot has to do two things - travel | + | Now that the robot has the object in its gripper, the next task is to place the object in the destination. Let us use the dining area counter on the left side as our destination. So, to place the bottle the robot has to do two things - drive with the base to a location from where it can place the bottle on the dining table, and move the arm to place the object down. |
+ | |||
+ | So before this, let's define two more locations, which are the destination of the bottle and the position of the robot to be able to reach it respectively: | ||
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
(defparameter *final-object-destination* | (defparameter *final-object-destination* | ||
(make-pose " | (make-pose " | ||
- | PP-TUT> | + | |
(defparameter *base-pose-near-counter* | (defparameter *base-pose-near-counter* | ||
(make-pose " | (make-pose " | ||
</ | </ | ||
- | And finally: | + | |
+ | Now, finally, we are going to drive with the base, and the placing action is very similar to the picking-up action, but here we provide the target destination instead: | ||
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
;; Moving the robot near the counter. | ;; Moving the robot near the counter. | ||
(let ((?nav-goal *base-pose-near-counter*)) | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
- | (coe: | + | |
;; Setting the bottle down on the counter | ;; Setting the bottle down on the counter | ||
(let ((? | (let ((? | ||
(? | (? | ||
- | (exe:perform (desig:an action | + | (perform (an action |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (pose ? | + | (pose ? |
(park-arm :right)) | (park-arm :right)) | ||
</ | </ | ||
The robot has finally placed the object in the destination. | The robot has finally placed the object in the destination. | ||
+ | {{ : | ||
==== Simple Plan ==== | ==== Simple Plan ==== | ||
- | Even though we have achieved the goal we had set, the entire process would have felt a little tedious, especially if you want to perform it multiple times, when you have made a mistake, etc. So let's bundle what we have done so far into a single method called, '' | + | |
+ | Even though we have achieved the goal we had set, the entire process would have felt a little tedious, especially if you want to perform it multiple times, when you have made a mistake, etc. So let's bundle what we have done so far into a single method called, '' | ||
+ | |||
+ | First let's open the file which we will write into. Press '' | ||
+ | < | ||
+ | Find File: ~/< | ||
+ | ;; Notice that < | ||
+ | ;; on the machine | ||
+ | </ | ||
+ | |||
+ | The file will open up and you'll see that it has only one line as of now: | ||
+ | <code lisp> | ||
+ | (in-package :pp-tut) | ||
+ | </ | ||
+ | This line just selects the namespace that we're going to work with and you'll notice that it's the name of the package of this demo. Do not remove this line throughout the demo. | ||
+ | |||
+ | After that you can copy paste the following code into the file: | ||
<code lisp> | <code lisp> | ||
(defparameter *base-pose-near-table* | (defparameter *base-pose-near-table* | ||
- | (make-pose " | + | (make-pose " |
(defparameter *downward-look-coordinate* | (defparameter *downward-look-coordinate* | ||
Line 300: | Line 494: | ||
(defparameter *base-pose-near-counter* | (defparameter *base-pose-near-counter* | ||
(make-pose " | (make-pose " | ||
+ | |||
(defparameter *final-object-destination* | (defparameter *final-object-destination* | ||
(make-pose " | (make-pose " | ||
+ | |||
(defun move-bottle (bottle-spawn-pose) | (defun move-bottle (bottle-spawn-pose) | ||
(spawn-object bottle-spawn-pose) | (spawn-object bottle-spawn-pose) | ||
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
- | (exe: | ||
- | (type moving-torso) | ||
- | (joint-angle 0.3))) | ||
- | (park-arms) | ||
;; Moving the robot near the table. | ;; Moving the robot near the table. | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
+ | (perform (a motion | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
+ | (park-arms))) | ||
;; Looking towards the bottle before perceiving. | ;; Looking towards the bottle before perceiving. | ||
(let ((? | (let ((? | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type looking) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
;; Detect the bottle on the table. | ;; Detect the bottle on the table. | ||
(let ((? | (let ((? | ||
- | (? | + | (? |
- | | + | (type detecting) |
- | | + | (object (an object |
- | | + | (type bottle))))))) |
;; Pick up the bottle | ;; Pick up the bottle | ||
- | (exe:perform (desig:an action | + | (perform (an action |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
(park-arm ? | (park-arm ? | ||
;; Moving the robot near the counter. | ;; Moving the robot near the counter. | ||
(let ((?nav-goal *base-pose-near-counter*)) | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
- | (coe: | + | |
;; Setting the bottle down on the counter | ;; Setting the bottle down on the counter | ||
(let ((? | (let ((? | ||
- | (exe:perform (desig:an action | + | (perform (an action |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (pose ? | + | (pose ? |
(park-arm ? | (park-arm ? | ||
</ | </ | ||
- | '' | + | This defines a function called |
- | Now run '' | + | This function |
+ | After spawning the object, '' | ||
+ | |||
+ | First, save the file using the shortcut '' | ||
+ | |||
+ | Now to go back to the REPL to run the code, press '' | ||
+ | |||
+ | Now, run '' | ||
<code lisp> | <code lisp> | ||
PP-TUT> (move-bottle ' | PP-TUT> (move-bottle ' | ||
</ | </ | ||
+ | |||
You will see that the robot successfully picks the bottle and places it on the counter. | You will see that the robot successfully picks the bottle and places it on the counter. | ||
+ | //[If you're stuck, remember that you can always clean up the world using '' | ||
- | You have now written your first rudimentary plan to pick and place a bottle. | + | Congratulations! |
===== Recovering from Failures ===== | ===== Recovering from Failures ===== | ||
- | The previous example worked perfectly, because we knew the exact coordinates to look for the bottle. This is hardly true for the real life scenario, especially since we are dealing with a kitchen environment, | + | The previous example worked perfectly because we knew and provided |
<code lisp> | <code lisp> | ||
PP-TUT> (move-bottle '((-2 -0.9 0.860) (0 0 0 1))) | PP-TUT> (move-bottle '((-2 -0.9 0.860) (0 0 0 1))) | ||
</ | </ | ||
- | And the output will look like this. | + | Now, the output will look like this. |
<code lisp> | <code lisp> | ||
PP-TUT> | PP-TUT> | ||
+ | [(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: | ||
+ | [(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: | ||
+ | [(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: | ||
+ | [(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: | ||
; Evaluation aborted on #< | ; Evaluation aborted on #< | ||
</ | </ | ||
+ | |||
+ | Press " | ||
{{: | {{: | ||
- | Clearly the robot cannot find the object anymore because even though the robot is near the bottle, it is out of the field of vision of the robot due to the predefined base and object pose in our code. Let's try fixing this issue. | + | Clearly, the robot cannot find the object anymore because even though the robot is near the bottle, it is out of the field of vision of the robot due to the predefined base and object pose in our code. Let's try fixing this issue. |
- | To understand the syntax and get a refresher on failure handling, you can refer to [[tutorials: | + | We're going to add some code into fixing |
- | We're going to add some code into fixing perception in our case, so that the robot would still be able to find the bottle. Let's list down a plan of action for this. | + | |
- Tilt the head of the robot downwards | - Tilt the head of the robot downwards | ||
- Try to detect the bottle | - Try to detect the bottle | ||
- If, | - If, | ||
* successful in finding the bottle - continue with the rest of the code. | * successful in finding the bottle - continue with the rest of the code. | ||
- | * failed to find the bottle - turn the head to a different configuration (eg., left/right) and try detecting again. | + | * failed to find the bottle - turn the head to a different configuration (e.g., left/right) and try detecting again. |
- When all possible directions fail, error out. | - When all possible directions fail, error out. | ||
- | Let's define | + | Let's define |
<code lisp> | <code lisp> | ||
- | PP-TUT> | + | |
(defparameter *left-downward-look-coordinate* | (defparameter *left-downward-look-coordinate* | ||
- | (cl-transforms-stamped: | + | (make-pose " |
- | " | + | |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | |
(defparameter *right-downward-look-coordinate* | (defparameter *right-downward-look-coordinate* | ||
- | (cl-transforms-stamped: | + | (make-pose " |
- | " | + | |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | |
- | + | ||
</ | </ | ||
- | We defined two coordinates '' | ||
- | Now we define a method '' | + | We defined two coordinates, |
+ | These will be alternative directions (left and right respectively) to look at in the case when looking downward fails. | ||
+ | |||
+ | Now we define a method '' | ||
<code lisp> | <code lisp> | ||
- | (defun get-preferred-arm-for-direction (direction-looked) | + | |
- | (let ((preferred-arm :RIGHT)) | + | |
- | (when (eq direction-looked *left-downward-look-coordinate*) | + | |
- | (setf preferred-arm :LEFT)) | + | |
- | preferred-arm)) | + | |
- | | + | |
(defun find-object (? | (defun find-object (? | ||
(let* ((possible-look-directions `(, | (let* ((possible-look-directions `(, | ||
Line 421: | Line 619: | ||
, | , | ||
| | ||
- | (setf possible-look-directions (cdr possible-look-directions)) | + | (setf possible-look-directions (rest possible-look-directions)) |
- | (exe:perform (desig:a motion | + | ;; Look towards the first direction |
- | (type looking) | + | (perform (an action |
- | (target (desig:a location | + | |
- | | + | |
- | + | (pose ? | |
- | | + | |
- | ((cram-common-failures: | + | |
- | | + | ;; Now we're wrapping it in a failure |
- | | + | (handle-failure |
- | | + | ;; Try the action |
- | | + | |
- | | + | (type detecting) |
- | | + | |
- | | + | (type ? |
- | | + | |
- | | + | |
- | | + | ;; try different look directions until there is none left. |
- | | + | (when possible-look-directions |
- | (pose ? | + | (print "Perception error happened! |
- | | + | ;; Resetting the head to look forward before turning again |
- | (cpl:fail ' | + | |
- | + | | |
- | (let ((? | + | |
- | (exe:perform (desig:a motion | + | (setf ? |
- | (type detecting) | + | (setf possible-look-directions (rest possible-look-directions)) |
- | (object (desig:an object | + | (perform (an action |
- | (type ? | + | |
- | (values ?perceived-bottle (get-preferred-arm-for-direction ? | + | |
- | </ | + | (pose ? |
- | Let's see what this method does. The '' | + | ;; This statement retries the action again |
- | Also note that the '' | + | |
- | Let us also update | + | ;; If everything else fails, error out |
+ | ;; Reset the neck before erroring out | ||
+ | (perform (an action | ||
+ | | ||
+ | | ||
+ | (cpl:fail ' | ||
+ | </ | ||
+ | |||
+ | Let's see what this method does. The '' | ||
+ | |||
+ | // | ||
+ | <code lisp> | ||
+ | (handle-failure <the error type that needs to be handled> | ||
+ | (<all the actions to be performed under normal execution> | ||
+ | (< | ||
+ | |||
+ | </ | ||
+ | |||
+ | |||
+ | //We have used the methods '' | ||
+ | <code lisp> | ||
+ | PP-TUT> (first '(1 2 3 4)) | ||
+ | 1 | ||
+ | PP-TUT> (rest '(1 2 3 4)) | ||
+ | (2 3 4) | ||
+ | </ | ||
+ | //In our failure handling code, we use '' | ||
+ | |||
+ | |||
+ | Now we need to redefine | ||
<code lisp> | <code lisp> | ||
(defun move-bottle (bottle-spawn-pose) | (defun move-bottle (bottle-spawn-pose) | ||
- | (spawn-bottle | + | (spawn-object |
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
- | (exe: | ||
- | (type moving-torso) | ||
- | (joint-angle 0.3))) | ||
- | (pp-plans:: | ||
;; Moving the robot near the table. | ;; Moving the robot near the table. | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
- | ;; Find and detect the bottle on the table. | + | (perform (a motion |
- | (multiple-value-bind | + | (type moving-torso) |
- | | + | (joint-angle 0.3))) |
- | (exe:perform (desig:an action | + | (park-arms))) |
- | | + | ;; Find and detect the bottle on the table. |
- | | + | (let ((? |
- | | + | (? |
- | | + | (perform (an action |
- | (pp-plans::park-arms :arm ? | + | |
+ | | ||
+ | | ||
+ | | ||
+ | (park-arm ? | ||
;; Moving the robot near the counter. | ;; Moving the robot near the counter. | ||
(let ((?nav-goal *base-pose-near-counter*)) | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
- | + | ||
- | (coe: | + | |
;; Setting the object down on the counter | ;; Setting the object down on the counter | ||
(let ((? | (let ((? | ||
- | (exe:perform (desig:an action | + | (perform (an action |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (pose ? | + | (pose ? |
- | (pp-plans::park-arms :arm ? | + | (park-arm ? |
</ | </ | ||
+ | Save the changes, compile the file again and switch back to the REPL. | ||
+ | |||
+ | Now run: | ||
+ | <code lisp> | ||
+ | PP-TUT> (move-bottle '((-2 -0.9 0.860) (0 0 0 1))) | ||
+ | </ | ||
+ | |||
+ | And once again, you'll find that the robot succeeds in transporting the bottle. | ||
+ | |||
+ | If you get an error that says the manipulation goal is unreachable, | ||
- | Run '' | ||
{{: | {{: | ||
- | ===== Expanding Failure Management Capabilities ===== | + | ==== Handling More Failures |
- | Everything is good so far, but let's call this a lucky coincidence. For the robot, knowing which arm to use to pick up the bottle is not always enough. There are many positions with which we can grasp objects - from the object' | + | |
+ | Everything is good so far, even though by design, | ||
+ | |||
+ | {{: | ||
+ | * +ve x-axis -> front, -ve x-axis -> back | ||
+ | * +ve y-axis -> left, -ve y-axis -> right | ||
+ | * +ve z-axis -> top, -ve z-axis -> bottom | ||
+ | And a grasp means that the gripper of the robot approaches from that side and grasps it. So, a left-side grasp means the robot gripper approaches from the +ve y-axis of the bottle and grasps it. | ||
+ | |||
+ | One might think that since the bottle is a rotationally symmetric object, it doesn' | ||
- | Let's try to visualize | + | Let's try to create |
<code lisp> | <code lisp> | ||
- | PP-TUT> (spawn-bottle | + | PP-TUT> (spawn-object |
</ | </ | ||
- | Now clean up and run '' | + | Now run '' |
<code lisp> | <code lisp> | ||
- | PP-TUT> (move-bottle '((-1.1 -0.75 0.860) (0 0 0 1))) | + | PP-TUT> (move-bottle '((-1.0 -0.75 0.860) (0 0 0 1))) |
- | [(PICK-PLACE PICK-UP) INFO] 1550502686.470: Opening gripper | + | [(PICK-PLACE PICK-UP) INFO] 1566442996.350: Opening gripper |
- | [(PICK-PLACE PICK-UP) INFO] 1550502686.470: Reaching | + | [(PICK-PLACE PICK-UP) INFO] 1566442996.350: Reaching |
- | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) | + | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) |
| | ||
- | #< | + | #< |
- | #< | + | #< |
- | Ignoring. | + | |
- | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1550502687.092: | + | |
- | | + | |
- | #< | + | |
- | #< | + | |
Failing. | Failing. | ||
- | [(PP-PLANS PICK-UP) WARN] 1550502687.092: Manipulation messed up: #< | + | [(PP-PLANS PICK-UP) WARN] 1566442996.818: Manipulation messed up: #< |
| | ||
- | #< | + | #< |
- | #< | + | #< |
Ignoring. | Ignoring. | ||
- | ; Evaluation aborted on #<CRAM-COMMON-FAILURES: | + | [(PICK-PLACE PICK-UP) INFO] 1566442996.818: |
+ | [(PICK-AND-PLACE GRIP) WARN] 1566442996.857: | ||
+ | Retrying | ||
+ | [(PICK-AND-PLACE GRIP) WARN] 1566442996.868: | ||
+ | ; Evaluation aborted on #<GRIPPER-CLOSED-COMPLETELY | ||
</ | </ | ||
The robot has failed to grasp again, even though the bottle is well within perception and grasping range. | The robot has failed to grasp again, even though the bottle is well within perception and grasping range. | ||
+ | **Note:** //You might also encounter an error of a different type called "'' | ||
- | So what went wrong? | + | So what went wrong? |
- | <code lisp> | + | |
- | (defun get-preferred-arm-for-direction (direction-looked) | + | |
- | (let ((preferred-arm :RIGHT)) | + | |
- | ;; Always prefers right arm unless looking at the left side | + | |
- | (when (eq direction-looked *left-downward-look-coordinate*) | + | |
- | (setf preferred-arm :LEFT)) | + | |
- | preferred-arm)) | + | |
- | </ | + | |
- | And the part where we pick up the object: | + | |
- | <code lisp> | + | |
- | (exe: | + | |
- | (type picking-up) | + | |
- | (arm ? | + | |
- | (grasp left-side) | + | |
- | | + | |
- | </ | + | |
- | We see that the robot defaults the right arm when the object | + | |
- | Note: '' | + | {{:tutorials: |
- | '' | + | Not all objects |
- | '' | + | |
- | '' | + | <code lisp> |
- | + | PP-TUT> (list-defined-grasps :bottle) | |
- | {{: | + | </ |
- | Once again, let' | + | To figure out a good failure handling strategy, let' |
- Choose the favored arm. | - Choose the favored arm. | ||
- Get all possible grasp poses for the given type of the object and the arm. | - Get all possible grasp poses for the given type of the object and the arm. | ||
Line 571: | Line 797: | ||
- When attempted with all arms and grasps, error out. | - When attempted with all arms and grasps, error out. | ||
- | Let's encapsulate all this in a method called '' | + | Let's encapsulate all this in a method called '' |
<code lisp> | <code lisp> | ||
- | (defun pick-up-object (? | + | (defun pick-up-object (? |
- | (let ((? | + | (let* ((? |
- | | + | |
+ | | ||
+ | |||
(cpl: | (cpl: | ||
- | (cpl:with-failure-handling | + | ;; Outer handle |
- | ((common-fail:object-unreachable | + | (handle-failure |
- | (roslisp: | + | ;; Iner handle-failure |
- | | + | ((handle-failure |
- | | + | |
- | | + | |
- | | + | |
- | + | | |
- | | + | |
- | (let* ((? | + | |
- | | + | ;; When pick-up fails this block gets executed |
- | | + | (format t "Grasp failed! Error: |
- | (cpl:with-retry-counters ((grasp-retries 3)) | + | |
- | (cpl: | + | ;; Checks if we have any possible grasps left. |
- | (((or cram-common-failures: | + | ;; If yes, then the block nested to it gets executed, which will |
- | cram-common-failures: | + | ;; set the grasp that is used to the new value and trigger |
- | (roslisp: | + | |
- | | + | |
- | | + | |
- | (cpl:do-retry grasp-retries | + | |
- | | + | |
- | | + | (park-arms) |
- | " | + | (cpl: |
- | | + | ;; This will get executed when there are no more elements in the |
- | | + | ;; ?possible-grasps list. We print the error message and throw a new error |
- | (pp-plans:: | + | ;; which will be caught by the outer handle-failure |
- | (setf ?grasp (cut:lazy-car ?possible-grasp)) | + | |
- | | + | |
- | (roslisp: | + | |
- | | + | |
- | ;; Perform | + | ;; It changes the arm that is used to grasp |
- | (exe:perform (desig:an action | + | (print " |
- | | + | (print e) |
- | | + | |
- | | + | ;; Here we use the retry counter we defined. The value is decremented automatically |
- | (object | + | |
+ | ;; if the current grasping | ||
+ | (setf ?grasping-arm | ||
+ | :left | ||
+ | :right)) | ||
+ | (cpl:retry)) | ||
+ | ;; When all retries are exhausted print the error message. | ||
+ | (print "No more arm change retries left :(")))) | ||
? | ? | ||
</ | </ | ||
- | With this, the '' | + | With this, the '' |
- | + | ||
- | There are two nested failure-handling clauses here. The inner failure-handling part will take care of '' | + | |
- | And only upon the failure | + | Even though this code block looks big, we're mostly doing things that we learned in the previous |
- | Also let's redefine '' | + | Next is the '' |
+ | |||
+ | So let's see how both of these help with our situation. The inner failure-handling part will take care of '' | ||
+ | |||
+ | And only upon the failure of all these will the error be bubbled up. The method also returns the grasping arm which it used to pick the object up so that the rest of the code is aware of the arm in which the bottle rests. We have also written appropriate warning statements to be informed about the actions the robot is taking. | ||
+ | |||
+ | Also let's redefine '' | ||
<code lisp> | <code lisp> | ||
(defun move-bottle (bottle-spawn-pose) | (defun move-bottle (bottle-spawn-pose) | ||
- | (spawn-bottle | + | (spawn-object |
- | (pr2-proj:with-simulated-robot | + | (with-simulated-robot |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
- | (exe: | ||
- | (type moving-torso) | ||
- | (joint-angle 0.3))) | ||
- | (pp-plans:: | ||
;; Moving the robot near the table. | ;; Moving the robot near the table. | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
+ | (perform (a motion | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
+ | (park-arms))) | ||
| | ||
- | (multiple-value-bind | + | (let ((? |
- | | + | (? |
- | (setf ? | + | ;; We update the value of ? |
- | (pp-plans::park-arms :arm ? | + | (setf ? |
+ | (park-arm ? | ||
;; Moving the robot near the counter. | ;; Moving the robot near the counter. | ||
(let ((?nav-goal *base-pose-near-counter*)) | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (exe:perform (desig:a motion | + | (perform (an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
- | + | | |
- | (coe: | + | |
- | | + | |
(let ((? | (let ((? | ||
- | (exe:perform (desig:an action | + | (perform (an action |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (pose ? | + | (pose ? |
- | (pp-plans::park-arms :arm ? | + | (park-arm ? |
</ | </ | ||
+ | Save the file, compile and switch back to the REPL. | ||
+ | |||
You should see a result that looks like the one below. //[Some messages that would come up are suppressed here for readability.]// | You should see a result that looks like the one below. //[Some messages that would come up are suppressed here for readability.]// | ||
<code lisp> | <code lisp> | ||
- | PP-TUT> (init-projection) | + | PP-TUT> (move-bottle |
- | PP-TUT> (move-bottle) | + | |
[(PICK-PLACE PICK-UP) INFO] 1550504321.279: | [(PICK-PLACE PICK-UP) INFO] 1550504321.279: | ||
[(PICK-PLACE PICK-UP) INFO] 1550504321.279: | [(PICK-PLACE PICK-UP) INFO] 1550504321.279: | ||
Line 686: | Line 925: | ||
The robot has once again succeeded in grasping the object. | The robot has once again succeeded in grasping the object. | ||
+ | |||
+ | One trend you'll notice in both of the failure handling methods that we wrote is: for one line of execution of robot action, we had to write 10+ lines of code to take care of possible failures. And this is how the programs for robots are written, in which failure handling routines dominate a huge part of it. | ||
+ | |||
+ | |||
+ | |||
+ | ===== Visualizing Coordinates ===== | ||
+ | |||
+ | If you want to know if a coordinate you defined is correct, you can visualize the axis of the coordinate frame in the Bullet World and see for yourself. | ||
+ | Try the following: | ||
+ | |||
+ | <code lisp> | ||
+ | PP-TUT> (visualize-coordinates :bottle) | ||
+ | </ | ||
+ | This will spawn a coordinate frame on our bottle object. | ||
+ | The argument here is the object type for which we want to visualize the coordinates. | ||
+ | Make sure you use '':'' | ||
+ | {{ : | ||
+ | |||
+ | You can also visualize the coordinates of an object with a specific name: | ||
+ | <code lisp> | ||
+ | PP-TUT> (spawn-object '((1.5 1 1) (0 0 0 1)) :weisswurst ' | ||
+ | PP-TUT> (visualize-coordinates ' | ||
+ | </ | ||
+ | This will spawn a green sausage on the sink counter and visualize its coordinate frame. | ||
+ | Note how here we used the name of the object, ''' | ||
+ | weisswurst might have a different coordinate axes than that shown in the picture below for you below because of | ||
+ | its shape and the randomness of the simulation// | ||
+ | {{ : | ||
+ | |||
+ | You can also visualize poses that don't have any objects on them yet, for example: | ||
+ | <code lisp> | ||
+ | PP-TUT> (init-projection) | ||
+ | PP-TUT> (visualize-coordinates *base-pose-near-table*) | ||
+ | </ | ||
+ | You will see the coordinate near the table on the right side of the kitchen visualized. | ||
+ | {{ : | ||
+ | |||
+ | You can also change the scale of the coordinate frame, if you don't quite see it: | ||
+ | <code lisp> | ||
+ | PP-TUT> (visualize-coordinates *base-pose-near-table* 1.0) | ||
+ | </ | ||
+ | That will make the axes 1 meter long. | ||
+ | {{ : | ||
+ | |||
+ | You can also visualize a pose in a coordinate frame different to " | ||
+ | but our visualizer always shows the coordinates with respect to the " | ||
+ | So, if you want to visualize a looking location or so, assume that the robot is standing in its initial location, | ||
+ | which is the '' | ||
+ | <code lisp> | ||
+ | PP-TUT> (visualize-coordinates *right-downward-look-coordinate*) | ||
+ | </ | ||
+ | {{ : | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | ===== Exercise 1 ===== | ||
+ | |||
+ | Difficulty level: Easy. | ||
+ | |||
+ | Now that we figured out how to do some failure handling, let's do an exercise! | ||
+ | Move the bottle completely out of the reach of the robot. For example, place it at the kitchen island, near the narrower edge: | ||
+ | |||
+ | <code lisp> | ||
+ | PP-TUT> (spawn-object ' | ||
+ | </ | ||
+ | |||
+ | Now, try to write a failure handling strategy to adjust the base pose of the robot so that it can once again grab the bottle. | ||
+ | It should be able to grasp the object from both the old locations and the new ones. | ||
+ | The easiest is to define a couple more base poses, as global variables/ | ||
+ | |||
+ | Whenever you command the robot to move somewhere, make sure that you are not asking him to leave the environment, | ||
+ | The screenshot below is the top-view of the bullet world, where the red/brown region indicates the area where the PR2 is allowed to stand. | ||
+ | |||
+ | {{: | ||
+ | |||
+ | Here is a simple plan of action for this exercise: | ||
+ | * Using '' | ||
+ | * Find the current implementation of '' | ||
+ | * Wrap the part of '' | ||
+ | |||
+ | If you decide to do your '' | ||
+ | |||
+ | <code lisp> | ||
+ | (let ((? | ||
+ | |||
+ | (let ((my-new-awesome-lists ...)...) | ||
+ | (handle-failure ... | ||
+ | | ||
+ | (do-the-handling))) | ||
+ | |||
+ | (? | ||
+ | (the-insides-of-the-outer-let)) | ||
+ | </ | ||
+ | |||
+ | This might look very weird to a person, who's used to traditional imperative programming style, | ||
+ | but in functional programming this is totally possible and readable, if you use enough white space ;). | ||
+ | |||
+ | Exercise 1-B (Difficulty level: Advanced): To increase the complexity, try to write a function that can calculate a good base position for any location of the object, such that you are now not hardcoding the base poses but calculating them depending on the object pose. | ||
+ | With that the robot should be able to grasp the object from any location on any of the two tables. | ||
+ | |||
+ | |||
+ | ===== Exercise 2 ===== | ||
+ | |||
+ | Difficulty level: Easy. | ||
+ | |||
+ | Until now, we did not care about the order of different things that we try out. | ||
+ | For example, we always first try to pick up the object with the right arm, and only if that fails, with the left arm. | ||
+ | But what if it is more or less obvious, that the object is easier grasped with the left arm, for example, in case | ||
+ | the object is on the far left side of the robot. | ||
+ | |||
+ | So, let us suggest the robot a good arm to start with, and if that doesn' | ||
+ | |||
+ | For that, check the base pose of the robot and the relative pose of the bottle and see if you can write a method which suggests which arm to use to pick up, depending on the relative position of the bottle. | ||
+ | |||
+ | Here are the steps for solving this exercise: | ||
+ | * Define a new function called '' | ||
+ | * Inside the function, find the object pose in the map frame: '' | ||
+ | * Also, find the robot transform in map frame: '' | ||
+ | * Invert the robot transform to get '' | ||
+ | * Multiply inverted robot transform with object transform to get the object pose relative to the robot: '' | ||
+ | * Use the X and Y coordinates of the object pose to decide which arm to use. | ||
+ | |||
+ | You can use the following helper functions to solve this exercise: | ||
+ | |||
+ | <code lisp> | ||
+ | ;; gets a description of an object from FIND-OBJECT and returns the name, e.g., ' | ||
+ | (get-object-name PERCEIVED-OBJECT-DESCRIPTION) | ||
+ | |||
+ | ;; returns a pose in the " | ||
+ | (get-current-pose-of-object OBJECT-NAME) | ||
+ | |||
+ | ;; returns the transformation from robot frame into map frame, i.e., map-T-robot | ||
+ | (get-robot-transformation) | ||
+ | |||
+ | ;; inverts a transform, i.e. x-T-y turns into y-T-x | ||
+ | (inverse-transformation TRANSFORM) | ||
+ | |||
+ | ;; multiplies given TRANSFORM with POSE, i.e. x-T-y * y-P-z = x-P-z | ||
+ | (apply-transformation transform pose) | ||
+ | |||
+ | ;; returns the X component of a pose, e.g., for a '((1 2 3) (0 0 0 1)) that would be 1 | ||
+ | (get-x-of-pose POSE) | ||
+ | |||
+ | ;; returns the Y component of a pose, e.g., for a '((1 2 3) (0 0 0 1)) that would be 2 | ||
+ | (get-y-of-pose POSE) | ||
+ | </ | ||
+ | ===== Exercise 3 ===== | ||
+ | |||
+ | Difficulty level: Easy. | ||
+ | |||
+ | When an object cannot be found even though we looked into three different directions, | ||
+ | one other thing that can help is to move the torso up and down to increase the field of view even more. | ||
+ | |||
+ | Here's how we should do it: | ||
+ | * Find the previous implementation of FIND-OBJECT that we used. | ||
+ | * Create a list of possible torso positions: we could start with 0, then retry with 0.1, 0.2 and 0.3. | ||
+ | * Wrap the detecting action, which was already wrapped with a failure handling for looking in different directions, into yet another '' | ||
+ | * When implementing your failure handling, make sure that you reset the looking directions list and the variable, because otherwise the robot will think that he exhausted all the looking directions and will only look straight after you have moved the torso. | ||
+ | ===== Defining a New Grasp ===== | ||
+ | |||
+ | We discussed predefined possible grasps for a bottle in the previous section, which were 4 in number. Even though there are a lot of different possible grasps in real life, using only 4 limited our possible ways of grasping the bottle. So can we add more? Of course! Let's see that in this section. Let's try to grasp the bottle diagonally between positive x and y axes. We'll call this the front-left-diagonal grasp. | ||
+ | |||
+ | There are two things to be carried out during any grasp pose: First is the translation the gripper will have to make to perform the grasp and the other one is the orientation the gripper will have to make to match the grasp that is to be performed. | ||
+ | You car refer the image below again to see what axes correspond to what face of the object. | ||
+ | {{ : | ||
+ | |||
+ | For a front-left-diagonal grasp, the gripper has to come positive x and y-axis side and then close in and finally do a lift to complete the object pick-up. So we'll define some parameters that will help us do the translation part of the grasping. The values are taken from the predefined grasp offset values in <your workspace>/ | ||
+ | |||
+ | <code lisp> | ||
+ | (defparameter *lift-z-offset* 0.15 "in meters" | ||
+ | (defparameter *lift-offset* `(0.0 0.0 , | ||
+ | |||
+ | (defparameter *bottle-pregrasp-xy-offset* 0.15 "in meters" | ||
+ | (defparameter *bottle-grasp-xy-offset* 0.02 "in meters" | ||
+ | (defparameter *bottle-grasp-z-offset* 0.005 "in meters" | ||
+ | </ | ||
+ | |||
+ | As for the orientation, | ||
+ | |||
+ | To visualize this easily, open your left palm with the thumb perpendicular to the other 4 fingers, now imagine the y-axis going along your thumb, the x-axis along your other 4 fingers and the z-axis coming from inside your palm perpendicular to the surface. Now to grasp a bottle, you'll face the palm to the bottle which is the z-axis and your 4 fingers will be horizontally perpendicular to the height (z-axis) of the bottle. | ||
+ | |||
+ | <code lisp> | ||
+ | (defparameter *sin-pi/4* (sin (/ pi 4))) | ||
+ | (defparameter *-sin-pi/4* (- (sin (/ pi 4)))) | ||
+ | |||
+ | (defparameter *diagonal-rotation* | ||
+ | `((, | ||
+ | (, | ||
+ | (0 1 0))) | ||
+ | </ | ||
+ | The rotation matrix given above is the rotation matrix required to bring the gripper from the identity pose to the pose that we need to make a left-diagonal grasp. This is achieved by rotating the identity pose along the x-axis by +90 degrees first, and then rotating the new pose-along the y-axis by -45 degrees. Try to visualize these rotations mentally. | ||
+ | Note:-Since sin(pi/4) = cos(pi/4), we have only defined one variable and used it interchangeably in the defined rotation matrix | ||
+ | |||
+ | Now let's define the method that tie up all this and let ROS know that a new grasp is available | ||
+ | <code lisp> | ||
+ | (cram-object-interfaces: | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | </ | ||
+ | The summary of this code is pretty simple, we have defined a grasp called '' | ||
+ | |||
+ | Now that we have defined our new grasp let's see it in action. First, spawn the bottle again and position the robot ready to pick up. | ||
+ | <code lisp> | ||
+ | (spawn-object ' | ||
+ | </ | ||
+ | |||
+ | <code lisp> | ||
+ | (pr2-proj: | ||
+ | (let ((? | ||
+ | (cpl:par | ||
+ | (exe: | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
+ | (park-arms) | ||
+ | ;; Moving the robot near the table. | ||
+ | (exe: | ||
+ | (type going) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | ;; Looking towards the bottle before perceiving. | ||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type looking) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | ;; Detect the bottle on the table. | ||
+ | (setf *perceived-bottle* (exe: | ||
+ | (type detecting) | ||
+ | | ||
+ | (type : | ||
+ | </ | ||
+ | |||
+ | Now let's call the pick-up action with the grasp '' | ||
+ | <code lisp> | ||
+ | (pr2-proj: | ||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type picking-up) | ||
+ | (arm right) | ||
+ | | ||
+ | | ||
+ | </ | ||
+ | You will see that PR2 will successfully grasp the bottle from the grasp pose that we defined. | ||
+ | |||
+ | {{: | ||
+ | |||
+ | |||
+ | ===== Exercise 4 ===== | ||
+ | |||
+ | Difficulty level: Medium. | ||
+ | |||
+ | Now that we learned how to define new grasps, try to define your own! | ||
+ | A top grasp should be a simple one: you need to align the direction of the gripper with the negative Z axis of the object. | ||
+ | The Z axis of the object is the one that looks from bottom to top and you want the gripper to look from top to bottom. | ||
+ | |||
+ | |||
+ | |||
+ | ===== Exercise 5 ===== | ||
+ | |||
+ | Difficulty level: Advanced. | ||
+ | |||
+ | Even though we already covered a lot of scenarios here, there are many more things that can be added and handled. | ||
+ | One of the advanced things that you could do is: if an object placement location is unreachable with the current grasp, try to place it down somewhere intermediately and regrasp it in a different way. | ||
+ | |||
+ | |||
+ | [[tutorials: | ||
+ | |||
+ | ===== Credits ===== | ||
+ | |||
+ | This tutorial was created with the combined efforts of Amar Fayaz, Arthur Niedzwiecki and Gayane Kazhoyan. | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | < | ||
+ | |||
+ | ===== FEEDBACK ===== | ||
+ | |||
+ | |||
+ | |||
+ | --></ |