This is an old revision of the document!


Pepper robot shopping assistant (tutorial in construction)

Setting Up The Workspace

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.

VM Set Up

Open the terminal by just typing ctrl+alt+t and then type and execute the following command:

$ sudo apt update && sudo apt upgrade  

Let’s update ROS and CRAM components. In the terminal type and execute the following commands:

$ cd ~/workspace/ros/
$ sudo apt install ros-kinetic-joint-state-publisher-gui
$ sudo apt-get install ros-kinetic-pepper-meshes   

Please note that during this process, a window will pop up, and you must accept the license. Press tab key to select 0K then Yes.

Install ros-control and roslisp_common package

$ sudo apt-get install ros-kinetic-ros-control ros-kinetic-ros-controllers
$ sudo apt install ros-melodic-roslisp-common  

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.

Run the following in your terminal.

$ cd ~/workspace/ros/src/cram
$ git checkout 3f5b268504cb5226709daa7a5d52364c2b05a93d
$ git branch 

Use the last command to confirm that you are using a branch named 3f5b268.

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.

Now let's begin.

First, we need to set up our Robot and Shelf models to upload them to the ROS parameter server. Through that, we can visualize these models in RViz and the Bullet world.

Let's begin by creating a package in the src of our ROS workspace by running the command below in a terminal.

$ cd ~/path-to-workspace/ros/src
$ catkin_create_pkg pepper_description

The terminal should be looking like the sample below.

Created file pepper_description/package.xml
Created file pepper_description/CMakeLists.txt
Successfully created files in /home/path-to-workspace/ros/src/pepper_description. Please adjust the values in package.xml.

Next, we are going to adjust the values in the package.xml file. Here we want to add all the project dependencies to this file. Replace everything in the file with the code below.

<?xml version="1.0"?>
<package format="2">
  <name>pepper_description</name>
  <version>1.0.0</version>
  <description>The pepper_description package</description>
 
  <maintainer email="pepper@todo.todo">pepper</maintainer>
 
  <license>TODO</license>
 
  <buildtool_depend>catkin</buildtool_depend>
 
  <build_depend>urdf</build_depend>
  <build_depend>controller_manager</build_depend>
  <build_depend>joint_state_controller</build_depend>
  <build_depend>robot_state_publisher</build_depend>
 
  <build_export_depend>urdf</build_export_depend>
 
  <exec_depend>urdf</exec_depend>
  <exec_depend>controller_manager</exec_depend>
  <exec_depend>joint_state_controller</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
 
</package>

Next, we edit the CMakeList.txt file. Here we need to tell catkin which other packages are required for building our package. Add the same list of packages from the package.xml file to the find_package section of the CmakeList.txt file. It should look like the sample below.

find_package(catkin REQUIRED COMPONENTS
  urdf
  controller_manager
  joint_state_controller
  robot_state_publisher
)

Rename the file “PATH-TO-YOUR-WORKSPACE/ros/src/iai_maps/iai_maps/maps/map-2017-11-ease-demo.pgm” to “PATH-TO-YOUR-WORKSPACE/ros/src/iai_maps/iai_maps/maps/map-2017-11-ease-demo-old.pgm”. Download the file “map-2017-11-ease-demo.pgm” from here and replace the old file with this new one. Here we are replacing the default map with our map.

Next, open this file: “PATH-TO-YOUR-WORKSPACE/ros/src/cram/cram_common/cram_robot_pose_gaussian_costmap/src/current-pose-generator-and-pose-validator.lisp” in any text editor.

Comment out the following code by putting semi-colon in front of them. It should be on line 40 - 43.

