Differences

This shows you the differences between two versions of the page.

Link to this comparison view

tutorials:demo:fetch_and_place [2019/08/15 21:16]
arthur [VirtualBox Setup]
tutorials:demo:fetch_and_place [2019/08/23 15:30] (current)
amar [Moving Around] Added screenshot for the "map" coordinate frame
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"​ category, which assumes no prior knowledge of Ubuntu, ROS, Emacs, Lisp or robotics.+This tutorial is from the "Demo Tutorials"​ category, which assumes no prior knowledge of Linux, ROS, Emacs, Lisp or robotics. 
 + 
 + 
 + 
  
 ===== Setting Up ===== ===== Setting Up =====
Line 7: Line 11:
 You will need a virtual machine to run this tutorial. We use VirtualBox to run the VM that contains all the preinstalled requisites needed. First, download and install VirtualBox on your computer, then download and configure the VM. You will need a virtual machine to run this tutorial. We use VirtualBox to run the VM that contains all the preinstalled requisites needed. First, download and install VirtualBox on your computer, then download and configure the VM.
  
-====Install====+==== Install ==== 
 Here are some hints on how to install VirtualBox on Ubuntu, Arch, Windows and MacOS Here are some hints on how to install VirtualBox on Ubuntu, Arch, Windows and MacOS
  
