Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
tutorials:intermediate:pepper_shopping [2020/08/19 01:02] – derricktutorials:intermediate:pepper_shopping [2022/02/08 14:11] (current) – [Creating an ASDF system for the cram_pepper_demo] sarthou
Line 1: Line 1:
-====== Pepper robot shopping assistant (tutorial in construction) ======+====== Pepper Robot Shopping Assistant ======
  
 ===== Setting Up The Workspace ===== ===== Setting Up The Workspace =====
  
 This process assumes you have already installed CRAM on your laptop. If not, please visit the installation section.  This process assumes you have already installed CRAM on your laptop. If not, please visit the installation section. 
-If you are using the image version of CRAM, please follow the next section.+If you are using the image version of CRAM, please follow the next section. Please note that Ubuntu 16.04 was used for this project.
  
 ==== VM Set Up ==== ==== VM Set Up ====
Line 11: Line 11:
  
 <code> <code>
-$ sudo apt update && sudo apt upgrade  +$ sudo apt-get update && sudo apt-get upgrade 
 </code> </code>
  
Line 17: Line 17:
  
 <code> <code>
-$ cd ~/workspace/ros/ +$ sudo apt-get install ros-kinetic-joint-state-publisher-gui
-$ sudo apt install ros-kinetic-joint-state-publisher-gui+
 $ sudo apt-get install ros-kinetic-pepper-meshes    $ sudo apt-get install ros-kinetic-pepper-meshes   
 </code> </code>
Line 28: Line 27:
 <code> <code>
 $ sudo apt-get install ros-kinetic-ros-control ros-kinetic-ros-controllers $ sudo apt-get install ros-kinetic-ros-control ros-kinetic-ros-controllers
-$ sudo apt install ros-melodic-roslisp-common  +$ sudo apt-get install ros-kinetic-roslisp-common  
 </code> </code>
  
-At this point, we need an older version of CRAM. We a specific branch of the CRAM architecture. Therefore, if you have some work done in the current version of CRAM, please make a copy and place it in a different directory for safekeeping or commit your current branch.+At this point, we need an older version of CRAM. We need a specific branch of the CRAM architecture. Therefore, if you have some work done in the current version of CRAM, please create a backup or commit your current branch before you proceed.
  
 Run the following in your terminal. Run the following in your terminal.
Line 37: Line 36:
 <code> <code>
 $ cd ~/workspace/ros/src/cram $ cd ~/workspace/ros/src/cram
 +$ git pull
 $ git checkout 3f5b268504cb5226709daa7a5d52364c2b05a93d $ git checkout 3f5b268504cb5226709daa7a5d52364c2b05a93d
 $ git branch  $ git branch 
Line 45: Line 45:
 ==== Native Set Up ==== ==== Native Set Up ====
  
-If you did a native installation of ROS and CRAM, you would most likely install all the necessary components. However, you have to go through the setup process for the VM to be sure everything is setup. Otherwise, skip to the part where you change the branch, then you are set to go.+If you did a native installation of ROS and CRAM, you would most likely install all the necessary components. However, you have to go through the setup process for the VM to be sure everything is setup. Otherwise, skip to the part where you update the repository and change the branch, then you are set to go.
  
 Now let's begin. Now let's begin.
Line 113: Line 113:
 Comment out the following code by putting semi-colon in front of them. It should be on line 40 - 43. Comment out the following code by putting semi-colon in front of them. It should be on line 40 - 43.
  