; (desig:register-location-generator
;  3 robot-current-pose-tf-generator
; "We should move the robot only if we really need to move. Try the
;  current robot pose as a first solution.")

Now we can compile. Run the code below.

$ cd .. && catkin_make

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.

Download these files and put them in the URDF folder. Download these meshes and put them in the meshes folder. Download the upload.launch file and put it in the launch folder.

Now let’s explain the code in the upload.launch file.

<include file="$(find iai_maps)/launch/map.launch" />

This line uploads the occupancy map for navigation. It is the environment or free space for the robot to move around.

<param name="robot_description" textfile="$(find pepper_description)/urdf/pepper.urdf" />
<param name="shelf_description" command="$(find xacro)/xacro --inorder '$(find pepper_description)/urdf/dm_shelves.urdf.xacro'" />

These two lines are the representation of the Pepper robot and shelves in the URDF. Take note of the name parameter. We will need it for Rviz visualization.

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
  <node name="shelf_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
      <remap from="robot_description" to="shelf_description"/>
</node>

The next line is a ROS node to publish the robot and the shelf joint state values to make them available to other nodes.

<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" />
  <node pkg="robot_state_publisher" type="state_publisher" name="shelf_tf_state_publisher">
    <param name="publish_frequency" value="25"/>
    <remap from="robot_description" to="shelf_description" />
</node>

This line is also a ROS node to publish the robot state values of the robot and the shelves.

<node pkg="tf" type="static_transform_publisher" name="pepper_wrt_map_tf" args="0 0 0 0 0 0 /map /base_link 25"></node>
<node pkg="tf" type="static_transform_publisher" name="shelf_wrt_map_tf" args="0 0 0 0 0 0 /map /room_link 25"></node>

The last two lines are nodes to publish transform frames with respect to the map.

Save and compile your workspace.

Now let's upload the models to the ROS parameter server. Run the code below.

$ roslaunch pepper_description upload.launch

Open another terminal and open RViz.

$ rviz

You should see a window like the one below.

Now we need to setup RViz so we can visualize our robot and shelves. RViz contains various display modules for visualizing multiple components of a robot. Please note that RViz cannot be used for simulations, only for visualizations.

Click on the Add button, as shown in the image below.

A dialog box opens where we select the display modules we want to add as shown below. In our case, we will add two of the RobotModel. RViz does not support quantity specification, so we must do the add process twice.

Afterward, you should have the display column looking like the image below.

Now we are ready to configure the scene. First, change the “Fixed Frame” parameter to “map”. You must select it from a dropdown menu when you click that space. If RViz automatically chooses it for you, then move on to the next configuration.

The next one is the “RobotModel” module. Remember the name parameter from the launch file; this is where we will be using it. When you drop down the first “RobotModel”, you should see “Robot_description” among the parameters. The first “RobotModel” parameter should have “shelf_description,” and the second “RobotModel” should have “robot_description”. Note that these are based on the designer’s preferences. The order can be changed at will. However, make sure the name in the launch file is the same one used in RViz. Your final display column should look like the image below.

And your scene should look like the image below. You should see the robot at the center of the map with the shelves in front of it.

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.

Before we begin, let’s create a directory for the entire project. Run the following code to create the following directories PATH-TO-YOUR-WORKSPACE/ros/src/cram/cram_pepper if none exists.

$  mkdir -p path-to-your-workspace/ros/src/cram/cram_pepper

Change directory to “cram_pepper.”

$ cd path-to-your-workspace/ros/src/cram/cram_pepper 

Now let us create a ROS package called cram_pepper_description.

$ catkin_create_pkg cram_pepper_description

Your results should look like below.

Created file cram_pepper_description/CMakeLists.txt
Created file cram_pepper_description/package.xml
Successfully created files in path-to-your-workspace/ros/src/cram/cram_pepper/cram_pepper_description. Please adjust the values in package.xml.

Since we have already done an example, let’s go ahead and replace everything in the package.xml with the following:

<package format="2">
  <name>cram_pepper_description</name>
  <version>0.7.0</version>
  <description>
    Prolog predicates describing pepper robot
  </description>
  <author>Your Name</author>
  <maintainer email="youremail@mail.com">Your Name</maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>

  <depend>cram_prolog</depend>
  <depend>cram_robot_interfaces</depend>
  <depend>cram_location_costmap</depend>
</package> 

The last three lines in the package tag are libraries we will need for describing Pepper in CRAM. The first will help us write Prolog predicates, the next one will help us map the URDF to the actual functionality of the robot in the Bullet world, and the last one will help us use cost maps to generate locations in the Bullet world.

Next, let us replace everything in the CmakeLists.txt with the following:

cmake_minimum_required(VERSION 2.8.3)
project(cram_pepper_description)
find_package(catkin REQUIRED)
catkin_package()

This cleanup makes our file easy to read.

Now let’s compile to be sure everything is correctly setup. If you run into any errors, you might want to go back and recheck every step.

Creating an ASDF system

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.

(defsystem cram-pepper-description
  :depends-on (cram-prolog
               cram-robot-interfaces
               cram-location-costmap ; for robot-specific costmap metadata
               )
  :components
  ((:module "src"
    :components
    ((:file "package")
     (:file "general-knowledge" :depends-on ("package"))
     (:file "arms" :depends-on ("package"))
     (:file "neck" :depends-on ("package"))
     ))))

Next, we have to create the files listed in the components section in our src folder. However, we don't have an src folder at this point, so we go ahead and create one in our cram_pepper_description package.

$ mkdir src

Let's create the files; package.lisp, general-knowledge.lisp, arms.lisp, and neck.lisp in the source folder. These files represent the various parts of the robot. This division is just a design decision, so we can easily read it.

Let’s define our package in the “package.lisp” file with the following code.

(in-package :cl-user)

(defpackage cram-pepper-description
  (:nicknames #:pepper-descr)
  (:use #:common-lisp #:cram-prolog #:cram-robot-interfaces)
  (:export #:pepper))

Again, refer to the beginner tutorial to understand what is happening here.

Now we need to describe the Pepper robot semantically. We need to decide how we define each part of the robot in the package using the files we just created. This part of the setup has been explained in Introducing a new robot to CRAM. Therefore, we will jump straight into what to do in our case.

In the “neck.lisp” file, copy the following code, and paste it in there.

(in-package :pepper-descr)

(defparameter *neck-good-looking-down-state*
  '(("HeadYaw" 0.00)
    ("HeadPitch" 0.62)))

(defparameter *neck-good-looking-left-state*
  '(("HeadYaw" 0.86)
    ("HeadPitch" 0.00)))

(defparameter *neck-parking-joint-states*
  '(("HeadYaw" 0.00)
    ("HeadPitch" -0.00)))


(def-fact-group pepper-neck-facts (robot-neck-links
                                 robot-neck-joints
                                 robot-neck-base-link
                                 robot-joint-states
                                 camera-in-neck-ee-pose)

  (<- (robot-neck-links pepper
                        "Neck"
                        "Head"))

  (<- (robot-neck-joints pepper
                         "HeadYaw"
                         "HeadPitch"))

  (<- (robot-joint-states pepper :neck ?there-is-only-one-neck :away ?joint-states)
    (symbol-value *neck-parking-joint-states* ?joint-states))

  (<- (robot-joint-states pepper :neck ?there-is-only-one-neck :forward ?joint-states)
    (symbol-value *neck-parking-joint-states* ?joint-states))

  (<- (robot-joint-states pepper :neck ?there-is-only-one-neck :down ?joint-states)
    (symbol-value *neck-good-looking-down-state* ?joint-states))

  (<- (robot-joint-states pepper :neck ?there-is-only-one-neck :down-left ?joint-states)
    (symbol-value *neck-good-looking-left-state* ?joint-states)))
    

This code defines the neck structure of the Pepper robot in CRAM. We are telling the CRAM architecture the functionality of the neck based on the given URDF.

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.

(in-package :pepper-descr)

(defparameter *standard-to-pepper-gripper-transform*
  (cl-transforms-stamped:make-identity-transform))

(defparameter *left-parking-joint-states*
  '(("LShoulderPitch" 1.08)
    ("LShoulderRoll" 0.51)
    ("LElbowYaw" -0.15)
    ("LElbowRoll" -0.79)
    ("LWristYaw" 0.00)))

(defparameter *right-parking-joint-states*
  '(("RShoulderPitch" 1.08)
    ("RShoulderRoll" -0.53)
    ("RElbowYaw" 0.0)
    ("RElbowRoll" 0.79)
    ("RWristYaw" 0.0)))