Line 32: Line 37:
  
 Download [[https://​download.virtualbox.org/​virtualbox/​5.2.32/​VirtualBox-5.2.32-132073-OSX.dmg | the .dmg installer for OS X systems]], execute the file and follow the instructions. Download [[https://​download.virtualbox.org/​virtualbox/​5.2.32/​VirtualBox-5.2.32-132073-OSX.dmg | the .dmg installer for OS X systems]], execute the file and follow the instructions.
 +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://​seafile.zfn.uni-bremen.de/​d/​60fda63a3a4047aaa59b/​|cram_vm.vdi here]]. You need to enter your Uni-Bremen credentials to access the ZfN file-sharing platform. It contains an Ubuntu 16.04 with ROS kinetic and CRAM v7.0 pre-installed. ​+  * Get the Virtual Disk Image [[https://​seafile.zfn.uni-bremen.de/​d/​60fda63a3a4047aaa59b/​|cram_vm.vdi here]]. It contains an Ubuntu 16.04 with ROS kinetic and CRAM v7.0 pre-installed. ​
   * Launch VirtualBox.   * Launch VirtualBox.
  
Line 56: Line 62:
  
 ===Settings (optional)=== ===Settings (optional)===
 +
 For better performance the VM could use some of the CPU's power. Choose the newly created VM and hit '​Settings'​. Go to '​System'​ and '​Processor'​ and choose how many processor cores you want to give the VM. For better performance the VM could use some of the CPU's power. Choose the newly created VM and hit '​Settings'​. Go to '​System'​ and '​Processor'​ and choose how many processor cores you want to give the VM.
 {{ :​tutorials:​demo:​vm4proc.png?​direct |}} {{ :​tutorials:​demo:​vm4proc.png?​direct |}}
  
 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 '​cram'​. 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 '​cram'​.
-===== Crash Course on Ubuntu, ROS, Emacs, Lisp and Robotics :D ===== 
  
-==== Launching ​the terminal ​==== + 
-To launch ​the terminal press the Super Key (The windows key on the keyboard) to launch ​search ​and type in "​terminal"​ and launch the program. ​The keyboard shortcut ​is "''​Ctrl + Alt + T''"​+ 
 + 
 + 
 + 
 + 
 + 
 + 
 + 
 +===== Understanding ​the Basics ===== 
 + 
 +==== 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 ​terminalpress the Super Key (the windows key on the keyboard) to open a search ​bar, type in "​terminal"​ and launch the program. 
 +Alternatively,​ you can use a keyboard shortcut ​for starting a terminal: ​"''​Ctrl + Alt + T''"​ 
 + 
 +==== 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 "​Ctrl-C -- Ctrl-V"​ you have "​Ctrl-K -- Ctrl-Y",​ which stands for "​killing"​ and "​yanking"​. 
 +In a Linux terminal, "​Ctrl-C -- Ctrl-V"​ also don't exist, but "​Ctrl-K -- Ctrl-Y"​ do. 
 + 
 +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 google hangouts chat, with a music plugin you can listen to radio through Emacs, etc. 
 + 
 +We are going to use Emacs as an Interactive Development Environment (IDE) for programming our robot. 
 + 
 +=== Launching Emacs === 
 + 
 +To launch Emacs with the IDE plugin, type into a terminal 
 +<​code>​ 
 +$ roslisp_repl & 
 +</​code>​ 
 + 
 +To see useful key shortcuts for your Emacs IDE, [[http://​cram-system.org/​doc/​ide?&#​key_bindings|follow this link]]. 
 + 
 + 
 +==== ROS ==== 
 + 
 +ROS stands for Robot Operating System. 
 +It is software that helps a robot programmer'​s life easier. 
 +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://​cram-system.org/​doc/​package/​why_lisp|go here]].
  
  
 ===== 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 which deals with the mathematics related to manipulation,​ a map of the static parts of the environment with the furniture, etc.+
  
-Let's load all these into memory and running ​by running a script called the launch file. Open a new terminal and run the following command.+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 of the environment with all the furniture,​ 
 +  * as well as a 3D description of the robot itself. 
 + 
 +Let's load all these by running a script called the launch file. 
 +Open a new terminal and run the following command:
 <​code>​ <​code>​
-$ roslaunch ​cram_bullet_world_tutorial ​world.launch+$ roslaunch ​cram_pick_place_tutorial ​world.launch
 </​code>​ </​code>​
 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 ​a new instance of the terminal and start the LISP REPL from the terminal ​by typing:+Open a new terminal ​window ​and start Emacs with the Lisp command line, if you don't already have it open. 
 +For that type in the terminal:
 <​code>​ <​code>​
-$ roslisp_repl+$ roslisp_repl ​&
 </​code>​ </​code>​
  
-An emacs window with the REPL will have launched. ​Now let's load the package which holds the pick and place tutorialType in the following two commands into the REPL :+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"​) 
 +</​code>​
  
 ''​**Note**''​ - ''"​CL-USER>"''​ is a part of your prompt and should not be typed in ''​**Note**''​ - ''"​CL-USER>"''​ is a part of your prompt and should not be typed in
 +
 +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>
-CL-USER> (ros-load:​load-system "cram_bullet_world_tutorial" :cram-bullet-world-tutorial) +CL-USER> (ros-load:​load-system "cram_pick_place_tutorial" :cram-pick-place-tutorial) 
-CL-USER> (in-package :cram-bullet-world-tutorial)+CL-USER> (in-package :cram-pick-place-tutorial)
 </​code>​ </​code>​
  
-Your prompt will now change to ''​BTW-TUT>''​ which indicates that you have loaded ​and are now working inside ​the package called "​cram_bullet_world_tutorial"​.+To paste something from, e.g., a browser, into Emacs, press "​Ctrl ​Y". 
 +Alternatively,​ in Linux you can select any piece of text in any application ​and paste it by pressing ​the middle click.
  
-==== Launching ​the Bullet World ====+Your prompt will now change to ''​PP-TUT>''​ which indicates that you have loaded and are now working inside ​the package called "​cram-pick-place-tutorial"​.
  
-Now to launch a simulation ​of the map and the robot, type the following command into the REPL:+==== Launching the simulator ==== 
 + 
 +Now to launch a simulation ​with a kitchen environment ​and our robot, type the following command into your Lisp command line:
 <code lisp> <code lisp>
-BTW-TUT> (roslisp-utilities:​startup-ros)+PP-TUT> (roslisp-utilities:​startup-ros)
 </​code>​ </​code>​
  
-You will get a pop-up window ​which will load a kitchen environment ​with robot inside, called ​the Bullet ​worldThis robot is a simulation of a robot called ​PR2, and we're going to use it to perform various tasks inside the bullet world.+You will get a pop-up window with the environment and the robot inside
 +We call this environment ​the Bullet ​World. 
 +The robot we have there is a simulation of the real-life ​PR2 robot, 
 +and we're going to use it to perform various tasks inside the Bullet World. 
 +{{ :​tutorials:​demo:​simulator_launch.png?600 |}}
  
 +If you get an error saying "''​Could not contact master at ...''",​ it's because you forgot to [[tutorials/​demo/​fetch_and_place#​robot_environment_setup|start the launch file]].
 +Once you've done that, press "​q"​ to exit the debugger and try again.
  
-At any point during the tutorial, if you want to clean/reset the bullet world, enter the following command into the REPL:+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>
-BTW-TUT> (init-projection)+PP-TUT> (init-projection)
 </​code>​ </​code>​
  
-NowLet'​s ​define a method ​to spawn bottle into the world and apply the physics using the Bullet physics engine and simulate ​the world for 10 seconds:+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'​t do anything, but you can zoom in and zoom out by scrolling. 
 +==== Moving Around ==== 
 + 
 +Firstlet'​s ​try moving our PR2 robot around the kitchen. Before that let's try to create ​pose, which can be given as the target destination for the robot. Execute ​the following function in the Lisp command line: 
 <code lisp> <code lisp>
-BTW-TUT> +PP-TUT> (make-pose "​map"​ '((0 1 0) (0 0 0 1)))
-(defun spawn-bottle (spawn-pose+
-  ​(unless ​(assoc :bottle btr::​*mesh-files*) +
-    ​(add-objects-to-mesh-list)) +
-  (btr-utils:​spawn-object '​bottle-1 :bottle :color '​(1 ​0 0) :pose spawn-pose) +
-  (btr:​simulate btr:​*current-bullet-world* 10))+
 </​code>​ </​code>​
 +The ''​make-pose''​ function creates the pose and returns it. The "​map"​ is the coordinate frame in which the pose is considered. The argument is a list of two sublists - the first one is the x y z position and the second one is the quaternion of the orientation [( (x y z) (x y z w) )].
 +
 +The screenshot below shows axes where the "​map"​ coordinate frame is located. Red is the +ve x-axis, Green is the +ve y-axis and Blue is the +ve z-axis. This is the pose that is referred by '( (0 0 0) (0 0 0 1) ) in "​map"​. All the poses that are in reference to the "​map"​ is relative to this pose.
 +{{ :​tutorials:​demo:​map_coordinate.png?​600 |}}
 +
 +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 "​Inspect"​. That shows the guts of the object.
 +Press "​q"​ to exist the inspector tab.
  
-Now let'​s ​run the method in the REPL with some arguments and see what happens.+Let'​s ​use this to move around our robot:
 <code lisp> <code lisp>
-BTW-TUT> (spawn-bottle '((-1.6 -0.9 0.82) (0 0 0 1)))+PP-TUT> 
 +(with-simulated-robot 
 +  ​(let ((?​navigation-goal (make-pose "​map"​ '((0) (0 0 0 1))))) 
 +    (perform (an action 
 +                 (type going) 
 +                 ​(target (a location  
 +                            (pose ?​navigation-goal)))))))
 </​code>​ </​code>​
 +To go onto a new line without executing, press "​Ctrl-J"​.
  
 +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 "​map"​ reference frame. Try to move around different locations on the map by changing the poses. You'll notice that you will get an error of type
 +''​CRAM-COMMON-FAILURES:​NAVIGATION-POSE-UNREACHABLE''​ if you try to move the robot to positions, which are not valid (due to collisions).
  
 +Note:- Notice how we're using ''​with-simulated-robot''​ to enclose the code to perform actions on the robot. This method tells CRAM that our code needs to be run on the simulation and not on the actual PR2 itself.
 +
 +Note:- //The ''​let''​ keyword in Lisp is a handy way to define local variables within its scope. Eg://
 <code lisp> <code lisp>
-BTW-TUT> +PP-TUT> (let ((a 1) 
-(defparameter *final-object-destination* +               ​(2)
-  ​(cl-transforms-stamped:​make-pose-stamped +          (print a) 
-   "​map"​ 0.0 +          b) 
-   (cl-transforms:​make-3d-vector -0.8 0.9+
-   ​(cl-transforms:​make-identity-rotation)))+
 +PP-TUT> (print b) 
 +The variable B is unbound. 
 +</​code>​ 
 +//To exit the debugger, press "​q"​.//​ 
 +//The variables a and b have been declared with values under the scope of let and as soon as the body of ''​let''​ was over, the scope of these variables ended as well.//
  
 +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 variable.
 +<code lisp>
 +PP-TUT>
 (defparameter *base-pose-near-table* (defparameter *base-pose-near-table*
-  (cl-transforms-stamped:​make-pose-stamped +  (make-pose "​map"​ '((-1.447d0 -0.150d0 0.0d0) (0.0d0 0.0d0 -0.7071067811865475d0 0.7071067811865476d0)))) 
-   ​"map" 0.0 + 
-   (cl-transforms:make-3d-vector ​-1.447d0 -0.150d0 0.0d0+(with-simulated-robot 
-   ​(cl-transforms:axis-angle->quaternion ​(cl-transforms:make-3d-vector ​0 0 1) (/ pi -2)))) +  (let ((?​navigation-goal *base-pose-near-table*)) 
-   ​+    ​(perform (an action 
 +                 (type going) 
 +                 ​(target (a location  
 +                            (pose ?​navigation-goal))))))) 
 +</​code>​ 
 + 
 +Don't be surprised if you occasionally see "d0" ​at the end of the numbers, e.g., ''​-0.150d0''​. This simply means that the number has a double precision. you can just as well write ''​-0.15'',​ if you don't require very high precision, which we don't in this tutorial. 
 + 
 +{{ :​tutorials:​demo:​moving_around.png?​600 |}} 
 + 
 +In Lisp, "'';''"​ is the symbol for commentaries. Traditionally,​ one uses "'';''"​ for inline comments, and "'';;''"​ for comments that take a full line. 
 + 
 +To go up in the history of your Lisp command line, press "Ctrl-Up" while your cursor is at the prompt. You can scroll through the history by pressing "​Ctrl-Up"​ repeatedly. 
 + 
 +To clean all the input that is currently sitting in the prompt, instead of pressing "​Backspace"​ for many many seconds, try "​Alt-Ctrl-Backspace"​. 
 +==== Spawning Objects ==== 
 + 
 +Now let's try to spawn a new object into the world: 
 + 
 +<code lisp> 
 +PP-TUT> (spawn-object '((-1.-0.0.82) (0 0 0 1))
 +</​code>​ 
 +{{ :​tutorials:​demo:​bottle_spawn.png?​600 |}} 
 + 
 +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 from our library, like a cube, you can provide it as an argument.  
 +Eg: 
 +<code lisp> 
 +PP-TUT> ​(spawn-object '​((1.44 1.28 0.85) (0 0 0 1)) :cube 'cube-1) 
 +</​code>​ 
 +Here, '':​cube''​ is the type of the object and '''​cube-1''​ is its name. 
 + 
 +To see a list of all available objects, call: 
 +<code lisp> 
 +PP-TUT> (list-available-objects) 
 +</​code>​ 
 + 
 +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 '​wurst-1 '(0 1 0)) 
 +</code> 
 +This should have spawned a green sausage on the floor near the origin of the "​map"​ coordinate frame. 
 +{{ :​tutorials:​demo:​weisswurst.png?​600 |}} 
 + 
 +==== Perceiving Objects ==== 
 + 
 +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'​s cameras. There are some actions that can be done for this 
 +  * 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 neck such that the camera is directed towards where you expect the object to be. 
 + 
 +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 ''​cpl:​par''​. Even navigation can be bundled together with this. So try this: 
 +<code lisp> 
 +PP-TUT>​ 
 +(with-simulated-robot 
 +  (let ((?​navigation-goal *base-pose-near-table*)) 
 +    (cpl:par 
 +      ;; Moving the robot near the table. 
 +      (perform (an action 
 +                   (type going) 
 +                   ​(target (a location  
 +                              (pose ?​navigation-goal))))) 
 +      ;; Increasing the height of the torso by setting the joint angle to 0.3 meters  
 +      (perform (a motion  
 +                  (type moving-torso) 
 +                  (joint-angle 0.3))) 
 +      ;; Parking the arms                 
 +      (park-arms)))) 
 +</​code>​ 
 +The robot will do all the said actions in parallel. 
 + 
 +{{ :​tutorials:​demo:​perception-preparation.png?​600 |}} 
 + 
 +Note how until now we were performing ''​action''​-s,​ and ''​moving-torso''​ is a motion. 
 +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 ''​moving-torso''​ is a motion, and everything else is an action. 
 + 
 +If you get a navigation pose unreachable error, it might be that there are objects on the floor. 
 +To clean up the environment,​ you can always call ''​(init-projection)''​. 
 + 
 +Now the next thing to do is to look towards the direction where the bottle will be. 
 +We have found the corresponding coordinates for you and now let's save them in a variable and use them.  
 +<code lisp> 
 +PP-TUT>
 (defparameter *downward-look-coordinate* (defparameter *downward-look-coordinate*
-  (cl-transforms-stamped:​make-pose-stamped +  (make-pose "​base_footprint" ​'((0.65335d0 ​0.076d0 0.758d0) (0 0 0 1)))) 
-   "​base_footprint"​ 0.0 +;; This coordinate frame has base_footprint as reference, which is the reference 
-   ​(cl-transforms:make-3d-vector 0.65335d0 0.076d0 0.758d0+;; frame of PR2. 
-   (cl-transforms:make-identity-rotation)))+ 
 +(with-simulated-robot 
 +  (let ((?​looking-direction *downward-look-coordinate*)) 
 +    (perform (an action 
 +                 (type looking) 
 +                 ​(target (a location  
 +                            (pose ?​looking-direction))))))) 
 +</​code>​ 
 +Notice that the robot now tilts it's head downwards, somewhat in the direction of where the bottle is at. 
 + 
 +{{ :tutorials:​demo:​looking_down.png?​600 |}} 
 + 
 +''"​base_footprint"''​ is a coordinate frame attached to the robot, at the very bottom of its base, on the floor, in the middle of the base square. Whenever the robot moves, the ''"​base_footprint"''​ moves with it. 
 + 
 + 
 +Now the only thing remaining is to detect, or "​perceive"​ the bottle. We use an action called "​detecting"​ to do this: 
 + 
 +<code lisp> 
 +PP-TUT> 
 +(with-simulated-robot 
 +  (perform (an action 
 +               (type detecting) 
 +               ​(object (an object  
 +                           (type bottle)))))) 
 +</​code>​ 
 + 
 +If you get an error that an object could not be perceived, the robot is probably not looking at an object of type ''​bottle''​. 
 +So press "​q"​ to exit the debugger and tweak the location of the base or the direction of the gaze of the robot and try to perceive again. 
 + 
 +After the object has been perceived successfully,​ our method returns a description of the bottle with its complete information. 
 +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))))))) 
 +</​code>​ 
 + 
 +''​setf''​ is used to assign a value to a variable. 
 + 
 +==== Picking up Objects ==== 
 +Once the object has been found, picking up is very straightforward:​ 
 +<code lisp> 
 +PP-TUT>​ 
 +(with-simulated-robot  
 +  (let ((?​perceived-bottle *perceived-bottle*)) 
 +    (perform (an action 
 +                 (type picking-up) 
 +                 (arm right) 
 +                 ​(grasp left-side) 
 +                 ​(object ?​perceived-bottle)))) 
 +   ;; Parking the right arm after grasping, bringing the bottle close to the robot 
 +   ;; and freeing up the field of view. 
 +   (park-arm :right)) 
 +</​code>​ 
 +You will notice that we have provided in our action description information like which arm to pick up with (''​right''​),​ which side the robot should be grasping from (''​left-side''​) and which object to pick up (''?​perceived-bottle''​). 
 + 
 +By now, the robot should have successfully picked up the bottle with its right arm. 
 +{{ :​tutorials:​demo:​picking_up.png?​600 |}} 
 + 
 +==== 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 - drive with the base to a location from where it can place the bottle on the dining table, and perform the placing action itself.  
 + 
 +So before this, let's define two more locations again, which are the destination of the bottle and the position of the robot to be able to reach it respectively:​ 
 +<code lisp> 
 +PP-TUT>​ 
 +(defparameter *final-object-destination* 
 +  (make-pose "​map"​ '((-0.8 2 0.9) (0 0 0 1))))
  
 (defparameter *base-pose-near-counter* (defparameter *base-pose-near-counter*
-  (cl-transforms-stamped:​make-pose-stamped +  (make-pose "​base_footprint" ​'((-0.150d0 2.0d0 0.0d0) (0.0d0 0.0d0 -1.0d0 0.0d0))))
-   "​base_footprint" ​0.0 +
-   (cl-transforms:​make-3d-vector ​-0.150d0 2.0d0 0.0d0) +
-   (cl-transforms:​make-quaternion ​0.0d0 0.0d0 -1.0d0 0.0d0)))+
 </​code>​ </​code>​
 +
 +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>
 +(with-simulated-robot ​
 +  ;; Moving the robot near the counter.
 +  (let ((?nav-goal *base-pose-near-counter*))
 +    (perform (an action
 +                 (type going)
 +                 ​(target (a location ​
 +                            (pose ?​nav-goal))))))
 +  ;; Setting the bottle down on the counter
 +  (let ((?​drop-pose *final-object-destination*)
 +        (?​perceived-bottle *perceived-bottle*))
 +    (perform (an action
 +                 (type placing)
 +                 (arm right)
 +                 ​(object ?​perceived-bottle)
 +                 ​(target (a location ​
 +                            (pose ?​drop-pose))))))
 +  (park-arm :right))
 +</​code>​
 +The robot has finally placed the object in the destination. ​
 +{{ :​tutorials:​demo:​placed_at_dest.png?​600 |}}
 +
 +==== 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, ''​move-bottle''​ which can easily be reused:
 +
 +<code lisp>
 +(defparameter *base-pose-near-table*
 +  (make-pose "​map"​ '​((-1.447 -0.15 0.0) (0.0 0.0 -0.7071 0.7071))))
 + 
 +(defparameter *downward-look-coordinate*
 +  (make-pose "​base_footprint"​ '​((0.65335 0.076 0.758) (0 0 0 1))))
 + 
 +(defparameter *base-pose-near-counter*
 +  (make-pose "​base_footprint"​ '​((-0.15 2 0) (0 0 -1 0))))
 + 
 +(defparameter *final-object-destination*
 +  (make-pose "​map"​ '​((-0.8 2 0.9) (0 0 0 1))))
 + 
 (defun move-bottle (bottle-spawn-pose) (defun move-bottle (bottle-spawn-pose)
-  (spawn-bottle ​bottle-spawn-pose) +  (spawn-object ​bottle-spawn-pose) 
-  (pr2-proj:with-simulated-robot+  (with-simulated-robot
     (let ((?​navigation-goal *base-pose-near-table*))     (let ((?​navigation-goal *base-pose-near-table*))
       (cpl:par       (cpl:par
-        (exe:​perform (desig:a motion ​ 
-                              (type moving-torso) 
-                              (joint-angle 0.3))) 
-        (pp-plans::​park-arms) 
         ;; Moving the robot near the table.         ;; Moving the robot near the table.
-        (exe:perform (desig:a motion +        (perform (an action 
-                              (type going) +                     ​(type going) 
-                              (target (desig:a location  +                     ​(target (a location  
-                                               ​(pose ?​navigation-goal)))))))+                                (pose ?​navigation-goal))))
 +        (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 ((?​looking-direction *downward-look-coordinate*))     (let ((?​looking-direction *downward-look-coordinate*))
-      (exe:perform (desig:a motion ​ +      (perform (an action 
-                            (type looking) +                   ​(type looking) 
-                            (target (desig:a location  +                   ​(target (a location  
-                                             ​(pose ?​looking-direction))))))+                              (pose ?​looking-direction))))))
     ;; Detect the bottle on the table.     ;; Detect the bottle on the table.
     (let ((?​grasping-arm :right)     (let ((?​grasping-arm :right)
-          (?​perceived-bottle (exe:perform (desig:a motion +          (?​perceived-bottle (perform (an action 
-                                                   ​(type detecting) +                                          (type detecting) 
-                                                   ​(object (desig:an object  +                                          (object (an object  
-                                                                     ​(type :bottle)))))))+                                                      (type bottle)))))))
       ;; Pick up the bottle       ;; Pick up the bottle
-      (exe:perform (desig:an action +      (perform (an action 
-                             ​(type picking-up) +                   ​(type picking-up) 
-                             ​(arm ?​grasping-arm) +                   ​(arm ?​grasping-arm) 
-                             ​(grasp left-side) +                   ​(grasp left-side) 
-                             ​(object ?​perceived-bottle))) +                   ​(object ?​perceived-bottle))) 
-      (pp-plans::park-arms :arm ?​grasping-arm)+      (park-arm ?​grasping-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) +                     ​(type going) 
-                              (target (desig:a location  +                     ​(target (a location  
-                                               ​(pose ?​nav-goal)))))) +                                (pose ?​nav-goal))))))
-      (coe:​on-event (make-instance '​cpoe:​robot-state-changed))+
       ;; Setting the bottle down on the counter       ;; Setting the bottle down on the counter
       (let ((?​drop-pose *final-object-destination*))       (let ((?​drop-pose *final-object-destination*))
-        (exe:perform (desig:an action +        (perform (an action 
-                               ​(type placing) +                     ​(type placing) 
-                               ​(arm ?​grasping-arm) +                     ​(arm ?​grasping-arm) 
-                               ​(object ?​perceived-bottle) +                     ​(object ?​perceived-bottle) 
-                               ​(target (desig:a location  +                     ​(target (a location  
-                                                (pose ?​drop-pose)))))) +                                (pose ?​drop-pose)))))) 
-      (pp-plans::park-arms :arm ?​grasping-arm))))+      (park-arm ?​grasping-arm))))
 </​code>​ </​code>​
-Move bottle accepts the position the bottle will spawn in and then move it to the final destination,​ which is the counter. + 
-Now run ''​move-bottle''​ with the arguments ​we initially used to spawn the bottle.+This defines a function called ''​move-bottle''​ with the Lisp command ''​defun''​. 
 +This function ​accepts ​one argument, ''​bottle-spawn-pose'',​ which is the position the bottle will be spawned ​in
 +After spawning the object, ''​move-bottle''​ commands the robot to pick the bottle and place it at its final destination,​ which is the counter. 
 + 
 +Nowrun ''​move-bottle''​ with the coordinates ​we initially used to spawn the bottle:
 <code lisp> <code lisp>
-BTW-TUT> (move-bottle '​((-1.6 -0.9 0.82) (0 0 0 1)))+PP-TUT> (move-bottle '​((-1.6 -0.9 0.82) (0 0 0 1)))
 </​code>​ </​code>​
 +
 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 ''​​(init-projection)''​​.]//​
 +
 +Congratulations! You have just written your first rudimentary plan to pick and place a bottle.
  
 ===== Recovering from Failures ===== ===== Recovering from Failures =====
  
-The previous example worked perfectlybecause 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,​ far from being precise compared to a factory floor. What would happen if the bottle was moved a little bit to the right of its previous spawn position? For this, let's change the arguments to reflect this on our method, and let's see what happens.+The previous example worked perfectly because we knew and provided ​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, ​which is far from being precise compared to a factory floor. What would happen if the bottle was moved a little bit to the right of its previous spawn position? For this, let's change the arguments to reflect this on our method, and let's see what happens.
  
 <code lisp> <code lisp>
-BTW-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)))
 </​code>​ </​code>​
-And the output will look like this.+Now, the output will look like this. 
 <code lisp> <code lisp>
-BTW-TUT>+PP-TUT>
 ; Evaluation aborted on #<​CRAM-COMMON-FAILURES:​PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>​. ; Evaluation aborted on #<​CRAM-COMMON-FAILURES:​PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>​.
 </​code>​ </​code>​
 +
 +Press "​q"​ to exit the debugger. If that doesn'​t help, make sure you're in the debugger window, for that just click on the debugger tab to select it.
  
 {{:​tutorials:​intermediate:​btw-tut-cant-see-bottle.png?​800|}} {{:​tutorials:​intermediate:​btw-tut-cant-see-bottle.png?​800|}}
  
-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. ​+Clearlythe 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:​beginner:​failure_handling|Failure Handling]]. +We're going to add some code into fixing ​the 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.
-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 ​some additional parameters to aid us:+Let's define ​two additional parameters to aid us:
 <code lisp> <code lisp>
-BTW-TUT>+PP-TUT>
 (defparameter *left-downward-look-coordinate* (defparameter *left-downward-look-coordinate*
-  (cl-transforms-stamped:​make-pose-stamped +  (make-pose "​base_footprint" ​'((0.65335 0.76 0.758) (0 0 0 1))))
-   "​base_footprint" ​0.0 +
-   (cl-transforms:​make-3d-vector ​0.65335d0 ​0.76d0 0.758d0) +
-   (cl-transforms:​make-identity-rotation)))+
  
 (defparameter *right-downward-look-coordinate* (defparameter *right-downward-look-coordinate*
-  (cl-transforms-stamped:​make-pose-stamped +  (make-pose "​base_footprint" ​'((0.65335 -0.76 0.758) (0 0 0 1))))
-   "​base_footprint" ​0.0 +
-   (cl-transforms:​make-3d-vector ​0.65335d0 ​-0.76d0 0.758d0) +
-   (cl-transforms:​make-identity-rotation))) +
-   +
 </​code>​ </​code>​
-We defined two coordinates ''​*left-downward-look-coordinate*''​ and ''​*right-downward-look-coordinate*''​ with respective to our robot base footprint as alternative directions to look for when downward look fails. 
  
-Now we define a method ''​find-object''​ which encapsulates our plan with failure handling :+We defined two coordinates,​ ''​*left-downward-look-coordinate*''​ and ''​*right-downward-look-coordinate*'',​ in the coordinate frame of our robot'​s base. 
 +These will be alternative directions (left and right respectively) to look at in the case when looking downward fails. 
 + 
 +Now we define a method ''​find-object''​ which encapsulates our plan with failure handling:
 <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 (?​object-type) (defun find-object (?​object-type)
   (let* ((possible-look-directions `(,​*downward-look-coordinate*   (let* ((possible-look-directions `(,​*downward-look-coordinate*
Line 268: Line 575:
                                      ,​*right-downward-look-coordinate*))                                      ,​*right-downward-look-coordinate*))
          ​(?​looking-direction (first possible-look-directions)))          ​(?​looking-direction (first possible-look-directions)))
-    (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  +                 ​(type looking) 
-                                           ​(pose ?​looking-direction))))) +                 ​(target (a location  
-  +                            (pose ?​looking-direction))))) 
-    ​(cpl:with-failure-handling +     
-        ((cram-common-failures:​perception-object-not-found (e+    ​;; perception-object-not-found is the error that we get when the robot cannot find the object. 
-           ​;; Try different look directions until there is none left. +    ;; Now we're wrapping it in a failure ​handling ​clause to handle it 
-           ​(when possible-look-directions +    (handle-failure ​perception-object-not-found 
-             ​(roslisp:​ros-warn (perception-failure) ​"~a~%Turning head." ​e+        ;; Try the action 
-             ​(exe:perform (desig:a motion ​ +        ​((perform (an action 
-                                   ​(type looking)  +                      (type detecting
-                                   ​(direction forward))) +                      ​(object (an object  
-             ​(setf ?​looking-direction (first possible-look-directions)) +                                  (type ?​object-type)))))) 
-             ​(setf possible-look-directions (cdr possible-look-directions)) +       
-             ​(exe:perform (desig:a motion  +      ​;; If the action fails, try the following:​ 
-                                   ​(type looking) +      ;; try different look directions until there is none left. 
-                                   ​(target (desig:a location +      (when possible-look-directions 
-                                                    (pose ?​looking-direction))))) +        (print "Perception error happened! ​Turning head."​) 
-             ​(cpl:​retry)) +        ;; Resetting the head to look forward before turning again 
-           ​(cpl:fail 'common-fail:​looking-high-level-failure))) +        ​(perform (an action 
-  +                     ​(type looking)  
-      (let ((?​perceived-bottle +                     ​(direction forward))) 
-              (exe:​perform (desig:a motion +        (setf ?​looking-direction (first possible-look-directions)) 
-                                    (type detecting) +        (setf possible-look-directions (rest possible-look-directions)) 
-                                    (object (desig:an object  +        (perform (a motion  
-                                                      (type ?object-type))))))) +                    (type looking) 
-        (values ?perceived-bottle (get-preferred-arm-for-direction ?​looking-direction)))))) +                    (target (a location 
-        </​code>​ +                               ​(pose ?​looking-direction))))) 
-Let's see what this method does. The ''​with-failure-handling''​ clause here deals with ''​cram-common-failures:​perception-object-not-found''​ which if you remember, was the error raised when the robot couldn'​t find the bottle. So instead of only looking downwards, the code will now iterate between downwards, left and right, and only upon a failure in all these, will an error be bubbled up - thus increasing our effective field of view.  +        ;; This statement retries the action again 
-Also note that the ''​find-object'' ​suggests an arm to use, depending on the direction it looked+        ​(cpl:​retry)) 
-Let us also update ​our ''​move-bottle''​ to use this method.+      ;; If everything else fails, error out 
 +      ​(cpl:fail '​object-nowhere-to-be-found)))) 
 +</​code>​ 
 + 
 +Let's see what this method does. The ''​handle-failure''​ clause here deals with ''​perception-object-not-found''​whichif you remember, was the error raised when the robot couldn'​t find the bottle. So instead of only looking downwards, the code will now iterate between downwards, left and right, and only upon a failure in all these, will an error be bubbled up - thus increasing our effective field of view. You have to note that ''​cpl:​retry''​ in the failure handling part will trigger a retry of the main action, so it's advised to put it under some conditionals so that you won't end up in an endless loop. 
 + 
 +//​Let'​s try to understand the syntax of ''​handle-failure''​. The first argument is the type of error message you're trying ​to catch and work around. The second argument is the list of steps you want to protect from the said error message and the last part are the actions that you perform once you encounter the said error - ''​cpl:​retry''​ is called within this blockThis looks like the following (Elements under the tag <> are to be replaced with real programming elements)://​ 
 +<code lisp> 
 +(handle-failure <the error type that needs to be handled>​ 
 +    (<all the actions to be performed under normal execution>​) 
 +  (<​actions that need to be performed if there is an error of the declared type. Call retry if necessary>​)) 
 +     
 +</​code>​ 
 + 
 +Now we need to redefine ​our ''​move-bottle''​ to use this method
 <code lisp> <code lisp>
 (defun move-bottle (bottle-spawn-pose) (defun move-bottle (bottle-spawn-pose)
-  (spawn-bottle ​bottle-spawn-pose) +  (spawn-object ​bottle-spawn-pose) 
-  (pr2-proj:with-simulated-robot+  (with-simulated-robot
     (let ((?​navigation-goal *base-pose-near-table*))     (let ((?​navigation-goal *base-pose-near-table*))
       (cpl:par       (cpl:par
-        (exe:​perform (desig:a motion ​ 
-                              (type moving-torso) 
-                              (joint-angle 0.3))) 
-        (pp-plans::​park-arms) 
         ;; Moving the robot near the table.         ;; Moving the robot near the table.
-        (exe:perform (desig:a motion +        (perform (an action 
-                              (type going) +                     ​(type going) 
-                              (target (desig:a location  +                     ​(target (a location  
-                                               ​(pose ?​navigation-goal))))))) +                                (pose ?​navigation-goal))))
-    ;; Find and detect the bottle on the table. +        (perform (a motion  
-    (multiple-value-bind ​(?​perceived-bottle ​?​grasping-arm)  +                    (type moving-torso) 
-        ​(find-object :bottle) +                    (joint-angle 0.3))) 
-      (exe:perform (desig:an action +        (park-arms))) 
-                             ​(type picking-up) +    ;; Find and detect the bottle on the table. ​We use the new method here 
-                             ​(arm ?​grasping-arm) +    (let ((?​perceived-bottle (find-object :bottle)
-                             ​(grasp left-side) +          (?​grasping-arm ​:right)) 
-                             ​(object ?​perceived-bottle))) +      (perform (an action 
-      (pp-plans::park-arms :arm ?​grasping-arm)+                   ​(type picking-up) 
 +                   ​(arm ?​grasping-arm) 
 +                   ​(grasp left-side) 
 +                   ​(object ?​perceived-bottle))) 
 +      (park-arm ?​grasping-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) +                     ​(type going) 
-                              (target (desig:a location  +                     ​(target (a location  
-                                               ​(pose ?​nav-goal)))))) +                                (pose ?​nav-goal))))))
- +
-      (coe:​on-event (make-instance '​cpoe:​robot-state-changed))+
       ;; Setting the object down on the counter       ;; Setting the object down on the counter
       (let ((?​drop-pose *final-object-destination*))       (let ((?​drop-pose *final-object-destination*))
-        (exe:perform (desig:an action +        (perform (an action 
-                               ​(type placing) +                     ​(type placing) 
-                               ​(arm ?​grasping-arm) +                     ​(arm ?​grasping-arm) 
-                               ​(object ?​perceived-bottle) +                     ​(object ?​perceived-bottle) 
-                               ​(target (desig:a location  +                     ​(target (a location  
-                                                (pose ?​drop-pose)))))) +                                (pose ?​drop-pose)))))) 
-      (pp-plans::park-arms :arm ?​grasping-arm))))+      (park-arm ?​grasping-arm)))) 
 +</​code>​ 
 +Now run: 
 +<code lisp> 
 +PP-TUT> (move-bottle '((-2 -0.9 0.860) (0 0 0 1)))
 </​code>​ </​code>​
  
-Run ''​move-bottle'' ​again, and this time, you'll find that the robot succeeds in transporting the bottle.+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,​ that's because the Inverse Kinematics solver is not deterministic and sometimes finds a solution and sometimes does not, so simply press "​q"​ to exit the debugger and try again. 
 {{:​tutorials:​intermediate:​btw-tut-found-bottle-again.png?​800|}} {{:​tutorials:​intermediate:​btw-tut-found-bottle-again.png?​800|}}
  
-===== 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'​s front, back, left, right, etc. One might think that since the bottle is a rotationally symmetric object, it doesn'​t matter which side you approach from. But consider the bottle as an object model, which has a specific front, back and sides according to its orientation with respect to the bullet world. In which case, the side with which the arm approaches the bottle greatly matters, accounting for the configuration of the joints the robot arm will make while trying to grasp - some poses may be unachievable,​ while others may result in the collision of the arm with the table.+
  
-Let's try to visualize ​this issue, by spawning the bottle in yet another position:+Everything is good so far, even though by design, 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'​s front, back, left, right, etc. So what decides the "​side"​ (left, right, top, bottom) of the object? As you might have guessed, these are defined for objects according to the coordinate reference frame of each object. Every object spawned in the bullet world has its own coordinate axes - even rotationally symmetric objects like the bottle here. (Red:+ve x, Green: +ve y, Blue: +ve z axes)  
 + 
 +{{:​tutorials:​demo:​bottle_axes.png?​600|}} 
 +  * +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'​t matter which side you approach from. But consider the bottle as a generic object, which does not necessarily have to be symmetric. Each object is represented as a 3D model, which has a specific front, back, and sides according to the coordinate frame that model is defined in. In which case, the side with which the arm approaches the bottle greatly matters, accounting for the configuration of the joints the robot arm will make while trying to grasp - some poses may be unachievable,​ while others may result in the collision of the arm with the table. 
 + 
 + 
 +Let's try to create ​this issue in our scenario, by spawning the bottle in yet another position:
 <code lisp> <code lisp>
-BTW-TUT> (spawn-bottle ​'((-1.-0.75 0.860) (0 0 0 1)))+PP-TUT> (spawn-object ​'((-1.-0.75 0.860) (0 0 0 1)))
 </​code>​ </​code>​
-Now clean up and run ''​move-bottle''​ once more, and the output should be something similar to as below:+Now run ''​move-bottle''​ once more, and the output should be something similar to the following:
 <code lisp> <code lisp>
-BTW-TUT> (move-bottle '((-1.-0.75 0.860) (0 0 0 1))) +PP-TUT> (move-bottle '((-1.-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) ​WARN1550502686.797: #<​POSE-STAMPED ​+[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ​ERROR1566442996.818: #<​POSE-STAMPED ​
    ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0    ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0
-   #<​3D-VECTOR (0.6819813024319948d0 ​0.4206671881870251d0 ​-0.11278482277792945d0)> +   #<​3D-VECTOR (0.4865934453898557d0 ​0.39450056642817605d0 ​-0.2601495772666721d0)> 
-   #<​QUATERNION (-0.005172943672216379d0 ​0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>>​ is unreachable for EE. +   #<​QUATERNION (-0.001459550578147173d0 ​0.006997527088969946d0 ​0.11667527258396149d0 ​0.9931443929672241d0)>> is unreachable for EE.
-Ignoring. +
-[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1550502687.092:​ #<​POSE-STAMPED  +
-   ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0 +
-   #<​3D-VECTOR (0.6797228574484719d0 0.4210222500057509d0 -0.2627674055849005d0)>​ +
-   #<​QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 ​0.6199836767360266d0)>> is unreachable for EE.+
 Failing. Failing.
-[(PP-PLANS PICK-UP) WARN] 1550502687.092: Manipulation messed up: #<​POSE-STAMPED ​+[(PP-PLANS PICK-UP) WARN] 1566442996.818: Manipulation messed up: #<​POSE-STAMPED ​
    ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0    ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0
-   #<​3D-VECTOR (0.6797228574484719d0 ​0.4210222500057509d0 ​-0.2627674055849005d0)> +   #<​3D-VECTOR (0.4865934453898557d0 ​0.39450056642817605d0 ​-0.2601495772666721d0)> 
-   #<​QUATERNION (-0.005172943672216379d0 ​0.0055962335340426494d0 -0.7845776913102387d0 ​0.6199836767360266d0)>> is unreachable for EE.+   #<​QUATERNION (-0.001459550578147173d0 ​0.006997527088969946d0 ​0.11667527258396149d0 ​0.9931443929672241d0)>> is unreachable for EE.
 Ignoring. Ignoring.
-; Evaluation aborted on #<CRAM-COMMON-FAILURES:​MANIPULATION-POSE-UNREACHABLE ​{10100E64C3}>.+[(PICK-PLACE PICK-UP) INFO] 1566442996.818:​ Gripping 
 +[(PICK-AND-PLACE GRIP) WARN] 1566442996.857:​ There was no object to grip 
 +Retrying 
 +[(PICK-AND-PLACE GRIP) WARN] 1566442996.868:​ No retries left. Propagating up. 
 +; Evaluation aborted on #<GRIPPER-CLOSED-COMPLETELY ​{100C2C57A3}>.
 </​code>​ </​code>​
  
 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.
  
-So what went wrong? ​If you go back to definition of ''​get-preferred-arm-for-direction''​+So what went wrong? ​For this, we have to think back to the definition of our ''​?grasping-arm''​. Notice that we always use the right arm to grasp the object, with the hardcoded ​grasp from the left side of the object. But if you look at the position of the bottle now (refer to the figure below), hardcoding these might not be the best idea at all. After all, we have a perfectly functioning left arm and a bunch of grasp configurations to try.
-<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)) +
-</​code>​ +
-And the part where we pick up the object: +
-<code lisp> +
-        (exe:​perform (desig:an action +
-                               (type picking-up) +
-                               (arm ?​grasping-arm) +
-                               (grasp left-side) +
-                               ​(object ?​perceived-bottle))) +
-</​code>​ +
-We see that the robot defaults the right arm when the object ​is in the center and will always try to grasp the left side of the bottle, even when the object ​is slightly favoring ​the left side under it (Refer the figure below). ​+
  
-Note''​We have been using left-side here a lot. In the context of (grasp left-side), it refers to the left-side of the bottleBut what defines the "​left-side"​?''​+{{:tutorials:​intermediate:​btw-tut-grasp-config-fail.png?800|}}
  
-''​Before we understand it, we have to acknowledge that every object spawned in the bullet world has its own coordinate axes - even rotationally symmetric objects like the bottle hereAnd the definition ​for left-side is the side of the -ve Y-axis in the bottle coordinate frame. Similarly right-side is +ve Y-axis, front is denoted by +ve X-axis and the back by -ve X-axis. ''​ +Not all objects ​have the same possible graspsHow do we know what grasps are defined in our system ​for specific objects? Try this function:
-''​With ​this the "​left-side"​ grasp can change based on how the object is oriented in the world. But it remains the w.r.t the object coordinate frame.''​+
  
-''​The file at <your workspace>/cram/​cram_common/​cram_object_knowledge/​src/​household.lisp holds some definitions for grasps supported by the household objects supporte in CRAM.''​+<code lisp> 
 +PP-TUT> (list-defined-grasps :bottle) 
 +</code>
  
-{{:​tutorials:​intermediate:​btw-tut-grasp-config-fail.png?​800|}} +To figure out a good failure handling strategy, let'​s ​once again come up with a plan, formulated in natural language:
- +
-Once again, let'​s ​formulate ​strategy like the previous case here. The plan we are going for will look something like this:+
   - 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 420: Line 737:
 Let's encapsulate all this in a method called ''​pick-up-object'':​ Let's encapsulate all this in a method called ''​pick-up-object'':​
 <code lisp> <code lisp>
-(defun pick-up-object (?​perceived-object ​?​object-type ​?​grasping-arm) +(defun pick-up-object (?​perceived-object ?​grasping-arm) 
-  (let ((?​possible-arms '​(:​right :left))) +  (let((?​possible-grasps ​'(:​left-side ​:right-side :front :back)
-    ;;​Retry by changing the arm+         ​(?​grasp (first ?​possible-grasps)) 
 +         (?​possible-grasp (rest ?​possible-grasps))) 
 +          ​
     (cpl:​with-retry-counters ((arm-change-retry 1))     (cpl:​with-retry-counters ((arm-change-retry 1))
-        (cpl:with-failure-handling +      ;; Outer handle ​failure handling ​arm change 
-            ((common-fail:object-unreachable ​(e) +      (handle-failure ​object-unreachable 
-               (roslisp:​ros-warn (arm-failure) "​Manipulation failed: ~a~%" e) +          ;; Iner handle-failure ​handling grasp change 
-               ​(cpl:​do-retry arm-change-retry +          ((handle-failure ​(or manipulation-pose-unreachable gripper-closed-completely
-                 ​(setf ?grasping-arm (car (remove ?grasping-arm ?possible-arms))) +               ​;; Try to perform ​the pick up 
-                 ​(cpl:​retry)) +               ​((perform ​(an action 
-               ​(roslisp:​ros-warn (arm-failures) "No more retries left"​))+                             ​(type picking-up) 
-              +                             ​(arm ​?​grasping-arm) 
-          ​;; Retry by changing ​the grasp +                             ​(grasp ?grasp) 
-          (let* ((?​possible-grasp +                             ​(object ?perceived-object)))) 
-                   ​(cram-object-interfaces:​get-object-type-grasps ?​object-type nil nil nil ?​grasping-arm)+             ;; When pick-up fails this block gets executed 
-                 ​(?grasp (cut:​lazy-car ​?possible-grasp))+             (format t "Grasp failed! Error: ​~a~%Grasp: ​~a~%Arm: ​~a~%
-            (cpl:with-retry-counters ((grasp-retries 3)) +                     ​e ?grasp ?​grasping-arm) 
-              (cpl:​with-failure-handling +             ;; Checks if we have any possible grasps left. 
-                  (((or cram-common-failures:​manipulation-pose-unreachable +             ;; If yes, then the block nested to it gets executed, which will 
-                        cram-common-failures:​gripper-closed-completely(e+             ;; set the grasp that is used to the new value and trigger ​retry 
-                     (roslisp:​ros-warn (grasp-failure) +             ​(when (first ?​possible-grasp) 
-                                       "~a~%Failed to grasp from ~a using ~a arm +               ​(format t "Retyring! ​Trying to grasp from ~a using ~a arm" 
-                                       ​e ?grasp ?​grasping-arm) +                       ​?grasp ?​grasping-arm) 
-                     (cpl:do-retry grasp-retries +               ​(setf ?grasp (first ?​possible-grasp)) 
-                       ​(when (cut:​lazy-car ​?​possible-grasp) +               ​(setf ?possible-grasp (rest ?​possible-grasp)) 
-                         ​(roslisp:​ros-info (trying-new-grasp) +               (park-arms) 
-                                           "​Trying to grasp from ~a using ~a arm" +               (cpl:​retry)) 
-                                         ​?grasp ?​grasping-arm) +             ;; This will get executed when there are no more elements in the  
-                         ​(setf ?possible-grasp (cut:​lazy-cdr ​?​possible-grasp)) +             ;; ?possible-grasp ​list. We print the error message and throw a new error 
-                         (pp-plans::​park-arms) +             ;; which will be caught by the outer handle-failure 
-                         (setf ?grasp (cut:​lazy-car ​?​possible-grasp)) +             ​(print  ​"No more grasp retries left :(") 
-                         ​(cpl:retry))) +             ​(cpl:fail '​object-unreachable))) 
-                     (roslisp:​ros-warn (grasp-failures) ​"No more retries left"​) +         
-                     ​(cpl:fail 'common-fail:​object-unreachable))) +        ​;; This is the failure management of the outer handle-failure call 
-                ;; Perform ​the grasp +        ;; It changes the arm that is used to grasp 
-                (exe:perform (desig:an action +        (print "​Manipulation failed!!"​) 
-                                       ​(type picking-up+        (print e)  ?grasping-arm
-                                       ​(arm ?grasping-arm) + 
-                                       ​(grasp ?grasp) +        ;; Here we use the retry counter we defined. The value is decremented automatically 
-                                       (object ​?perceived-object)))))))))+        ​(cpl:​do-retry ​arm-change-retry 
 +          ;; if the current grasping ​arm is right set left, else set right 
 +          (setf ?grasping-arm ​(if (eq ?grasping-arm :right 
 +                                  :left 
 +                                  :right)) 
 +          (cpl:retry)) 
 +        ;; When all retries are exhausted print the error message. 
 +        (print "No more arm change retries left :("))))
   ?​grasping-arm)   ?​grasping-arm)
 </​code>​ </​code>​
-With this, the ''​pick-up-object''​ can now iterate through all possible grasp configurations stored in ''?​possible-grasp''​. The ''​cram-object-interaces:​get-object-type-grasps'' ​is a useful method which will give these values as a lazy list, provided the grasping arm and the object type. +With this, the ''​pick-up-object''​ can now iterate through all possible grasp configurations stored in ''?​possible-grasps''​.
-  +
-There are two nested failure-handling clauses here. The inner failure-handling part will take care of ''​cram-common-failures:​manipulation-pose-unreachable''​ and ''​cram-common-failures:​gripper-closed-completely''​ - both of which are errors which can occur during a failed grasp. The recovery method will iterate through all possible grasp configurations for a given arm. Upon failure of this, the outer failure handling clause will take care of the ''​common-fail:​object-unreachable''​ error thrown by the inner part and change the arm it's trying to grasp the object with and then retry the entire procedure again.+
  
-And only upon the failure of all these will the error be bubbled up. The method also returns the grasping arm it used to pick the object up so that the rest of the code is aware of the arm in which the bottle rests. Also, note that the arm we decided on earlier in ''​find-object''​ now acts as a bias to reduce the number of searches we do to find a successful configuration. We have also written appropriate warning statements to be informed about the actions the robot is taking.+Even though this code block looks big, we're mostly doing things that we learned in the previous failure handling scenario. But there are two new things that we do here: First, we have used two ''​handle-failure''​ nested, meaning one handle-failure is called within the other, with the main action nested inside the innermost ''​handle-failure'',​ allowing us to layer our issues. When the program is executed, whenever a failure is encountered,​ it checks within the inner failure handling clause and retries according to it's conditions, and only upon failure of all its scenarios will it bubble up the error to the outer failure handling clause.  
 + 
 +Next is the ''​cpl:​with-retry-counters''​. We discussed earlier that we need to have some conditions to not let the retry be called an infinite number of times and get stuck in a loop. ''​cpl:​with-retry-counters''​ is one method of doing this, by allowing the user to specify a number to limit the retries, by allowing the user to initialize a counter with a specified number of retries. This counter is used track of how many times the retry is being called. And whenever you see call to ''​cpl:​do-retry''​ with the name of the  counter, this means that the retry is done only when the counter has a non-zero value and after a retry is done the counter values is decremented. You have to note that the initial try is not counted towards a retry. 
 + 
 +So let's see how both of these help with our situation. The inner failure-handling part will take care of ''​manipulation-pose-unreachable''​ or ''​gripper-closed-completely''​ - both of which are errors which can occur during a failed grasp. The recovery method will iterate through all possible grasp configurations for a given arm. Upon failure of this, the outer failure handling clause will take care of the ''​object-unreachable''​ error thrown by the inner part and it'll change the arm it's trying to grasp the object with and then retry the entire procedure again. The arm change is only required once and thus we use a retry counter called ''​arm-change-retry''​ to do it. 
 + 
 +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. 
 + 
 +//We have used the methods ''​first''​ and ''​rest''​ to iterate between our possible grasps. ''​first''​ always returns the first element in a list. ''​rest''​ returns a list without including the first element.//​ 
 +<code lisp> 
 +PP-TUT> (first '(1 2 3 4)) 
 +
 +PP-TUT> (rest '(1 2 3 4)) 
 +(2 3 4) 
 +</​code>​ 
 +//In our failure handling code, we use ''​(first ?​possible-grasp)''​ to get the first element in the possible grasping pose and set it as our new grasp. Then we reduce our possible grasp list by setting it to the rest of the list ''​(setf ?​possible-grasp (rest ?​possible-grasp))''//​
  
 Also let's redefine ''​move-bottle''​ again to include these: Also let's redefine ''​move-bottle''​ again to include these:
 <code lisp> <code lisp>
 (defun move-bottle (bottle-spawn-pose) (defun move-bottle (bottle-spawn-pose)
-  (spawn-bottle ​bottle-spawn-pose) +  (spawn-object ​bottle-spawn-pose) 
-  (pr2-proj:with-simulated-robot+  (with-simulated-robot
     (let ((?​navigation-goal *base-pose-near-table*))     (let ((?​navigation-goal *base-pose-near-table*))
       (cpl:par       (cpl:par
-        (exe:​perform (desig:a motion ​ 
-                              (type moving-torso) ​ 
-                              (joint-angle 0.3))) 
-        (pp-plans::​park-arms) 
         ;; Moving the robot near the table.         ;; Moving the robot near the table.
-        (exe:perform (desig:a motion +        (perform (an action 
-                              (type going) +                     ​(type going) 
-                              (target (desig:a location  +                     ​(target (a location  
-                                               ​(pose ?​navigation-goal)))))))+                                (pose ?​navigation-goal))))
 +        (perform (a motion 
 +                    (type moving-torso)  
 +                    (joint-angle 0.3))) 
 +        (park-arms)))
       ​       ​
-    (multiple-value-bind ​(?​perceived-bottle ​?​grasping-arm)  +    (let ((?​perceived-bottle (find-object :bottle)
-        ​(find-object :bottle) +          (?​grasping-arm :right)) 
-      (setf ?​grasping-arm (pick-up-object ?perceived-bottle :bottle ?​grasping-arm)) +      ;; We update the value of ?​grasping-arm according to what the method used 
-      (pp-plans::park-arms :arm ?​grasping-arm)+      (setf ?​grasping-arm (pick-up-object ?​perceived-bottle ?​grasping-arm)) 
 +      (park-arm ?​grasping-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) +                     ​(type going) 
-                              (target (desig:a location  +                     ​(target (a location  
-                                               ​(pose ?​nav-goal)))))) +                                (pose ?​nav-goal)))))) 
-  +       ​;; Setting the object down on the counter
-      (coe:​on-event (make-instance '​cpoe:​robot-state-changed)) +
-      ​;; Setting the object down on the counter+
       (let ((?​drop-pose *final-object-destination*))       (let ((?​drop-pose *final-object-destination*))
-        (exe:perform (desig:an action +        (perform (an action 
-                               ​(type placing) +                     ​(type placing) 
-                               ​(arm ?​grasping-arm) +                     ​(arm ?​grasping-arm) 
-                               ​(object ?​perceived-bottle) +                     ​(object ?​perceived-bottle) 
-                               ​(target (desig:a location  +                     ​(target (a location  
-                                                (pose ?​drop-pose)))))) +                                (pose ?​drop-pose)))))) 
-      (pp-plans::park-arms :arm ?​grasping-arm))))+      (park-arm ?​grasping-arm))))
 </​code>​ </​code>​
 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> ​
-BTW-TUT> (init-projection) +PP-TUT> (move-bottle ​'​((-1.0 -0.75 0.860) (0 0 0 1)))
-BTW-TUT> (move-bottle)+
 [(PICK-PLACE PICK-UP) INFO] 1550504321.279:​ Opening gripper [(PICK-PLACE PICK-UP) INFO] 1550504321.279:​ Opening gripper
 [(PICK-PLACE PICK-UP) INFO] 1550504321.279:​ Reaching [(PICK-PLACE PICK-UP) INFO] 1550504321.279:​ Reaching
Line 533: Line 870:
  
 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)
 +</​code>​
 +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 '':''​ in front of the bottle because it's an object type.
 +{{ :​tutorials:​demo:​bottle_coordinate.png?​600 |}}
 +
 +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 '​wurst-1 '(0 1 0))
 +PP-TUT> (visualize-coordinates '​wurst-1)
 +</​code>​
 +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, '''​wurst-1'',​ instead of its type, '':​weisswurst''​.
 +{{ :​tutorials:​demo:​wurst_coordinate.png?​600 |}}
 +
 +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*)
 +</​code>​
 +You will see the coordinate near the table on the right side of the kitchen visualized.
 +{{ :​tutorials:​demo:​base_pose_coordinate.png?​600 |}}
 +
 +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)
 +</​code>​
 +That will make the axes 1 meter long.
 +{{ :​tutorials:​demo:​base_pose_coordinate_large_.png?​600 |}}
 +
 +You can also visualize a pose in a coordinate frame different to "​map",​
 +but our visualizer always shows the coordinates with respect to the "​map"​ frame.
 +So, if you want to visualize a looking location or so, assume that the robot is standing in its initial location,
 +which is the ''​(0,​ 0, 0)''​ or the map, and consider the visualization with respect to the robot standing there:
 +<code lisp>
 +PP-TUT> (visualize-coordinates *right-downward-look-coordinate*)
 +</​code>​
 +{{ :​tutorials:​demo:​right_downward_look.png?​600 |}}
 +
 +
 +
 +
 +
 +===== Exercise 1 =====
 +
 +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 '​((-1.0 0.75 0.860) (0 0 0 1)))
 +</​code>​
 +
 +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/​parameters,​ to try from.
 +
 +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.
 +Just make sure that you are not leaving the environment with the robot, PR2 cannot leave the premises of the room.
 +
 +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.
 +{{:​tutorials:​demo:​reachability_rviz.png?​300|}} {{:​tutorials:​demo:​reachability_bullet_world.png?​300|}}
 +
 +===== 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. ​
 +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 orkspace>/​cram/​cram_knowrob/​cram_knowrob_pick_place/​src/​grasping.lisp
 +
 +<code lisp>
 +(defparameter *lift-z-offset* 0.15 "in meters"​)
 +(defparameter *lift-offset* `(0.0 0.0 ,​*lift-z-offset*))
 +
 +(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"​)
 +</​code>​
 +
 +As for the orientation,​ first, we need to know what the axes of the gripper are like to match it with the direction we want to approach it from. For a standard gripper, Z-axis points towards the object, ​ X is along the palm, Y is defined by the right-hand rule. Thus, to make the front-left diagonal grasp, the z-axis of the gripper should point 45 degrees in between the x and y-axes of the object. And since we are grasping across the z-axis of the bottle, since the bottle is standing along the z-axis, we'll keep our palm, which is our x-axis, perpendicular to the z-axis of the bottle.
 +
 +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*
 +        `((,​*sin-pi/​4* 0 ,​*-sin-pi/​4*)
 +          (,​*-sin-pi/​4* 0 ,​*-sin-pi/​4*)
 +          (0 1 0)))
 +</​code>​
 +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:​def-object-type-to-gripper-transforms '​(:​drink :bottle) '​(:​left :right) :​fl-diagonal
 +  :​grasp-translation `(,(- *bottle-grasp-xy-offset*) ,(- *bottle-grasp-xy-offset*) ,​*bottle-grasp-z-offset*)
 +  :​grasp-rot-matrix *diagonal-rotation*
 +  :​pregrasp-offsets `(,​*bottle-grasp-xy-offset* ,​*bottle-pregrasp-xy-offset* ,​*lift-z-offset*)
 +  :​lift-offsets *lift-offset*)
 +</​code>​
 +The summary of this code is pretty simple, we have defined a grasp called ''​fl-diagonal''​ for objects ''​drink''​ and ''​bottle'',​ which can be accessed with the ''​left''​ or ''​right''​ arm of the robot. The pre-grasp offset gives you the distance the gripper will be positioned before the grasp. The grasp-traslation gives the translation that the gripper will perform during the grasping. You can see that they'​re coming from the positive x and y axis and closing that distance during translation. The grasp-rot-matrix gives the orientation required for gripping. ​
 +
 +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 '​((-1.6 -0.9 0.82) (0 0 0 1)))
 +</​code>​
 +
 +<code lisp>
 +  (pr2-proj:​with-simulated-robot
 +    (let ((?​navigation-goal *base-pose-near-table*))
 +      (cpl:par
 +        (exe:​perform (desig:a motion ​
 +                              (type moving-torso)
 +                              (joint-angle 0.3)))
 +        (park-arms)
 +        ;; Moving the robot near the table.
 +        (exe:​perform (desig:a motion
 +                              (type going)
 +                              (target (desig:a location ​
 +                                               (pose ?​navigation-goal)))))))
 +    ;; Looking towards the bottle before perceiving.
 +    (let ((?​looking-direction *downward-look-coordinate*))
 +      (exe:​perform (desig:a motion ​
 +                            (type looking)
 +                            (target (desig:a location ​
 +                                             (pose ?​looking-direction))))))
 +    ;; Detect the bottle on the table.
 +    (setf *perceived-bottle* (exe:​perform (desig:a motion
 +                                                   (type detecting)
 +                                                   ​(object (desig:an object ​
 +                                                                     (type :​bottle)))))))
 +</​code>​
 +
 +Now let's call the pick-up action with the grasp ''​fl-diagonal''​ as we have defined.
 +<code lisp>
 +(pr2-proj:​with-simulated-robot ​
 +  (let ((?​perceived-bottle *perceived-bottle*))
 +    (exe:​perform (desig:an action
 +                           (type picking-up)
 +                           (arm right)
 +                           ​(grasp fl-diagonal)
 +                           ​(object ?​perceived-bottle)))))
 +</​code>​
 +You will see that PR2 will successfully grasp the bottle from the grasp pose that we defined.
 +
 +{{:​tutorials:​demo:​diagonal_grasp.png?​600|}}
 +
 +===== Exercise 2 =====
 +
 +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 3 =====
 +
 +Even though we already covered a lot of scenarios here, there are many more things that can be added and handled.
 +You can try to do some exercises like the ones below:
 +
 +  * Try to 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.
 +  * Try to move the torso of the robot up and down to increase the effective field of view.
 +  * 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.
 +