-<code>+<code lisp>
 ; (desig:register-location-generator ; (desig:register-location-generator
 ;  3 robot-current-pose-tf-generator ;  3 robot-current-pose-tf-generator
Line 127: Line 127:
 </code> </code>
  
-==== Uploading Models to the Parameter Server ====+===== Uploading Models to the Parameter Server =====
  
 After successfully building the package, we need to add the models we will be uploading to the ROS parameter server. Change directory to the pepper_description package, and let's create three folders, namely //launch//, //meshes//, and //urdf//. After successfully building the package, we need to add the models we will be uploading to the ROS parameter server. Change directory to the pepper_description package, and let's create three folders, namely //launch//, //meshes//, and //urdf//.
Line 220: Line 220:
 {{ :tutorials:intermediate:finalscene.png?nolink&800 |}} {{ :tutorials:intermediate:finalscene.png?nolink&800 |}}
  
-==== CRAM_Pepper Description ====+===== CRAM_Pepper Description =====
  
 We want to visualize our robot and shelves in the CRAM bullet world, but first, we need to map the various parts of the URDF to their functionality. CRAM needs to know which part of the robot is meant to do what. We want to visualize our robot and shelves in the CRAM bullet world, but first, we need to map the various parts of the URDF to their functionality. CRAM needs to know which part of the robot is meant to do what.
Line 252: Line 252:
 Since we have already done an example, let’s go ahead and replace everything in the package.xml with the following: Since we have already done an example, let’s go ahead and replace everything in the package.xml with the following:
  
-<code>+<code XML>
 <package format="2"> <package format="2">
   <name>cram_pepper_description</name>   <name>cram_pepper_description</name>
Line 275: Line 275:
 Next, let us replace everything in the //CmakeLists.txt// with the following: Next, let us replace everything in the //CmakeLists.txt// with the following:
  
-<code>+<code lisp>
 cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
 project(cram_pepper_description) project(cram_pepper_description)
Line 290: Line 290:
 By now, you should have done the beginner tutorial, which explains all about .asd files. Therefore let's create one for our cram_pepper_description. Create a file //cram-pepper-description.asd// and put the following code in it. By now, you should have done the beginner tutorial, which explains all about .asd files. Therefore let's create one for our cram_pepper_description. Create a file //cram-pepper-description.asd// and put the following code in it.
  
-<code>+<code lisp>
 (defsystem cram-pepper-description (defsystem cram-pepper-description
   :depends-on (cram-prolog   :depends-on (cram-prolog
Line 316: Line 316:
 Let’s define our package in the "//package.lisp//" file with the following code. Let’s define our package in the "//package.lisp//" file with the following code.
  
-<code>+<code lisp>
 (in-package :cl-user) (in-package :cl-user)
  
Line 331: Line 331:
 In the “//neck.lisp//” file, copy the following code, and paste it in there. In the “//neck.lisp//” file, copy the following code, and paste it in there.
  
-<code>+<code lisp>
 (in-package :pepper-descr) (in-package :pepper-descr)
  
Line 379: Line 379:
 Next, we need to do the same for the arms of the robot. Again, copy the following code and paste it in the "//arms.lisp//" file. Next, we need to do the same for the arms of the robot. Again, copy the following code and paste it in the "//arms.lisp//" file.
  
-<code>+<code lisp>
 (in-package :pepper-descr) (in-package :pepper-descr)
  
Line 494: Line 494:
 Finally, we need to provide a general knowledge of the robot. We do this in the "//general-knowledge.lisp//" file. Once again, copy the following code and paste it in the file. Finally, we need to provide a general knowledge of the robot. We do this in the "//general-knowledge.lisp//" file. Once again, copy the following code and paste it in the file.
  
-<code>+<code lisp>
 (in-package :pepper-descr) (in-package :pepper-descr)
  
Line 550: Line 550:
 Now let’s see if we can load our cram package in Emacs.  Now let’s see if we can load our cram package in Emacs. 
  
-First, run the following.+First, run the following in your terminal.
  
 <code> <code>
Line 581: Line 581:
 To be able to visualize our robot and shelves, let's set up our demo folder. To be able to visualize our robot and shelves, let's set up our demo folder.
  
-==== CRAM Pepper Demo ====+===== CRAM Pepper Demo =====
  
 Just like the //cram_pepper_description//, let’s create a new package in //cram_pepper// called //cram_pepper_demo//.  Just like the //cram_pepper_description//, let’s create a new package in //cram_pepper// called //cram_pepper_demo//. 
Line 607: Line 607:
 As explained before, open the //package.xml// file in any text editor and replace everything with the following code. As explained before, open the //package.xml// file in any text editor and replace everything with the following code.
  
-<code>+<code XML>
 <package format="2"> <package format="2">
   <name>cram_pepper_demo</name>   <name>cram_pepper_demo</name>
Line 668: Line 668:
 Next, replace open the //CmakeLists.txt// in any text editor and replace everything with the following code.  Next, replace open the //CmakeLists.txt// in any text editor and replace everything with the following code. 
  
-<code>+<code lisp>
 cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
 project(cram_pepper_demo) project(cram_pepper_demo)
Line 762: Line 762:
 </code> </code>
  
-Next, download the following resource folder from here (Github link) and put the entire folder in the cram_pepper_demo package folder. The folder contains our products for the shop.+Next, download the resource folder from [[https://github.com/danricky/cram-pepper/tree/master/cram_pepper_demo/resource|here]] and put the entire folder in the cram_pepper_demo package folder. The folder contains our products for the shop.
  
 Compile the project, make sure to have the "roslaunch" running at the background, and open Emacs. Compile the project, make sure to have the "roslaunch" running at the background, and open Emacs.
Line 907: Line 907:
  
   )   )
-<code>+</code>
  
 This function creates the bullet-world, extracts the necessary information about the robot and the shelves from the ros-parameter server, spawns a static floor (plane), generates the shelves, and spawns the robot. This function creates the bullet-world, extracts the necessary information about the robot and the shelves from the ros-parameter server, spawns a static floor (plane), generates the shelves, and spawns the robot.
Line 1071: Line 1071:
 So far, so good. So far, so good.
  
-==== Pack arms ====+===== Pack Arms =====
  
 At this point, we don’t want our robot arm looking a zombie. So let’s naturally park the arms. At this point, we don’t want our robot arm looking a zombie. So let’s naturally park the arms.
Line 1101: Line 1101:
 {{ :tutorials:intermediate:parkarm.png?nolink&600 |}} {{ :tutorials:intermediate:parkarm.png?nolink&600 |}}
  
-==== Import human ====+===== Import Human =====
  
 Now let’s bring in the human avatar. Let’s create a function for that as well. Append the following code to the “cram-plans.lisp” file. Now let’s bring in the human avatar. Let’s create a function for that as well. Append the following code to the “cram-plans.lisp” file.
Line 1131: Line 1131:
 Now that we have our human in the scene, we need to reposition the robot to face the human. It may be perceived as rude for the robot to turn the back to the human.  Now that we have our human in the scene, we need to reposition the robot to face the human. It may be perceived as rude for the robot to turn the back to the human. 
  
-==== Reposition Robot ====+===== Reposition Robot =====
  
 To reposition the robot, let's add the following function to the "cram-plans.lisp" file. To reposition the robot, let's add the following function to the "cram-plans.lisp" file.
Line 1299: Line 1299:
  
 Great job so far. Up to this point, we can initialize the simulation scene, spawn the products and the human, reposition the robot to face the human, and create some basic interaction between the robot and the human. Next, we will see how we can search for the location of the product, navigate to that location, and point to the product. Great job so far. Up to this point, we can initialize the simulation scene, spawn the products and the human, reposition the robot to face the human, and create some basic interaction between the robot and the human. Next, we will see how we can search for the location of the product, navigate to that location, and point to the product.
 +
 +===== Knowledge Base (Prolog) =====
 +
 +We need a knowledge base system for the robot to be able to search for information. The robot can perform the necessary actions required based on the acquired data. Therefore, in this section, we will describe the knowledge base system used in the implementation of this project. The robot uses this knowledge base to find which category a product belongs to, and the possible location of that product.
 +
 +Copy the code below and paste it in the "//shelf-prolog.lisp//" file.
 +
 +<code lisp>
 +(in-package :demo)
 +
 +(def-fact-group product-type-predicates () 
 +
 +  (<- (is-of-type ?product ?type)
 +    (is-type ?product ?type)
 +    (product ?product)
 +    (type ?type))
 + 
 +  (<- (product cereal))
 +  (<- (product milk))
 +  (<- (product bowl)) 
 +
 +  (<- (type :breakfast-cereal))
 +  (<- (type :milk))     
 +
 +  (<- (is-type cereal :breakfast-cereal))
 +  (<- (is-type milk :milk))) 
 +
 +(def-fact-group shelf-product-predicates () 
 +
 +  (<- (is-on-shelf ?object ?shelf)
 +    (is-on ?object ?shelf)
 +    (object ?object)
 +    (shelf ?shelf))
 +
 +  (<- (is-on-shelf ?object)
 +    (is-on ?object ?shelf)
 +    (shelf ?shelf))
 +
 +  (<- (shelf shelf-one-level-1))
 +  (<- (shelf shelf-one-level-2))
 +  (<- (shelf shelf-one-level-3))
 +  (<- (shelf shelf-one-level-4))
 +  (<- (shelf shelf-one-level-5))
 +
 +  (<- (shelf shelf-two-level-1))
 +  (<- (shelf shelf-two-level-2))
 +  (<- (shelf shelf-two-level-3))
 +  (<- (shelf shelf-two-level-4))
 +  (<- (shelf shelf-two-level-5))
 +
 +  (<- (object cereal))
 +  (<- (object somat))
 +  (<- (object milk))
 +  (<- (object bowl))         
 +  (<- (object denkmit))         
 +
 +  (<- (is-on cereal shelf-one-level-4))
 +  (<- (is-on cereal shelf-two-level-3))
 +  (<- (is-on cereal shelf-one-level-2))
 +
 +  (<- (is-on somat shelf-one-level-2))
 +  (<- (is-on somat shelf-one-level-3))
 +  (<- (is-on somat shelf-one-level-4))
 +
 +  (<- (is-on milk shelf-one-level-4))
 +  (<- (is-on milk shelf-two-level-4))
 +  
 +  (<- (is-on bowl shelf-two-level-1))
 +
 +  (<- (is-on denkmit shelf-two-level-2))) 
 +
 +
 +
 +(def-fact-group shelf-location-predicates () 
 +
 +  (<- (is-located-at ?shelfs ?location)
 +    (is-at ?shelfs ?location)
 +    (location ?location)
 +    (shelfs ?shelfs))
 +
 +  (<- (shelfs shelf-one-level-1))
 +  (<- (shelfs shelf-one-level-2))
 +  (<- (shelfs shelf-one-level-3))
 +  (<- (shelfs shelf-one-level-4))
 +  (<- (shelfs shelf-one-level-5))
 +
 +  (<- (shelfs shelf-two-level-1))
 +  (<- (shelfs shelf-two-level-2))
 +  (<- (shelfs shelf-two-level-3))
 +  (<- (shelfs shelf-two-level-4))
 +  (<- (shelfs shelf-two-level-5))
 +
 +  (<- (location :|KITCHEN.shelf_1_level_1_link|))
 +  (<- (location :|KITCHEN.shelf_1_level_2_link|))
 +  (<- (location :|KITCHEN.shelf_1_level_3_link|))
 +  (<- (location :|KITCHEN.shelf_1_level_4_link|))
 +  (<- (location :|KITCHEN.shelf_1_level_5_link|))          
 +
 +  (<- (location :|KITCHEN.shelf_2_level_1_link|))
 +  (<- (location :|KITCHEN.shelf_2_level_2_link|))
 +  (<- (location :|KITCHEN.shelf_2_level_3_link|))
 +  (<- (location :|KITCHEN.shelf_2_level_4_link|))
 +  (<- (location :|KITCHEN.shelf_2_level_5_link|)) 
 +
 +  (<- (is-at shelf-one-level-1 :|KITCHEN.shelf_1_level_1_link| ))
 +  (<- (is-at shelf-one-level-2 :|KITCHEN.shelf_1_level_2_link| ))
 +  (<- (is-at shelf-one-level-3 :|KITCHEN.shelf_1_level_3_link| ))
 +  (<- (is-at shelf-one-level-4 :|KITCHEN.shelf_1_level_4_link| ))
 +  (<- (is-at shelf-one-level-5 :|KITCHEN.shelf_1_level_5_link| ))
 +
 +  (<- (is-at shelf-two-level-1 :|KITCHEN.shelf_2_level_1_link| ))
 +  (<- (is-at shelf-two-level-2 :|KITCHEN.shelf_2_level_2_link| ))
 +  (<- (is-at shelf-two-level-3 :|KITCHEN.shelf_2_level_3_link| ))
 +  (<- (is-at shelf-two-level-4 :|KITCHEN.shelf_2_level_4_link| )) 
 +  (<- (is-at shelf-two-level-5 :|KITCHEN.shelf_2_level_5_link| ))) 
 +</code>
 +
 +The first fact-group maps the products to their various types/categories while the next one maps the products to their respective shelf locations based on the shelf urdf. The last fact group maps the shelf names to their names in the bullet-world. To understand more about the cram prolog, please refer to this link.
 +
 +Now that we have our cram prolog setup let's run some queries to ensure it works. Don’t forget to compile.
 +Run the code below
 +
 +<code lisp>
 +(prolog `(and(is-on-shelf ?object ?shelf)))
 +</code>
 +
 +You should get a result like below.
 +
 +<code lisp>
 +(((?OBJECT . CEREAL) (?SHELF . SHELF-ONE-LEVEL-4))
 + . #S(CRAM-UTILITIES::LAZY-CONS-ELEM
 +      :GENERATOR #<CLOSURE (LAMBDA () :IN CRAM-UTILITIES:LAZY-MAPCAR)
 +                   {100667BBBB}>))
 +</code>
 +
 +The code returns a single result, which is the first matching result from the prolog knowledge base. What if we want it to return all matching results? We need to modify our line of code a little bit. Try the code below and see the results.
 +
 +<code lisp>
 +DEMO> (cut:force-ll (prolog `(and(is-on-shelf ?object ?shelf))))
 +</code>
 +
 +You should get a result like below:
 +<code lisp>
 +(((?OBJECT . CEREAL) (?SHELF . SHELF-ONE-LEVEL-4))
 + ((?OBJECT . CEREAL) (?SHELF . SHELF-TWO-LEVEL-3))
 + ((?OBJECT . CEREAL) (?SHELF . SHELF-ONE-LEVEL-2))
 + ((?OBJECT . SOMAT) (?SHELF . SHELF-ONE-LEVEL-2))
 + ((?OBJECT . SOMAT) (?SHELF . SHELF-ONE-LEVEL-3))
 + ((?OBJECT . SOMAT) (?SHELF . SHELF-ONE-LEVEL-4))
 + ((?OBJECT . MILK) (?SHELF . SHELF-ONE-LEVEL-4))
 + ((?OBJECT . MILK) (?SHELF . SHELF-TWO-LEVEL-4))
 + ((?OBJECT . BOWL) (?SHELF . SHELF-TWO-LEVEL-1))
 + ((?OBJECT . DENKMIT) (?SHELF . SHELF-TWO-LEVEL-2)))
 +</code>
 +
 +As we can see, the code returns the entire matching results. Add  "force-ll" from the "cut" package tells the prolog package to return the full results and not just one.
 +
 +Now that we have proved that our knowledge base works, let's proceed to query the knowledge base from the program.
 +
 +Let's outline the next actions that we need to take. When we get the product information from the user (name of the product), we need to identify the possible location. Base on our knowledge-base setup, we need to find where the product is located on the shelf (urdf), see the actual name of that location in the bullet-world, and use the results to get the pose from the bullet-world.
 +To achieve that, copy the function below and place it in your //cram-plans.lisp// file.
 +
 +<code lisp>
 +;; This function returns a list of shelf poses
 +;; It takes the product name as an argument 
 +(defun get-shelf-pose(?product-name)
 +
 + (let* ((list-of-shelves 
 + (cut:force-ll (prolog `(and(is-on-shelf ,?product-name ?shelf))))) 
 + (shelf-location-list nil)
 + (shelf-name nil)
 + (shelf-btr-name nil)
 + (shelf-pose-trans nil)
 + (shelf-str nil)
 + )
 +
 + (dolist (element list-of-shelves)
 + (setf shelf-name (cdar element))
 +
 + (setf shelf-btr-name (cdaar (cut:force-ll (prolog `(and(is-located-at ,shelf-name ?location))))))
 +
 + (setf shelf-pose-trans 
 + (cl-transforms:pose->transform
 + (btr:pose 
 + (find shelf-btr-name
 + (btr:rigid-bodies (btr:get-environment-object)) :key #'btr::name :test #'equalp))))
 + (setf shelf-str (subseq (write-to-string shelf-btr-name) 10 17))
 +
 +
 + (cond ((string-equal shelf-str "shelf_1")
 + (setf shelf-location-list (cons
 + (cl-transforms:transform->pose
 + (cl-transforms:transform* shelf-pose-trans *shelfoneOffset*))
 +
 + shelf-location-list)))
 +
 + ((string-equal shelf-str "shelf_2")
 + (setf shelf-location-list (cons
 + (cl-transforms:transform->pose
 + (cl-transforms:transform* shelf-pose-trans *shelftwoOffset*))
 +
 + shelf-location-list))
 + )))
 +
 + shelf-location-list
 + )
 +
 + )
 +</code>
 +
 +The function above returns a list of possible locations of a given product. As mentioned before, it queries the knowledge base to find the products' possible locations based on the shelf names from the shelf URDF file. Using a loop, we get the actual pose from the bullet-world and store the results in a list. After which the list is returned.
 +
 +Also, in the above function, we use two global variables; "//*shelfoneOffset*//" and "//*shelftwoOffset*//". Copy the following code and place it at the top of the "//cram-plans.lisp//" file.
 +
 +<code lisp>
 +(defparameter *shelfoneOffset* 
 + (cl-tf:make-transform (cl-tf:make-3d-vector 0.0 -0.33 0.05) (cl-tf:make-quaternion 0 0 0 1)))
 +
 +(defparameter *shelftwoOffset* 
 + (cl-tf:make-transform (cl-tf:make-3d-vector 0.0 -0.25 0.1) (cl-tf:make-quaternion 0 0 0 1)))
 +</code>
 +
 +===== Generating Robot Pose =====
 +Now that we have a list of possible locations of the product, we want to move the robot to a position closer to the possible location one at a time. The idea here is that the robot can end up finding the given product at the first location or the last location in the list. Therefore, for every possible location, the robot will try and find the product. However, we cannot just move the robot to a possible location. The robot could end damaging itself by colliding into the shelf or other humans. Therefore we need to generate a pose for the robot to move to using the possible locations. To do that, please copy the code below and append to the "cram-plans.lisp" file.
 +
 +<code lisp>
 +;; This function generates a possible location from which
 +;; the robot can see the product.
 +;; It takes the product name as an argument
 +(defun generate-robot-pose(?shelf-pose)
 + (let* ((?product-pose ?shelf-pose)
 + (?product-pose-stamped (cl-transforms-stamped:pose->pose-stamped "map" 0 ?product-pose))
 + (?product-location-designator (desig:a location (pose ?product-pose-stamped)))
 + (to-see-designator (desig:a location (visible-for pepper)
 + (location ?product-location-designator)
 + (object (desig:an object (type :shelf)))))) ; This line was needed for the location generation to work. 
 + (desig:reference to-see-designator)))
 +</code>
 +
 +In this function, we are saying that based on the given possible product location from the list we generate in the previous function, create a position from which the robot can see that pose. The function returns a pose to which the robot can move to. You can read more about how the pose generation is done from here.
 +
 +Move the Robot to locations.
 +Now that we have our pose, we can move the robot to the results. To achieve this, add the code below to the "cram-plans.lisp" file.
 +
 +<code lisp>
 +;; This function moves the robot to a given pose
 +(defun move-to-location(?pose)
 + (let ((?navigation-goal ?pose))
 + (perform (desig:an action
 + (type going)
 + (target (desig:a location 
 + (pose ?navigation-goal)))))))
 +</code>
 +
 +The function above moves the robot to the given location/pose.
 +
 +Now let’s test the functions we just created. Let’s create a function that will house our entire demo. In this function, we can call the functions we create. Let's create a function called demo-one, as shown below. It takes two parameters; the list of shelf poses and the product name. Also, we specify which platform we are using, which is the simulated robot.
 +
 +<code lisp>
 +;; Demo one
 +(defun demo-one(?product-name ?shelf-pose-lists)
 + (cram-urdf-projection:with-simulated-robot
 +
 + ;;function details
 +
 +
 + ))
 +</code>
 +
 +Before we continue, we need to update the interaction so we can get the list of shelf poses after the user has indicated the product he/she wants and then parse the results to the “demo-one” function. So we are going to call the “get-shelf-pose” after we get the product name and store the results in a local variable. Your interaction function should look like below (changes have been highlighted in red). Also, let's print the variables to check if we are getting some values.
 +
 +<code lisp>
 +;; Interaction execution
 +(defun interaction()
 + (cram-urdf-projection:with-simulated-robot (park-arms))
 + (reposition-robot)
 + (menu-one)
 +
 + (let* '((?productname nil)
 + (?productposelist nil))
 + (setf response (read t))
 +
 + ;;first conditional statement start
 + (cond ((= response 1)
 + (menu-two)
 +
 + ;;second conditional statement start
 + (setf proresponse (read t))
 + (cond ((= proresponse 1)
 + (setf ?productname 'cereal)
 + (setf ?productposelist (get-shelf-pose ?productname))
 + (format t "Alright. Please follow me!"))
 + ((= proresponse 2)
 + (setf ?productname 'milk)
 + (setf ?productposelist (get-shelf-pose ?productname))
 + (format t "Alright. Please follow me!"))
 + )
 + ;;second conditional statement end
 + (print ?productname)
 + (print ?productposelist)
 + )
 + ((= response 2 )
 + (format t "That's nice of you. Thanks!"))
 + );;first conditional statement end
 +
 + ) ;;let close brackets
 + ) ;;interaction close brackets
 +</code>
 +
 +Compile all your edited files and run the "interaction" function again. Select 1 in the first menu and 1 for the other menu. Your terminal should look like the image below.
 +
 +{{ :tutorials:intermediate:printonetest.png?nolink&600 |}}
 +
 +As we can see from the image above, the "cereal" can be located at three locations, which means that everything works up until this point. Let's parse the variables to the "demo-one" function.
 +Note: You can take out the print lines and replace those lines with the following code.
 +
 +<code lisp>
 +(demo-one ?productname ?productposelist)
 +</code>
 +
 +Update the "//demo-one//" function with the following code. Place it under the function details comment.
 +
 +<code lisp>
 +        (let*  ((shelf-pose-lists-copy ?shelf-pose-lists)
 + (product-name-copy ?product-name)
 + (product-pose nil)
 + (found nil))
 +
 +        ;;;expression
 + (dolist (?shelf-pose shelf-pose-lists-copy)
 +
 + (setf product-pose (generate-robot-pose ?shelf-pose))
 +
 + (cpl:with-retry-counters ((error-counter 2))
 + (cpl:with-failure-handling
 +
 + ((cram-common-failures:navigation-pose-unreachable (e)
 +
 +
 + (roslisp:ros-warn (navigation-failure) "~a~%
 + Could not move to location...retrying" e)
 +
 + (cpl:do-retry error-counter
 +
 + (setf product-pose (generate-robot-pose ?shelf-pose))
 +
 + (cpl:retry))
 + (return)))
 +
 +
 + (move-to-location product-pose)))
 +
 + )) ;;endof let* and dolist
 +</code>
 +
 +This code above creates four local variables. Using a loop, we go through the list of poses, and then we move the robot to a generated pose. Since CRAM allows us to handle failures, in some cases, the generated pose might be invalid or unachievable, so we try generating another pose for the robot to move to. We do this two times; however, you might want to do it more than once in reality. If we ran our "//demo-one//" function, we would see that the robot moves to all the locations. The image below shows the robot at the last location.
 +
 +{{ :tutorials:intermediate:robotmoveto.png?nolink&600 |}}
 +
 +Now we can proceed to find the product on the shelf.
 +
 +Let’s start by creating a function for that purpose. Copy the code below, and let's explain what is happening. 
 +
 +<code lisp>
 +;; This function finds a product based on the name
 +;; It tries to look for the product from a different direction on a shelf 
 +;; It also handles failures accordingly
 +(defun find-product (?product-name ?product-pose)
 +
 + (generate-look-directions ?product-pose)
 +
 + (let* ((?object-type (get-product-type ?product-name))
 + (possible-look-directions `(,*look-center*
 + ,*look-right*
 + ,*look-left*))
 + (?looking-direction (first possible-look-directions)))
 +
 + (setf possible-look-directions (cdr possible-look-directions))
 +
 + (move-to-location (calculate-robot-navigation-goal-towards-target ?looking-direction))
 +
 + (look-at-product ?looking-direction)
 +
 + (cpl:with-failure-handling
 + ((cram-common-failures:perception-object-not-found (e)
 +           ;; Try different look directions until there is none left.
 +           (when possible-look-directions
 +            (roslisp:ros-warn (perception-failure) "~a~%Turning head." e)
 +
 +            (format t "Changing viewing direction!")
 +            (setf ?looking-direction (first possible-look-directions))
 +
 +            (setf possible-look-directions (cdr possible-look-directions))
 +
 +            (move-to-location (calculate-robot-navigation-goal-towards-target ?looking-direction))
 +            (look-at-product ?looking-direction)
 +
 +
 +            (cpl:retry))
 +           (return)))
 +
 +
 + (cram-executive:perform
 + (desig:an action
 + (type detecting)
 + (object (desig:an object
 + (type ?object-type)))))
 + )))
 +</code>
 +
 +From the code above, we first generate three different looking directions based on the initial/given pose; center, left, and right with the following code.
 +
 +<code lisp>
 +(generate-look-directions ?product-pose)
 +</code>
 +
 +For every direction generated, we adjust the base of the robot to face that direction, and then we turn the robot's neck towards the position of the product. The reason behind this process depends on the pose of the robot at the initial state, turning only the neck towards the position of the product wouldn't be enough. Therefore, to compensate for the extra turn/distance, we adjust the robot base to make the turning neck, less strenuous.
 +
 +<code lisp>
 +(setf possible-look-directions (cdr possible-look-directions))
 +
 +(move-to-location (calculate-robot-navigation-goal-towards-target ?looking-direction))
 +
 +(look-at-product ?looking-direction)
 +</code>
 +
 +At this point, the robot should be in a position to detect the product. However, there is a probability that the detection could fail. That's the product that may not be available in a particular direction. This failure will cause our program to throw an error. Therefore, we can use the failure handling mechanism of CRAM to handle the error. In our program, we solve this problem by looking for the product in other directions (center, left, or right), and then we report back to the user.
 +
 +<code lisp>
 +(cpl:with-failure-handling
 + ((cram-common-failures:perception-object-not-found (e)
 +           ;; Try different look directions until there is none left.
 +           (when possible-look-directions
 +            (roslisp:ros-warn (perception-failure) "~a~%Turning head." e)
 +
 +            (format t "Changing viewing direction!")
 +            (setf ?looking-direction (first possible-look-directions))
 +
 +            (setf possible-look-directions (cdr possible-look-directions))
 +
 +            (move-to-location (calculate-robot-navigation-goal-towards-target ?looking-direction))
 +            (look-at-product ?looking-direction)
 +
 +
 +            (cpl:retry))
 +           (return)))
 +
 +
 + (cram-executive:perform
 + (desig:an action
 + (type detecting)
 + (object (desig:an object
 + (type ?object-type)))))
 + )
 +</code>
 +
 +From the code above, we can see that there some helper functions. These include "//generate-look-directions//", "//get-product-type//", "//calculate-robot-navigation-goal-towards-target//", and "//look-at-product//". We also introduced a few global variables. So let's add them to our "//cram-plans.lisp//" file.
 +
 +First, add the following global variables to the top of the page.
 +
 +<code lisp>
 +(defparameter *look-center* nil)
 +(defparameter *look-right* nil)
 +(defparameter *look-left* nil)
 +</code>
 +
 +Next, we add the following functions.
 +
 +<code lisp>
 +;; This function generates possible looking directions for the robot
 +(defun generate-look-directions (?product-pose)
 + (setf *look-center* (cl-transforms-stamped:pose->pose-stamped "map" 0  ?product-pose))
 + (setf *look-right* (get-right-pose ?product-pose))
 + (setf *look-left* (get-left-pose ?product-pose)))
 +</code>
 +
 +It contains some additional helper functions. Let’s add them.
 +
 +<code lisp>
 +;; This function generates the left possible looking direction
 +;; It takes the product name as an argument
 +(defun get-left-pose(?product-pose)
 + (let* ((mapTshelf-pose ?product-pose)
 + (mapTshelf-trans (cl-transforms:pose->transform mapTshelf-pose)))
 +
 + (cl-transforms-stamped:pose->pose-stamped "map" 0 
 + (cl-transforms:transform->pose
 + (cl-transforms:transform* mapTshelf-trans *left-offset*)))))
 +
 +;; This function generates the right possible looking direction
 +;; It takes the product name as an argument
 +(defun get-right-pose(?product-pose)
 + (let* ((mapTshelf-pose ?product-pose)
 + (mapTshelf-trans (cl-transforms:pose->transform mapTshelf-pose)))
 + (cl-transforms-stamped:pose->pose-stamped "map" 0 
 + (cl-transforms:transform->pose
 + (cl-transforms:transform* mapTshelf-trans *right-offset*)))))
 +</code>
 +
 +These helper functions contain some global variables, so let's place them at the top of the file.
 +
 +<code lisp>
 +(defparameter *right-offset* 
 + (cl-tf:make-transform (cl-tf:make-3d-vector 0.3 0.0 0.05) (cl-tf:make-quaternion 0 0 0 1)))
 +
 +(defparameter *left-offset* 
 + (cl-tf:make-transform (cl-tf:make-3d-vector -0.3 0.0 0.05) (cl-tf:make-quaternion 0 0 0 1)))
 +</code>
 +
 +Next,
 +
 +<code lisp>
 +;; This function queries our reasoning base and returns the type of 
 +;; product based on the name
 +(defun get-product-type(?product-name)
 + (cdaar (cut:force-ll (prolog `(and(is-of-type ,?product-name ?type))))))
 +
 +;; This function helps the robot to look at the given
 +;; location.
 +(defun look-at-product (?product-direction)
 + (cpl:with-retry-counters ((error-counter 2))
 + (cpl:with-failure-handling
 +
 + ((cram-common-failures:ptu-goal-not-reached (e)
 +
 +        (roslisp:ros-warn (perception-failure) "~a~%Looking at product went wrong...repositioning" e)
 +        (cpl:do-retry error-counter
 +
 +         (move-to-location (get-robot-new-pose))
 +
 +         (cpl:retry))
 +        (cpl:fail 'common-fail:looking-high-level-failure)))
 +
 +
 + (cram-executive:perform (desig:a action 
 + (type looking)
 + (target (desig:a location
 + (pose ?product-direction))))))))
 +</code>
 +
 +There is a helper function that repositions the robot in case the robot needs to increase its field of view. Let’s add that function.
 +
 +<code lisp>
 +;; This function generates and returns a new robot position based 
 +;; on its current position. It multiplies the current position of
 +;; the robot by an offset.
 +(defun get-robot-new-pose()
 + (let* ((robot-cur-pose (cram-tf:robot-current-pose))
 + (robot-cur-trans (cl-transforms:pose->transform robot-cur-pose)))
 +
 + (cl-transforms-stamped:pose->pose-stamped "map" 0 
 + (cl-transforms:transform->pose
 + (cl-transforms:transform* robot-cur-trans  *robotPoseOffset*)))))
 +</code>
 +
 +This function contains a global variable to offset the position of the robot. Let's add that to a group of global variables.
 +
 +<code lisp>
 +(defparameter *robotPoseOffset* 
 + (cl-tf:make-transform (cl-tf:make-3d-vector -0.3 0.0 0.0) (cl-tf:make-quaternion 0 0 0 1)))
 +</code>
 +
 +Next,
 +
 +<code lisp>
 +;;;;
 +(defun calculate-robot-navigation-goal-towards-target (?location-pose)
 + (calculate-pose-towards-target
 + ?location-pose
 + (cram-tf:robot-current-pose)))
 +</code>
 +
 +This function also contains another helper function. Let’s add it.
 +
 +<code lisp>
 +;;"Given a `look-pose-stamped' and a `robot-pose-stamped' (both in fixed frame),
 +;; calculate the new robot-pose-stamped, which is rotated with an angle to point towards
 +;; the `look-pose-stamped'."
 +(defun calculate-pose-towards-target (look-pose-stamped robot-pose-stamped)
 +
 + (let* ((world->robot-transform
 + (cram-tf:pose-stamped->transform-stamped robot-pose-stamped "robot"))
 + (robot->world-transform
 + (cl-transforms:transform-inv world->robot-transform))
 + (world->look-pose-origin
 + (cl-transforms:origin look-pose-stamped))
 + (look-pose-in-robot-frame
 + (cl-transforms:transform-point
 + robot->world-transform
 + world->look-pose-origin))
 + (rotation-angle
 + (atan
 + (cl-transforms:y look-pose-in-robot-frame)
 + (cl-transforms:x look-pose-in-robot-frame))))
 + (cram-tf:rotate-pose robot-pose-stamped :z rotation-angle)))
 +</code>
 +
 +Now let’s call the find-product function after moving the robot to generated location. Append the following code to the “demo-one” function.
 +
 +<code lisp>
 +;; Finding product
 +(setf found (find-product product-name-copy ?shelf-pose))
 +</code>
 +
 +If we run "//demo-one//," we will notice that the robot will still go through all the possible product poses even if it finds it in the first position. Ideally, we want the robot to stop after seeing the product. Therefore, we need to check if the product has been found after every search. Let’s add the following code after trying to find the product.
 +
 +<code lisp>
 +(if (not (null found))
 + (return))
 +</code>
 +
 +This line of code will stop the search for the product after it has been found. If we try to run "demo-one," the robot should stop after the first try.
 +
 +
 +Suppose you have gotten to point without any issues kudos to you. We are almost done. Now, we need to point to the product on the shelf and move the human behind the robot. Let's add the necessary functions to perform these tasks. Update the "cram-plans.lisp" file with the following code.
 +
 +<code lisp>
 +(defun point-front-right() ; Point forward using the right arm
 + (cram-executive:perform
 + (desig:an action
 + (type positioning-arm)
 + (left-configuration park)
 + (right-configuration point-ahead))))
 +</code>
 +
 +Let's call this function when the product has been found. That's when "found" is not nil. We want to point to the product and then return. Therefore, we want to use the "progn" keyword. Update the demo-one function with the following code.
 +
 +<code lisp>
 +(if (not (null found))
 + (progn
 + (point-front-right)
 + (return)))
 +</code>
 +
 +Compile, and let's test the code to see if the robot will point to the product on the shelf. If everything is done correctly, you should have the robot point at the product, like the image below.
 +
 +{{ :tutorials:intermediate:pointing.png?nolink&600 |}}
 +
 +Now let’s bring the human closer to the robot. Let’s add the following functions.
 +
 +<code lisp>
 +;; This function moves the robot to a new position
 +(defun move-human-to-location()
 + (let* ((?new-human-location (get-human-new-pose)))
 + (setf (btr:pose (btr:object btr:*current-bullet-world* :my-human)) ?new-human-location)))
 +</code>
 +
 +This function has a helper function. Let’s add that as well.
 +
 +<code lisp>
 +;; This function generates and returns a new human position based 
 +;; on the robots position. It multiplies the current position of the robot
 +;; by an offset
 +(defun get-human-new-pose()
 + (let* ((robot-cur-pose (cram-tf:robot-current-pose))
 + (robot-cur-trans (cl-transforms:pose->transform robot-cur-pose)))
 + (cl-transforms-stamped:pose->pose-stamped "map" 0 
 + (cl-transforms:transform->pose
 + (cl-transforms:transform* robot-cur-trans  *humanPoseOffset*)))))
 +</code>
 +We have a global variable in this function as well. Let’s add it at the top.
 +
 +<code lisp>
 +(defparameter *humanPoseOffset* 
 + (cl-tf:make-transform (cl-tf:make-3d-vector -0.8 0.0 1.0) (cl-tf:make-quaternion 0 0 1 1)))
 +</code>
 +
 +Now call it after the robot has pointed to the product. Like this;
 +
 +<code lisp>
 +(if (not (null found))
 + (progn
 + (point-front-right)
 + (move-human-to-location)
 + (return)))
 +</code>
 +
 +You should get the following results.
 +
 +{{ :tutorials:intermediate:movehuman.png?nolink&600 |}}
 +
 +Now let’s add some text to be printed to the terminal. Like the following:
 +
 +<code lisp>
 +(if (not (null found))
 + (progn
 + (point-front-right)
 + (move-human-to-location)
 + (format t "There is your product!")
 +(return)))
 +</code>
 +
 +Also, what happens when the product is not found? We need to add an else part to the if statement like the following.
 +
 +<code lisp>
 +(if (not (null found))
 + (progn
 + (point-front-right)
 + (move-human-to-location)
 + (format t "There is your product!")
 +(return))
 + (progn
 + (move-human-to-location)
 + (format t "Sorry I could not find your product! However, 
 + I believe it is supposed to be in this area of the shelf.")))
 +</code>
 +If we try running the "interaction" function after compiling, we will realize that the human avatar will not reset back to its original position. See the image below.
 +
 +{{ :tutorials:intermediate:stuckhuman.png?nolink&600 |}}
 +
 +So we need a function to reset the human back to its original position. Update the "cram-plans.lisp" file with the code below.
 +
 +<code lisp>
 +;;Repositions the human avatar 
 +(defun reposition-human()
 + (prolog:prolog '(and (btr:bullet-world ?world)
 +                              (assert (btr:object-pose ?world :my-human ((-1 0 1) (0 0 1 1)))))))
 +</code>
 +Let's call this function in the "interaction" function, like below.
 +
 +<code lisp>
 +;; Interaction execution
 +(defun interaction()
 + (cram-urdf-projection:with-simulated-robot (park-arms))
 + (reposition-human)
 + (reposition-robot)
 + (menu-one)
 +….
 +</code>
 +
 +Let's end the previous process using ctrl+c twice, compile the edited files, and run "interaction" again. You should see the human moving back to its original position.
 +
 +Congratulations!!! You just created a shopping assistant using the Pepper robot. 
 +
 +You can access the entire project from [[https://github.com/danricky/cram-pepper|here]]