(defparameter *left-pointing-joint-states*
  '(("LShoulderPitch" -0.46)
    ("LShoulderRoll" 0.99)
    ("LElbowYaw" -1.18)
    ("LElbowRoll" -0.01)
    ("LWristYaw" 0.00)))

(defparameter *right-pointing-joint-states*
  '(("RShoulderPitch" -0.41)
    ("RShoulderRoll" -1.00)
    ("RElbowYaw" 0.50)
    ("RElbowRoll" 0.01)
    ("RWristYaw" 0.91)))

(defparameter *straight-pointing-joint-states*
  '(("RShoulderPitch" -0.13)
    ("RShoulderRoll" -0.37)
    ("RElbowYaw" 0.46)
    ("RElbowRoll" 0.69)
    ("RWristYaw" 1.82)))


(def-fact-group pepper-arm-facts (end-effector-link
                                robot-tool-frame
                                arm-joints arm-links
                                gripper-joint gripper-link
                                gripper-meter-to-joint-multiplier
                                standard-to-particular-gripper-transform
                                robot-joint-states)

  (<- (end-effector-link pepper :left "LThumb1_link"))
  (<- (end-effector-link pepper :right "RThumb1_link"))

  (<- (robot-tool-frame pepper :left "LThumb1"))
  (<- (robot-tool-frame pepper :right "RThumb1"))

  (<- (arm-joints pepper :left ("LShoulderPitch"
                              "LShoulderRoll"
                              "LElbowYaw"
                              "LElbowRoll"
                              "LWristYaw")))
  (<- (arm-joints pepper :right ("RShoulderPitch"
                              "RShoulderRoll"
                              "RElbowYaw"
                              "RElbowRoll"
                              "RWristYaw")))

  (<- (arm-links pepper :left ("LShoulder"
                             "LBicep"
                             "LElbow"
                             "LForeArm"
                             "l_wrist")))
  (<- (arm-links pepper :right ("RShoulder"
                             "RBicep"
                             "RElbow"
                             "RForeArm"
                             "r_wrist")))

  (<- (gripper-joint pepper :left "left_gripper_joint"))
  (<- (gripper-joint pepper :right "right_gripper_joint"))

  (<- (gripper-link pepper :left ?link)
    (bound ?link)
    (lisp-fun search "left_gripper" ?link ?pos)
    (lisp-pred identity ?pos))
  (<- (gripper-link pepper :right ?link)
    (bound ?link)
    (lisp-fun search "right_gripper" ?link ?pos)
    (lisp-pred identity ?pos))

  (<- (gripper-meter-to-joint-multiplier pepper 1.0))

  (<- (standard-to-particular-gripper-transform pepper ?transform)
    (symbol-value *standard-to-pepper-gripper-transform* ?transform))

  (<- (robot-joint-states pepper :arm :left :point ?joint-states)
    (symbol-value *left-pointing-joint-states* ?joint-states))

  (<- (robot-joint-states pepper :arm :right :point ?joint-states)
    (symbol-value *right-pointing-joint-states* ?joint-states))

  (<- (robot-joint-states pepper :arm :right :point-ahead ?joint-states)
    (symbol-value *straight-pointing-joint-states* ?joint-states))

  (<- (robot-joint-states pepper :arm :left :park ?joint-states)
    (symbol-value *left-parking-joint-states* ?joint-states))

  (<- (robot-joint-states pepper :arm :right :park ?joint-states)
    (symbol-value *right-parking-joint-states* ?joint-states)))

Similarly, we describe the joints and links of the arms of the Pepper robot. We establish certain positions that will be relevant to the execution of our tasks.

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.

(in-package :pepper-descr)

(def-fact-group pepper-metadata (robot
                               robot-odom-frame
                               robot-base-frame robot-torso-link-joint
                               arm
                               camera-frame
                               camera-minimal-height
                               camera-maximal-height)
  (<- (robot pepper))

  (<- (robot-odom-frame pepper "odom"))

  (<- (robot-base-frame pepper "base_link"))
  (<- (robot-torso-link-joint pepper "torse" "base_link_fixedjoint"))

  (<- (arm pepper :left))
  (<- (arm pepper :right))

  (<- (camera-frame pepper "CameraDepth_optical_frame"))

  (<- (camera-minimal-height pepper 1.00))
  (<- (camera-maximal-height pepper 1.10))
  )
  

; Gaussian distribution configuration
(def-fact-group location-costmap-metadata ( costmap:costmap-size
                                            costmap:costmap-origin
                                            costmap:costmap-resolution
                                            costmap:orientation-samples
                                            costmap:orientation-sample-step
                                            costmap:costmap-padding
                                            costmap:costmap-manipulation-padding
                                            costmap:costmap-in-reach-distance
                                            costmap:costmap-reach-minimal-distance
                                            costmap:visibility-costmap-size)
    (<- (costmap:costmap-size 12 12))
    (<- (costmap:costmap-origin -6 -6))
    (<- (costmap:costmap-resolution 0.04))
 
    (<- (costmap:costmap-padding 0.3))
    (<- (costmap:costmap-manipulation-padding 0.4))
    (<- (costmap:costmap-in-reach-distance 0.7))
    (<- (costmap:costmap-reach-minimal-distance 0.2))
    (<- (costmap:visibility-costmap-size 2))
    (<- (costmap:orientation-samples 2))
    (<- (costmap:orientation-sample-step 0.1)))

In this file, we provide the robot's name, robot base-frame, and other necessary details. We also offer some features about the costmaps we will be using for our implementation.

Now let’s see if we can load our cram package in Emacs.

First, run the following.

$ roslaunch pepper_description upload.launch

Next,

$ roslisp_repl

In the Emacs, run the following to verify that there is no error in our setup.

CL-USER> (ros-load:load-system "cram_pepper_description" :cram-pepper-description)

Load the package

CL-USER> (in-package :pepper-descr)

If successful, you should see the following in red.

#<PACKAGE "CRAM-PEPPER-DESCRIPTION">

To be able to visualize our robot and shelves, let's set up our demo folder.

CRAM Pepper Demo

Just like the cram_pepper_description, let’s create a new package in cram_pepper called cram_pepper_demo.

First, change the directory to “cram_pepper.”

$ cd path-to-your-workspace/ros/src/cram/cram_pepper 

Now let us create a ROS package called cram_pepper_demo

$ catkin_create_pkg cram_pepper_demo

Your results should look like below.

Created file cram_pepper_demo/CMakeLists.txt
Created file cram_pepper_demo/package.xml
Successfully created files in path-to-your-workspace/ros/src/cram/cram_pepper/cram_pepper_demo. Please adjust the values in package.xml.

As explained before, open the package.xml file in any text editor and replace everything with the following code.

<package format="2">
  <name>cram_pepper_demo</name>
  <version>0.7.0</version>
  <description>Demo code for shop assistant with pepper</description>
  <author>Your Name</author>
  <maintainer email="youremail@gmail.com">Your Name</maintainer>
  <license>BSD</license>

  <url type="website">http://cram-system.org</url>
  <url type="repository">http://github.com/cram2/cram</url>

  <buildtool_depend>catkin</buildtool_depend>

  <depend>roslisp_utilities</depend>

  <depend>cl_transforms</depend>
  <depend>cl_transforms_stamped</depend>
  <depend>cl_tf</depend>
  <depend>cl_tf2</depend>
  <depend>cram_tf</depend>

  <depend>cram_language</depend>
  <depend>cram_executive</depend>
  <depend>cram_designators</depend>
  <depend>cram_prolog</depend>
  <depend>cram_projection</depend>
  <depend>cram_occasions_events</depend>
  <depend>cram_utilities</depend>

  <depend>cram_common_failures</depend>
  <depend>cram_mobile_pick_place_plans</depend>
  <depend>cram_robot_interfaces</depend>
  <depend>cram_object_knowledge</depend>

  <depend>cram_physics_utils</depend>
  <depend>cl_bullet</depend>
  <depend>cram_bullet_reasoning</depend>
  <depend>cram_bullet_reasoning_belief_state</depend>
  <depend>cram_bullet_reasoning_utilities</depend>

  <depend>cram_robot_pose_gaussian_costmap</depend>
  <depend>cram_occupancy_grid_costmap</depend>
  <depend>cram_location_costmap</depend>
  <depend>cram_manipulation_interfaces</depend>

  <depend>cram_urdf_projection</depend>
  <depend>cram_pepper_description</depend>
  <depend>cram_fetch_deliver_plans</depend>

  <!-- launch file -->
  <exec_depend>iai_maps</exec_depend>
  <exec_depend>iai_kitchen</exec_depend>
  <exec_depend>map_server</exec_depend>
  <exec_depend>tf2_ros</exec_depend>
  
</package>

Next, replace open the CmakeLists.txt in any text editor and replace everything with the following code.

cmake_minimum_required(VERSION 2.8.3)
project(cram_pepper_demo)
find_package(catkin REQUIRED)
catkin_package()

Now let’s compile to be sure everything is correctly setup. If you run into any errors, you might want to go back and recheck every step.

Creating an ASDF system for the cram_pepper_demo

Create a file cram-pepper-demo.asd and put the following code in it.

(defsystem cram-pepper-demo
  :author "yourname"
  :license "BSD"
 
  :depends-on (roslisp-utilities ; for ros-init-function
 
               cl-transforms
               cl-transforms-stamped
               cl-tf
               cl-tf2
               cram-tf
 
               cram-language
               cram-executive
               cram-designators
               cram-prolog
               cram-projection
               cram-occasions-events
               cram-utilities ; for EQUALIZE-LISTS-OF-LISTS-LENGTHS
 
               cram-common-failures
               cram-mobile-pick-place-plans
               cram-robot-interfaces ; for *robot-urdf*
               cram-object-knowledge
 
               cram-physics-utils ; for reading "package://" paths
               cl-bullet ; for handling BOUNDING-BOX datastructures
               cram-bullet-reasoning
               cram-bullet-reasoning-belief-state
               cram-bullet-reasoning-utilities
               cram-btr-visibility-costmap
 
               cram-robot-pose-gaussian-costmap
               cram-occupancy-grid-costmap
               cram-location-costmap
               cram-manipulation-interfaces ; for standard rotations
 
               cram-urdf-projection      ; for with-simulated-robot
               cram-pepper-description
               cram-process-modules
               cram-fetch-deliver-plans
 
               )
 
  :components
  ((:module "src"
    :components
    ((:file "package")
     (:file "setup" :depends-on ("package"))
     (:file "projection-demo" :depends-on ("package"))
     (:file "cram-plans" :depends-on ("package"))
     (:file "shelf-prolog" :depends-on ("package"))
     ))))

Next, we have to create the files listed in the components section in our src folder. However, we don't have an src folder, so we go ahead and create one in our cram_pepper_demo package.

$ mkdir src

Let’s create the files; package.lisp, setup.lisp, projection-demo.lisp, cram-plans, and shelf-prolog.lisp in the src folder. Like the previous package setup, this is a design decision for easy reading.

Now, let’s define our package in the “package.lisp” file with the following code.

(in-package :cl-user)
 
(defpackage cram-pepper-description
  (:nicknames #:pepper-descr)
  (:use #:common-lisp #:cram-prolog #:cram-robot-interfaces)
  (:export #:pepper))

For the rest of the files, make sure to have the following line of code at the very top of the file and save.

(in-package :demo)

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.

Compile the project, make sure to have the “roslaunch” running at the background, and open Emacs.

In Emacs, let’s load our pepper demo package.

CL-USER> (ros-load:load-system "cram_pepper_demo" :cram-pepper-demo)

Load the package

CL-USER> (in-package :demo)

If successful, you should see the following in red.

#<PACKAGE "CRAM-PEPPER-DEMO">

Now let's update our setup file to automatically setup our bullet world and import the shelves and the Pepper robot. Instead of manually setting up every single element in the environment, we want to save time by running only one line of code. Therefore, copy the following code and paste it into the “setup.lisp” file.

(in-package :demo)
 
(defvar *kitchen-urdf* nil)
(defparameter *robot-parameter* "robot_description")
(defparameter *kitchen-parameter* "shelf_description")
 
(defun setup-bullet-world ()
  (setf btr:*current-bullet-world* (make-instance 'btr:bt-reasoning-world))
 
  (let ((robot (or rob-int:*robot-urdf*
                   (setf rob-int:*robot-urdf*
                         (cl-urdf:parse-urdf
                          (roslisp:get-param *robot-parameter*)))))
        (kitchen (or *kitchen-urdf*
                     (let ((kitchen-urdf-string
                             (roslisp:get-param *kitchen-parameter* nil)))
                       (when kitchen-urdf-string
                         (setf *kitchen-urdf* (cl-urdf:parse-urdf
                                               kitchen-urdf-string)))))))
    ;; set pepper URDF root link to be base_footprint not odom,
    ;; as with odom lots of problems concerning object-pose in bullet happen
    (setf (slot-value rob-int:*robot-urdf* 'cl-urdf:root-link)
          (or (gethash cram-tf:*robot-base-frame*
                       (cl-urdf:links rob-int:*robot-urdf*))
              (error "[setup-bullet-world] cram-tf:*robot-base-frame* was undefined or smt.")))
 
 
    (assert
     (cut:force-ll
      (prolog `(and
                (btr:bullet-world ?w)
                (btr:debug-window ?w)
                (btr:assert ?w (btr:object :static-plane :floor ((0 0 0) (0 0 0 1))
                                                         :normal (0 0 1) :constant 0))
                (btr:assert ?w (btr:object :urdf :kitchen ((0 0 0) (0 0 0 1))
                                                 :collision-group :static-filter
                                                 :collision-mask (:default-filter
                                                                  :character-filter)
                                                 :urdf ,kitchen
                                                 :compound T))
                (-> (cram-robot-interfaces:robot ?robot)
                    (btr:assert ?w (btr:object :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf ,robot))
                    (warn "ROBOT was not defined. Have you loaded a robot package?")))))))
 
  )
 
(defun init-projection ()
  (def-fact-group costmap-metadata (costmap:costmap-size
                                    costmap:costmap-origin
                                    costmap:costmap-resolution
                                    costmap:orientation-samples
                                    costmap:orientation-sample-step)
    (<- (costmap:costmap-size 12 12))
    (<- (costmap:costmap-origin -6 -6))
    (<- (costmap:costmap-resolution 0.04))
    (<- (costmap:orientation-samples 2))
    (<- (costmap:orientation-sample-step 0.1)))
 
 
  (setup-bullet-world)
 
  (setf cram-tf:*tf-default-timeout* 2.0)
 
  (setf prolog:*break-on-lisp-errors* t)
 
  (cram-bullet-reasoning:clear-costmap-vis-object)
 
  (btr:add-objects-to-mesh-list "cram_pepper_demo" :directory "resource" :extension "stl"))
 
(roslisp-utilities:register-ros-init-function init-projection)
(defvar *kitchen-urdf* nil)
(defparameter *robot-parameter* "robot_description")
(defparameter *kitchen-parameter* "shelf_description")

Here, we are defining global variables for our robot and shelves (kitchen-parameter).

(defun setup-bullet-world ()
  (setf btr:*current-bullet-world* (make-instance 'btr:bt-reasoning-world))
 
  (let ((robot (or rob-int:*robot-urdf*
                   (setf rob-int:*robot-urdf*
                         (cl-urdf:parse-urdf
                          (roslisp:get-param *robot-parameter*)))))
        (kitchen (or *kitchen-urdf*
                     (let ((kitchen-urdf-string
                             (roslisp:get-param *kitchen-parameter* nil)))
                       (when kitchen-urdf-string
                         (setf *kitchen-urdf* (cl-urdf:parse-urdf
                                               kitchen-urdf-string)))))))
    ;; set pepper URDF root link to be base_footprint not odom,
    ;; as with odom lots of problems concerning object-pose in bullet happen
    (setf (slot-value rob-int:*robot-urdf* 'cl-urdf:root-link)
          (or (gethash cram-tf:*robot-base-frame*
                       (cl-urdf:links rob-int:*robot-urdf*))
              (error "[setup-bullet-world] cram-tf:*robot-base-frame* was undefined or smt.")))
 
 
    (assert
     (cut:force-ll
      (prolog `(and
                (btr:bullet-world ?w)
                (btr:debug-window ?w)
                (btr:assert ?w (btr:object :static-plane :floor ((0 0 0) (0 0 0 1))
                                                         :normal (0 0 1) :constant 0))
                (btr:assert ?w (btr:object :urdf :kitchen ((0 0 0) (0 0 0 1))
                                                 :collision-group :static-filter
                                                 :collision-mask (:default-filter
                                                                  :character-filter)
                                                 :urdf ,kitchen
                                                 :compound T))
                (-> (cram-robot-interfaces:robot ?robot)
                    (btr:assert ?w (btr:object :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf ,robot))
                    (warn "ROBOT was not defined. Have you loaded a robot package?")))))))
 
  )
<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.
 
<code lisp>
(defun init-projection ()
  (def-fact-group costmap-metadata (costmap:costmap-size
                                    costmap:costmap-origin
                                    costmap:costmap-resolution
                                    costmap:orientation-samples
                                    costmap:orientation-sample-step)
    (<- (costmap:costmap-size 12 12))
    (<- (costmap:costmap-origin -6 -6))
    (<- (costmap:costmap-resolution 0.04))
    (<- (costmap:orientation-samples 2))
    (<- (costmap:orientation-sample-step 0.1)))
 
 
  (setup-bullet-world)
 
  (setf cram-tf:*tf-default-timeout* 2.0)
 
  (setf prolog:*break-on-lisp-errors* t)
 
  (cram-bullet-reasoning:clear-costmap-vis-object)
 
  (btr:add-objects-to-mesh-list "cram_pepper_demo" :directory "resource" :extension "stl"))

This function is used to reinitialize the bullet-world in case there is a need for it.

(roslisp-utilities:register-ros-init-function init-projection)

Lastly, we need to register the init-projection function so that it can be recognized in CRAM.

Now, let’s close Emacs, compile the project, open Emacs, and load the cram_pepper_demo package in Emacs.

Load the demo package, and this time run the following code.

DEMO> (roslisp-utilities:startup-ros)

You should see the bullet visualization window with the shelves and robot loaded like the image below.

Let us confirm that we have everything in the bullet-world. Type the following code in the Emacs window.

DEMO> btr:*current-bullet-world*

Your results should appear like below in red.

#<CRAM-BULLET-REASONING:BT-REASONING-WORLD {100552AF63}>

Right-click on the results and select inspect.

A window appears below the current working window. It presents the CRAM-BULLET-REASONING with the various slots. Click on the hash-table corresponding to the objects slot highlighted in green in the image below.

Clicking on it will present the objects present in the bullet-world. We can see that we have our floor, the shelves (kitchen), and Pepper from the image below.

Note that for most results in red, you can inspect it—hit q to exit/close that window. In the future, you can use this method to check the objects in the bullet-world.

Now let’s try and move the robot 2 meters forward (x-axis). Let’s run the following code in the Emacs window.

(prolog:prolog '(and (btr:bullet-world ?world)
                              (assert (btr:object-pose ?world cram-pepper-description:pepper
                                                       ((2 0 0) (0 0 0 1))))))

If it was successful, your results should look like below.

(((?WORLD . #<CRAM-BULLET-REASONING:BT-REASONING-WORLD {10039D64B3}>))
 . #S(CRAM-UTILITIES::LAZY-CONS-ELEM
      :GENERATOR #<CLOSURE (LAMBDA () :IN CRAM-UTILITIES:LAZY-MAPCAR)
                   {1009BA16FB}>))

Now that the robot seems to be moving let’s import a box of cereal onto the shelf. Run the code below in your Emacs window.

(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-cereal1 ((4.1 0 1.03) (0 0 1 1))
			:mass 0 :color (1 0 0) :mesh :breakfast-cereal))))

The code above imports an object of a mesh type with a name “my-cereal1”, pose "((4.1 0 1.03) (0 0 1 1))", a mass of “0”, a color of “(1 0 0)” (thus red), and category of the product is breakfast-cereal.

You should see a red box of cereal in front of the robot, like the image below.

Now that we have imported a product let's introduce the rest of the products to the shelves. To do this, copy the code below and paste it in the “cram-plans.lisp” file.

(defun spawn-products-one ()
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-cereal1 ((4.1 0 1.03) (0 0 1 1))
			:mass 0 :color (1 0 0) :mesh :breakfast-cereal))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-cereal2 ((2 -1.4 1.05) (0 0 1 1))
			:mass 0 :color (1 0 0) :mesh :breakfast-cereal))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-cereal3 ((4.1 0.15 1.73) (0 0 1 1))
			:mass 0 :color (1 0 0) :mesh :breakfast-cereal))))
 
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-sachet1 ((4.1 -0.55 1.03) (0 0 1 1))
			:mass 0 :color (0 0.3 0) :mesh :somat))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-sachet2 ((4.1 0.15 1.4) (0 0 1 1))
			:mass 0 :color (0 0.3 0) :mesh :somat))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-sachet3 ((4.1 -0.3 1.73) (0 0 1 1))
			:mass 0 :color (0 0.3 0) :mesh :somat))))
 
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-milk1 ((4.1 -0.55 1.4) (0 0 1 1))
			:mass 0 :color (1 0 1) :mesh :milk))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-milk2 ((1.5 -1.4 1.37) (0 0 1 1))
			:mass 0 :color (1 0 1) :mesh :milk))))
 
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-bowl1 ((1.5 -1.4 0.52) (0 0 1 1))
			:mass 0 :color (0 0.9 0.9) :mesh :bowl))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-bowl2 ((1.8 -1.4 0.52) (0 0 1 1))
			:mass 0 :color (0 0.9 0.9) :mesh :bowl))))
 
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-bar1 ((1.5 -1.4 0.77) (0 0 1 1))
			:mass 0 :color (0.5 0 0.1) :mesh :denkmit))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-bar2 ((1.8 -1.4 0.77) (0 0 1 1))
			:mass 0 :color (0.5 0 0.1) :mesh :denkmit))))
 
	)

Save and compile the package. With the bring-up still running on the background, restart Emacs, load the cram-pepper-demo package, and load up the scene. Let’s spawn all the products. Run the code below.

DEMO> (spawn-products-one)

Your results should look like below.

So far, so good.

Pack arms

At this point, we don’t want our robot arm looking a zombie. So let’s naturally park the arms. Let's create another function in our “cram-plans.lisp” file. Copy the following code and paste it in the “cram-plans.lisp” file.

(defun park-arms()
	(cram-executive:perform
		(desig:an action
			(type positioning-arm)
			(left-configuration park)
			(right-configuration park))))

Save and compile the package and set up the scene. Let's run our new function.

If we run the function in the Emacs terminal, we will get an error like the image below.

This error means that we need to specify which platform we want to run the code. That is, whether on a real robot or a simulated robot. Therefore, we need to wrap “cram-urdf-projection:with-simulated-robot” around the function call, as shown below.

DEMO> (cram-urdf-projection:with-simulated-robot (park-arms))

Your results should look like below.

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.

(defun spawn-human()
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-human ((-1 0 1) (0 0 1 1))
			:mass 0.2 :color (1 1 0) :mesh :body :scale 0.3)))))

Let's save and compile the package. To do this without having to leave Emacs, we have to open the “cram-plans.lisp” file in Emacs. Click on File→Open File. In the pop-up Dialog box, navigate the “cram-plans.lisp” file in your workspace. Open it and hit Ctrl + c and Ctrl + k to compile the entire file. If there is any error, you will be alerted to that. To return to the previous buffer, click on “Buffers” at the top menu and select “*slime-repl sbcl**.” In this current window, you will see the success of the file compilation. Now we can run the newly created function. Let's call the function. Enter the following code:

DEMO> (spawn-human)

Your results should be like this, and you should see a yellow human behind the robot.

(((?WORLD . #<CRAM-BULLET-REASONING:BT-REASONING-WORLD {100538FAF3}>))
 . #S(CRAM-UTILITIES::LAZY-CONS-ELEM
      :GENERATOR #<CLOSURE (LAMBDA () :IN CRAM-UTILITIES:LAZY-MAPCAR)
                   {100643B36B}>))

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

To reposition the robot, let's add the following function to the “cram-plans.lisp” file.

(defun reposition-robot()
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object-pose ?world cram-pepper-description:pepper
			((0 0 0) (0 0 1 0)))))))

Save and compile the file. Run the function in the Emacs window.

DEMO> (reposition-robot)

Your results should look like the following.

(((?WORLD . #<CRAM-BULLET-REASONING:BT-REASONING-WORLD {1012594BE3}>))
 . #S(CRAM-UTILITIES::LAZY-CONS-ELEM
      :GENERATOR #<CLOSURE (LAMBDA () :IN CRAM-UTILITIES:LAZY-MAPCAR)
                   {100FD17F5B}>))

Now we are ready to perform some functions.

Since we are creating a shopping assistant, the robot will have to get some information about the product the human is looking for. The code below is some fundamental interactions the robot uses to get the needed information. Copy the code and paste in the “projection-demo.lisp” file.

;; Menu functions start
(defun menu-one()
 (princ "Hello my name is Pepper and how may I help you?
		Please use the number corresponding to your request.
 
		------Select a number-----
		1. Help me find a product.
		2. I just wanted to say hi."))
 
(defun menu-two()
	(princ "What do you want?
		Please use the number corresponding to your request.
 
		------Select a number-----
		1. Cereal.
		2. Milk.
		3. I think I am fine."))

Now let's create a function called “interaction” which will call all the functions we have created so far. It will also house all other functions that will be creating in the future. Copy the code below and paste it in the “projection-demo.lisp” file.

;; Interaction execution
(defun interaction()
	(cram-urdf-projection:with-simulated-robot (park-arms))
	(reposition-robot)
	(menu-one)
) ;;interaction close brackets

We want to run the “spawn-human” function whenever we run the “spawn-products-one” function. Therefore, let's add the “spawn-human” function at the end of the “spawn-products-one” function. The last line of your new “spawn-products-one” function should look like the code below.

(defun spawn-products-one ()
 
	…..
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-bar1 ((1.5 -1.4 0.77) (0 0 1 1))
			:mass 0 :color (0.5 0 0.1) :mesh :denkmit))))
	(prolog:prolog '(and (btr:bullet-world ?world)
		(assert (btr:object ?world :mesh :my-bar2 ((1.8 -1.4 0.77) (0 0 1 1))
			:mass 0 :color (0.5 0 0.1) :mesh :denkmit))))
 
	(spawn-human)
	)

Now let's compile and restart our environment so we can test our new functions and modifications. To reinitialize the entire scene, run the code below, and compile the two files we modified without leaving Emacs.

(init-projection)

After the reinitialization and file compilations, let's test the functions. First, run the “spawn-products-one” function, so we verify that the function spawns the human as well. If it doesn't work for you, please go back and make sure that all you didn't miss any step.

Next, run the “interaction” function, as shown below. This function should park the robot's arms, reposition the robot to face the human, present the menu one, and then end. So far, so good.

Now let’s take user input and store it. Copy the code below and paste it after the “menu-one” function call in the “interaction” function.

(setf response (read t))
 
	;;first conditional statement start
	(cond ((= response 1)
			(menu-two)
 
  ;; second conditional statement start
 
		;;second conditional statement end
 
		)
		((= response 2 )
			(format t "That's nice of you. Thanks!")
			)
		);;first conditional statement end

From the code above, we first create a variable “response” to store the user input. If the user chooses 1, we present the second menu else we print a message to the user.

Now when the user selects 1 as the first option and presents the second menu, we want to take another response from the user. Copy the code below and paste it in the “second conditional statement” comment section.

		;;second conditional statement start
		(setf proresponse (read t))
		(cond ((= proresponse 1)
			(format t "Alright. Please follow me!"))
		((= proresponse 2)
			(format t "Alright. Please follow me!"))
		)
		;;second conditional statement end

Now when we compile and select one for the first input, we should get a request for the second menu, as shown in the image below.

Now we need a variable to store the name of the product the user is looking for. For this to happen, let's create a local variable using the “Let” function of lisp. The “interaction” function should now look like below.

;; Interaction execution
(defun interaction()
	(cram-urdf-projection:with-simulated-robot (park-arms))
	(reposition-robot)
	(menu-one)
 
	(let* ((?productname 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)
				(format t "Alright. Please follow me!"))
			((= proresponse 2)
				(setf ?productname 'milk)
				(format t "Alright. Please follow me!"))
			)
			;;second conditional statement end
 
			)
			((= response 2 )
				(format t "That's nice of you. Thanks!"))
			);;first conditional statement end
 
	) ;;let close brackets
 ) ;;interaction close brackets

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.