Differences
This shows you the differences between two versions of the page.
| Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
| tutorials:intermediate:pepper_shopping [2020/05/13 14:47] – added an example of lisp code gkazhoya | tutorials:intermediate:pepper_shopping [2022/02/08 14:11] (current) – [Creating an ASDF system for the cram_pepper_demo] sarthou | ||
|---|---|---|---|
| Line 1: | Line 1: | ||
| - | ====== Pepper | + | ====== Pepper |
| - | ===== This is an example section title ===== | + | ===== Setting Up The Workspace |
| - | ==== This is then a subsection | + | |
| - | | + | 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. Please note that Ubuntu 16.04 was used for this project. | ||
| + | |||
| + | ==== VM Set Up ==== | ||
| + | |||
| + | Open the terminal by just typing ctrl+alt+t and then type and execute the following command: | ||
| + | |||
| + | < | ||
| + | $ sudo apt-get update && sudo apt-get upgrade | ||
| + | </ | ||
| + | |||
| + | Let’s update ROS and CRAM components. In the terminal type and execute the following commands: | ||
| + | |||
| + | < | ||
| + | $ sudo apt-get 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-get install ros-kinetic-roslisp-common | ||
| + | </ | ||
| + | |||
| + | 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. | ||
| + | |||
| + | < | ||
| + | $ cd ~/ | ||
| + | $ git pull | ||
| + | $ 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 | ||
| + | |||
| + | 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 | ||
| + | |||
| + | < | ||
| + | $ cd ~/ | ||
| + | $ catkin_create_pkg pepper_description | ||
| + | </ | ||
| + | |||
| + | The terminal should be looking like the sample below. | ||
| + | |||
| + | < | ||
| + | Created file pepper_description/ | ||
| + | Created file pepper_description/ | ||
| + | Successfully created files in / | ||
| + | </ | ||
| + | |||
| + | Next, we are going to adjust the values in the // | ||
| + | |||
| + | <code XML> | ||
| + | <?xml version=" | ||
| + | <package format=" | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | < | ||
| + | |||
| + | < | ||
| + | |||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| | | ||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | </ | ||
| + | </ | ||
| + | |||
| + | Next, we edit the // | ||
| + | |||
| + | < | ||
| + | find_package(catkin REQUIRED COMPONENTS | ||
| + | urdf | ||
| + | controller_manager | ||
| + | joint_state_controller | ||
| + | robot_state_publisher | ||
| + | ) | ||
| + | </ | ||
| + | |||
| + | Rename the file "// | ||
| + | |||
| + | Next, open this file: "// | ||
| + | |||
| + | Comment out the following code by putting semi-colon in front of them. It should be on line 40 - 43. | ||
| + | |||
| + | <code lisp> | ||
| + | ; (desig: | ||
| + | ; 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 [[https:// | ||
| + | |||
| + | Now let’s explain the code in the // | ||
| + | |||
| + | < | ||
| + | <include file=" | ||
| + | </ | ||
| + | |||
| + | This line uploads the occupancy map for navigation. It is the environment or free space for the robot to move around. | ||
| + | |||
| + | < | ||
| + | <param name=" | ||
| + | <param name=" | ||
| + | </ | ||
| + | |||
| + | 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=" | ||
| + | <node name=" | ||
| + | <remap from=" | ||
| + | </ | ||
| + | </ | ||
| + | |||
| + | |||
| + | 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=" | ||
| + | <node pkg=" | ||
| + | <param name=" | ||
| + | <remap from=" | ||
| + | </ | ||
| + | </ | ||
| + | |||
| + | This line is also a ROS node to publish the robot state values of the robot and the shelves. | ||
| + | |||
| + | < | ||
| + | <node pkg=" | ||
| + | <node pkg=" | ||
| + | </ | ||
| + | |||
| + | 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, | ||
| + | |||
| + | 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 // | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | 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 " | ||
| + | |||
| + | The next one is the // | ||
| + | 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/ | ||
| + | |||
| + | < | ||
| + | $ mkdir -p path-to-your-workspace/ | ||
| + | </ | ||
| + | |||
| + | Change directory to " | ||
| + | |||
| + | < | ||
| + | $ cd path-to-your-workspace/ | ||
| + | </ | ||
| + | |||
| + | 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/ | ||
| + | Created file cram_pepper_description/ | ||
| + | Successfully created files in path-to-your-workspace/ | ||
| + | </ | ||
| + | |||
| + | Since we have already done an example, let’s go ahead and replace everything in the package.xml with the following: | ||
| + | |||
| + | <code XML> | ||
| + | <package format=" | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | Prolog predicates describing pepper robot | ||
| + | </ | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | </ | ||
| + | </ | ||
| + | |||
| + | 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 // | ||
| + | |||
| + | <code lisp> | ||
| + | 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 // | ||
| + | |||
| + | <code lisp> | ||
| + | (defsystem cram-pepper-description | ||
| + | :depends-on (cram-prolog | ||
| + | | ||
| + | | ||
| + | ) | ||
| + | :components | ||
| + | ((:module " | ||
| + | :components | ||
| + | ((:file " | ||
| + | | ||
| + | | ||
| + | | ||
| + | )))) | ||
| + | </ | ||
| + | |||
| + | 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 // | ||
| + | |||
| + | < | ||
| + | $ mkdir src | ||
| + | </ | ||
| + | |||
| + | Let's create the files; // | ||
| + | |||
| + | Let’s define our package in the "// | ||
| + | |||
| + | <code lisp> | ||
| + | (in-package :cl-user) | ||
| + | |||
| + | (defpackage cram-pepper-description | ||
| + | (:nicknames #: | ||
| + | (:use #: | ||
| + | (: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 [[tutorials: | ||
| + | |||
| + | In the “// | ||
| + | |||
| + | <code lisp> | ||
| + | (in-package : | ||
| + | |||
| + | (defparameter *neck-good-looking-down-state* | ||
| + | ' | ||
| + | (" | ||
| + | |||
| + | (defparameter *neck-good-looking-left-state* | ||
| + | ' | ||
| + | (" | ||
| + | |||
| + | (defparameter *neck-parking-joint-states* | ||
| + | ' | ||
| + | (" | ||
| + | |||
| + | |||
| + | (def-fact-group pepper-neck-facts (robot-neck-links | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | |||
| + | (<- (robot-neck-links pepper | ||
| + | " | ||
| + | " | ||
| + | |||
| + | (<- (robot-neck-joints pepper | ||
| + | " | ||
| + | " | ||
| + | |||
| + | (<- (robot-joint-states pepper :neck ? | ||
| + | (symbol-value *neck-parking-joint-states* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :neck ? | ||
| + | (symbol-value *neck-parking-joint-states* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :neck ? | ||
| + | (symbol-value *neck-good-looking-down-state* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :neck ? | ||
| + | (symbol-value *neck-good-looking-left-state* ? | ||
| + | | ||
| + | </ | ||
| + | |||
| + | 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 "// | ||
| + | |||
| + | <code lisp> | ||
| + | (in-package : | ||
| + | |||
| + | (defparameter *standard-to-pepper-gripper-transform* | ||
| + | (cl-transforms-stamped: | ||
| + | |||
| + | (defparameter *left-parking-joint-states* | ||
| + | ' | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | |||
| + | (defparameter *right-parking-joint-states* | ||
| + | ' | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | |||
| + | (defparameter *left-pointing-joint-states* | ||
| + | ' | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | |||
| + | (defparameter *right-pointing-joint-states* | ||
| + | ' | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | |||
| + | (defparameter *straight-pointing-joint-states* | ||
| + | ' | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | (" | ||
| + | |||
| + | |||
| + | (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 " | ||
| + | (<- (end-effector-link pepper :right " | ||
| + | |||
| + | (<- (robot-tool-frame pepper :left " | ||
| + | (<- (robot-tool-frame pepper :right " | ||
| + | |||
| + | (<- (arm-joints pepper :left (" | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | (<- (arm-joints pepper :right (" | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | |||
| + | (<- (arm-links pepper :left (" | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | (<- (arm-links pepper :right (" | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | " | ||
| + | |||
| + | (<- (gripper-joint pepper :left " | ||
| + | (<- (gripper-joint pepper :right " | ||
| + | |||
| + | (<- (gripper-link pepper :left ?link) | ||
| + | (bound ?link) | ||
| + | (lisp-fun search " | ||
| + | (lisp-pred identity ?pos)) | ||
| + | (<- (gripper-link pepper :right ?link) | ||
| + | (bound ?link) | ||
| + | (lisp-fun search " | ||
| + | (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* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :arm :left :point ? | ||
| + | (symbol-value *left-pointing-joint-states* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :arm :right :point ? | ||
| + | (symbol-value *right-pointing-joint-states* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :arm :right : | ||
| + | (symbol-value *straight-pointing-joint-states* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :arm :left :park ? | ||
| + | (symbol-value *left-parking-joint-states* ? | ||
| + | |||
| + | (<- (robot-joint-states pepper :arm :right :park ? | ||
| + | (symbol-value *right-parking-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 "// | ||
| + | |||
| + | <code lisp> | ||
| + | (in-package : | ||
| + | |||
| + | (def-fact-group pepper-metadata (robot | ||
| + | | ||
| + | | ||
| + | arm | ||
| + | | ||
| + | | ||
| + | | ||
| + | (<- (robot pepper)) | ||
| + | |||
| + | (<- (robot-odom-frame pepper " | ||
| + | |||
| + | (<- (robot-base-frame pepper " | ||
| + | (<- (robot-torso-link-joint pepper " | ||
| + | |||
| + | (<- (arm pepper :left)) | ||
| + | (<- (arm pepper :right)) | ||
| + | |||
| + | (<- (camera-frame pepper " | ||
| + | |||
| + | (<- (camera-minimal-height pepper 1.00)) | ||
| + | (<- (camera-maximal-height pepper 1.10)) | ||
| + | ) | ||
| + | | ||
| + | |||
| + | ; Gaussian distribution configuration | ||
| + | (def-fact-group location-costmap-metadata ( costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | |||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | |||
| + | </ | ||
| + | |||
| + | In this file, we provide the robot' | ||
| + | |||
| + | Now let’s see if we can load our cram package in Emacs. | ||
| + | |||
| + | First, run the following in your terminal. | ||
| + | |||
| + | < | ||
| + | $ roslaunch pepper_description upload.launch | ||
| + | </ | ||
| + | |||
| + | Next, | ||
| + | |||
| + | < | ||
| + | $ roslisp_repl | ||
| + | </ | ||
| + | |||
| + | In the Emacs, run the following to verify that there is no error in our setup. | ||
| + | |||
| + | <code lisp> | ||
| + | CL-USER> (ros-load: | ||
| + | </ | ||
| + | |||
| + | Load the package | ||
| + | |||
| + | <code lisp> | ||
| + | CL-USER> (in-package : | ||
| + | </ | ||
| + | |||
| + | If successful, you should see the following in red. | ||
| + | <code lisp> | ||
| + | #< | ||
| + | </ | ||
| + | |||
| + | To be able to visualize our robot and shelves, let's set up our demo folder. | ||
| + | |||
| + | ===== CRAM Pepper Demo ===== | ||
| + | |||
| + | Just like the // | ||
| + | |||
| + | First, change the directory to "// | ||
| + | |||
| + | < | ||
| + | $ cd path-to-your-workspace/ | ||
| + | </ | ||
| + | |||
| + | 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/ | ||
| + | Created file cram_pepper_demo/ | ||
| + | Successfully created files in path-to-your-workspace/ | ||
| + | </ | ||
| + | |||
| + | As explained before, open the // | ||
| + | |||
| + | <code XML> | ||
| + | <package format=" | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | <url type=" | ||
| + | <url type=" | ||
| + | |||
| + | < | ||
| + | |||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | < | ||
| + | < | ||
| + | < | ||
| + | |||
| + | <!-- launch file --> | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | < | ||
| + | | ||
| + | </ | ||
| + | </ | ||
| + | |||
| + | Next, replace open the // | ||
| + | |||
| + | <code lisp> | ||
| + | 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 // | ||
| + | |||
| + | <code lisp> | ||
| + | (defsystem cram-pepper-demo | ||
| + | :author " | ||
| + | :license " | ||
| + | |||
| + | :depends-on (roslisp-utilities ; for ros-init-function | ||
| + | |||
| + | | ||
| + | | ||
| + | cl-tf | ||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | |||
| + | ) | ||
| + | |||
| + | :components | ||
| + | ((:module " | ||
| + | :components | ||
| + | ((:file " | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | )))) | ||
| + | </ | ||
| + | |||
| + | 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; // | ||
| + | |||
| + | Now, let’s define our package in the " | ||
| + | |||
| + | <code lisp> | ||
| + | (in-package :cl-user) | ||
| + | |||
| + | (defpackage cram-pepper-description | ||
| + | (:nicknames #: | ||
| + | (:use #: | ||
| + | (: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 resource folder from [[https:// | ||
| + | |||
| + | Compile the project, make sure to have the " | ||
| + | |||
| + | In Emacs, let’s load our pepper demo package. | ||
| + | |||
| + | <code lisp> | ||
| + | CL-USER> (ros-load: | ||
| + | </ | ||
| + | |||
| + | Load the package | ||
| + | |||
| + | <code lisp> | ||
| + | CL-USER> (in-package :demo) | ||
| + | </ | ||
| + | |||
| + | If successful, you should see the following in red. | ||
| + | |||
| + | <code lisp> | ||
| + | #< | ||
| + | </ | ||
| + | |||
| + | 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, | ||
| + | |||
| + | <code lisp> | ||
| + | (in-package :demo) | ||
| + | |||
| + | (defvar *kitchen-urdf* nil) | ||
| + | (defparameter *robot-parameter* " | ||
| + | (defparameter *kitchen-parameter* " | ||
| + | |||
| + | (defun setup-bullet-world () | ||
| + | (setf btr: | ||
| + | |||
| + | (let ((robot (or rob-int: | ||
| + | (setf rob-int: | ||
| + | | ||
| + | (roslisp: | ||
| + | (kitchen (or *kitchen-urdf* | ||
| + | (let ((kitchen-urdf-string | ||
| + | | ||
| + | (when kitchen-urdf-string | ||
| + | (setf *kitchen-urdf* (cl-urdf: | ||
| + | | ||
| + | ;; 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: | ||
| + | (or (gethash cram-tf: | ||
| + | | ||
| + | (error " | ||
| + | |||
| + | |||
| + | (assert | ||
| + | | ||
| + | (prolog `(and | ||
| + | (btr: | ||
| + | (btr: | ||
| + | (btr:assert ?w (btr:object : | ||
| + | : | ||
| + | (btr:assert ?w (btr:object :urdf :kitchen ((0 0 0) (0 0 0 1)) | ||
| + | : | ||
| + | : | ||
| + | : | ||
| + | :urdf ,kitchen | ||
| + | : | ||
| + | (-> (cram-robot-interfaces: | ||
| + | (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: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | |||
| + | |||
| + | (setup-bullet-world) | ||
| + | |||
| + | (setf cram-tf: | ||
| + | |||
| + | (setf prolog: | ||
| + | |||
| + | (cram-bullet-reasoning: | ||
| + | |||
| + | (btr: | ||
| + | |||
| + | (roslisp-utilities: | ||
| + | </ | ||
| + | |||
| + | <code lisp> | ||
| + | (defvar *kitchen-urdf* nil) | ||
| + | (defparameter *robot-parameter* " | ||
| + | (defparameter *kitchen-parameter* " | ||
| + | </ | ||
| + | |||
| + | Here, we are defining global variables for our robot and shelves (kitchen-parameter). | ||
| + | |||
| + | <code lisp> | ||
| + | (defun setup-bullet-world () | ||
| + | (setf btr: | ||
| + | |||
| + | (let ((robot (or rob-int: | ||
| + | (setf rob-int: | ||
| + | | ||
| + | (roslisp: | ||
| + | (kitchen (or *kitchen-urdf* | ||
| + | (let ((kitchen-urdf-string | ||
| + | | ||
| + | (when kitchen-urdf-string | ||
| + | (setf *kitchen-urdf* (cl-urdf: | ||
| + | | ||
| + | ;; 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: | ||
| + | (or (gethash cram-tf: | ||
| + | | ||
| + | (error " | ||
| + | |||
| + | |||
| + | (assert | ||
| + | | ||
| + | (prolog `(and | ||
| + | (btr: | ||
| + | (btr: | ||
| + | (btr:assert ?w (btr:object : | ||
| + | : | ||
| + | (btr:assert ?w (btr:object :urdf :kitchen ((0 0 0) (0 0 0 1)) | ||
| + | : | ||
| + | : | ||
| + | : | ||
| + | :urdf ,kitchen | ||
| + | : | ||
| + | (-> (cram-robot-interfaces: | ||
| + | (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?" | ||
| + | |||
| + | ) | ||
| + | </ | ||
| + | |||
| + | This function creates the bullet-world, | ||
| + | |||
| + | <code lisp> | ||
| + | (defun init-projection () | ||
| + | (def-fact-group costmap-metadata (costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | (<- (costmap: | ||
| + | |||
| + | |||
| + | (setup-bullet-world) | ||
| + | |||
| + | (setf cram-tf: | ||
| + | |||
| + | (setf prolog: | ||
| + | |||
| + | (cram-bullet-reasoning: | ||
| + | |||
| + | (btr: | ||
| + | </ | ||
| + | |||
| + | This function is used to reinitialize the bullet-world in case there is a need for it. | ||
| + | |||
| + | <code lisp> | ||
| + | (roslisp-utilities: | ||
| + | </ | ||
| + | |||
| + | 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 // | ||
| + | |||
| + | Load the demo package, and this time run the following code. | ||
| + | |||
| + | <code lisp> | ||
| + | DEMO> (roslisp-utilities: | ||
| + | </ | ||
| + | |||
| + | 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. | ||
| + | |||
| + | <code lisp> | ||
| + | DEMO> btr: | ||
| + | </ | ||
| + | |||
| + | Your results should appear like below in red. | ||
| + | |||
| + | <code lisp> | ||
| + | #< | ||
| + | </ | ||
| + | |||
| + | 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. | ||
| + | |||
| + | <code lisp> | ||
| + | (prolog: | ||
| + | (assert (btr: | ||
| + | ((2 0 0) (0 0 0 1)))))) | ||
| + | </ | ||
| + | |||
| + | If it was successful, your results should look like below. | ||
| + | |||
| + | <code lisp> | ||
| + | (((?WORLD . #< | ||
| + | . # | ||
| + | :GENERATOR #< | ||
| + | | ||
| + | </ | ||
| + | |||
| + | 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. | ||
| + | |||
| + | <code lisp> | ||
| + | (prolog: | ||
| + | (assert (btr:object ?world :mesh :my-cereal1 ((4.1 0 1.03) (0 0 1 1)) | ||
| + | :mass 0 :color (1 0 0) :mesh : | ||
| + | </ | ||
| + | |||
| + | The code above imports an object of a mesh type with a name “my-cereal1”, | ||
| + | |||
| + | 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 "// | ||
| + | |||
| + | <code lisp> | ||
| + | (defun spawn-products-one () | ||
| + | (prolog: | ||
| + | (assert (btr:object ?world :mesh :my-cereal1 ((4.1 0 1.03) (0 0 1 1)) | ||
| + | :mass 0 :color (1 0 0) :mesh : | ||
| + | (prolog: | ||
| + | (assert (btr:object ?world :mesh :my-cereal2 ((2 -1.4 1.05) (0 0 1 1)) | ||
| + | :mass 0 :color (1 0 0) :mesh : | ||
| + | (prolog: | ||
| + | (assert (btr:object ?world :mesh :my-cereal3 ((4.1 0.15 1.73) (0 0 1 1)) | ||
| + | :mass 0 :color (1 0 0) :mesh : | ||
| + | |||
| + | (prolog: | ||
| + | (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: | ||
| + | (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: | ||
| + | (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: | ||
| + | (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: | ||
| + | (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: | ||
| + | (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: | ||
| + | (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: | ||
| + | (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 : | ||
| + | (prolog: | ||
| + | (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 : | ||
| + | |||
| + | ) | ||
| + | </ | ||
| + | |||
| + | 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. | ||
| + | |||
| + | <code lisp> | ||
| + | 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 " | ||
| + | |||
| + | <code lisp> | ||
| + | (defun park-arms() | ||
| + | (cram-executive: | ||
| + | (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 "// | ||
| + | |||
| + | <code lisp> | ||
| + | DEMO> (cram-urdf-projection: | ||
| + | </ | ||
| + | |||
| + | 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. | ||
| + | |||
| + | <code lisp> | ||
| + | (defun spawn-human() | ||
| + | (prolog: | ||
| + | (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 " | ||
| + | |||
| + | <code lisp> | ||
| + | DEMO> (spawn-human) | ||
| + | </ | ||
| + | |||
| + | Your results should be like this, and you should see a yellow human behind the robot. | ||
| + | |||
| + | <code lisp> | ||
| + | (((?WORLD . #< | ||
| + | . # | ||
| + | :GENERATOR #< | ||
| + | | ||
| + | </ | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | 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 " | ||
| + | |||
| + | <code lisp> | ||
| + | (defun reposition-robot() | ||
| + | (prolog: | ||
| + | (assert (btr: | ||
| + | ((0 0 0) (0 0 1 0))))))) | ||
| + | </ | ||
| + | |||
| + | Save and compile the file. Run the function in the Emacs window. | ||
| + | |||
| + | <code lisp> | ||
| + | DEMO> (reposition-robot) | ||
| + | </ | ||
| + | |||
| + | Your results should look like the following. | ||
| + | |||
| + | <code lisp> | ||
| + | (((?WORLD . #< | ||
| + | . # | ||
| + | :GENERATOR #< | ||
| + | | ||
| + | </ | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | 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. | ||
| + | |||
| + | <code lisp> | ||
| + | ;; Menu functions start | ||
| + | (defun menu-one() | ||
| + | | ||
| + | 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 " | ||
| + | |||
| + | <code lisp> | ||
| + | ;; Interaction execution | ||
| + | (defun interaction() | ||
| + | (cram-urdf-projection: | ||
| + | (reposition-robot) | ||
| + | (menu-one) | ||
| + | ) ;; | ||
| + | </ | ||
| + | |||
| + | We want to run the " | ||
| + | |||
| + | <code lisp> | ||
| + | (defun spawn-products-one () | ||
| + | |||
| + | ….. | ||
| + | (prolog: | ||
| + | (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 : | ||
| + | (prolog: | ||
| + | (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 : | ||
| + | |||
| + | (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. | ||
| + | |||
| + | <code lisp> | ||
| + | (init-projection) | ||
| + | </ | ||
| + | |||
| + | After the reinitialization and file compilations, | ||
| + | |||
| + | Next, run the " | ||
| + | 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. | ||
| + | |||
| + | <code lisp> | ||
| + | (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 " | ||
| + | ) | ||
| + | );;first conditional statement end | ||
| + | </ | ||
| + | |||
| + | From the code above, we first create a variable " | ||
| + | |||
| + | 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 " | ||
| + | |||
| + | <code lisp> | ||
| + | ;;second conditional statement start | ||
| + | (setf proresponse (read t)) | ||
| + | (cond ((= proresponse 1) | ||
| + | (format t " | ||
| + | ((= proresponse 2) | ||
| + | (format t " | ||
| + | ) | ||
| + | ;;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 " | ||
| + | |||
| + | <code lisp> | ||
| + | ;; Interaction execution | ||
| + | (defun interaction() | ||
| + | (cram-urdf-projection: | ||
| + | (reposition-robot) | ||
| + | (menu-one) | ||
| + | |||
| + | (let* ((? | ||
| + | (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 ? | ||
| + | (format t " | ||
| + | ((= proresponse 2) | ||
| + | (setf ? | ||
| + | (format t " | ||
| + | ) | ||
| + | ;;second conditional statement end | ||
| + | |||
| + | ) | ||
| + | ((= response 2 ) | ||
| + | (format t " | ||
| + | );;first conditional statement end | ||
| + | |||
| + | ) ;;let 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. | ||
| + | |||
| + | ===== 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 "// | ||
| + | |||
| + | <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 : | ||
| + | (<- (type : | ||
| + | |||
| + | (<- (is-type cereal : | ||
| + | (<- (is-type 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 : | ||
| + | (<- (location : | ||
| + | (<- (location : | ||
| + | (<- (location : | ||
| + | (<- (location : | ||
| + | |||
| + | (<- (location : | ||
| + | (<- (location : | ||
| + | (<- (location : | ||
| + | (<- (location : | ||
| + | (<- (location : | ||
| + | |||
| + | (<- (is-at shelf-one-level-1 : | ||
| + | (<- (is-at shelf-one-level-2 : | ||
| + | (<- (is-at shelf-one-level-3 : | ||
| + | (<- (is-at shelf-one-level-4 : | ||
| + | (<- (is-at shelf-one-level-5 : | ||
| + | |||
| + | (<- (is-at shelf-two-level-1 : | ||
| + | (<- (is-at shelf-two-level-2 : | ||
| + | (<- (is-at shelf-two-level-3 : | ||
| + | (<- (is-at shelf-two-level-4 : | ||
| + | (<- (is-at shelf-two-level-5 : | ||
| + | </ | ||
| + | |||
| + | The first fact-group maps the products to their various types/ | ||
| + | |||
| + | 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))) | ||
| + | </ | ||
| + | |||
| + | You should get a result like below. | ||
| + | |||
| + | <code lisp> | ||
| + | (((?OBJECT . CEREAL) (?SHELF . SHELF-ONE-LEVEL-4)) | ||
| + | . # | ||
| + | :GENERATOR #< | ||
| + | | ||
| + | </ | ||
| + | |||
| + | 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: | ||
| + | </ | ||
| + | |||
| + | You should get a result like below: | ||
| + | <code lisp> | ||
| + | (((?OBJECT . CEREAL) (?SHELF . SHELF-ONE-LEVEL-4)) | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | | ||
| + | </ | ||
| + | |||
| + | As we can see, the code returns the entire matching results. Add " | ||
| + | |||
| + | 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, | ||
| + | To achieve that, copy the function below and place it in your // | ||
| + | |||
| + | <code lisp> | ||
| + | ;; This function returns a list of shelf poses | ||
| + | ;; It takes the product name as an argument | ||
| + | (defun get-shelf-pose(? | ||
| + | |||
| + | (let* ((list-of-shelves | ||
| + | (cut: | ||
| + | (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: | ||
| + | |||
| + | (setf shelf-pose-trans | ||
| + | (cl-transforms: | ||
| + | (btr: | ||
| + | (find shelf-btr-name | ||
| + | (btr: | ||
| + | (setf shelf-str (subseq (write-to-string shelf-btr-name) 10 17)) | ||
| + | |||
| + | |||
| + | (cond ((string-equal shelf-str " | ||
| + | (setf shelf-location-list (cons | ||
| + | (cl-transforms: | ||
| + | (cl-transforms: | ||
| + | |||
| + | shelf-location-list))) | ||
| + | |||
| + | ((string-equal shelf-str " | ||
| + | (setf shelf-location-list (cons | ||
| + | (cl-transforms: | ||
| + | (cl-transforms: | ||
| + | |||
| + | shelf-location-list)) | ||
| + | ))) | ||
| + | |||
| + | shelf-location-list | ||
| + | ) | ||
| + | |||
| + | ) | ||
| + | </ | ||
| + | |||
| + | 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' | ||
| + | |||
| + | Also, in the above function, we use two global variables; "// | ||
| + | |||
| + | <code lisp> | ||
| + | (defparameter *shelfoneOffset* | ||
| + | (cl-tf: | ||
| + | |||
| + | (defparameter *shelftwoOffset* | ||
| + | (cl-tf: | ||
| + | </ | ||
| + | |||
| + | ===== 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 " | ||
| + | |||
| + | <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(? | ||
| + | (let* ((? | ||
| + | (? | ||
| + | (? | ||
| + | (to-see-designator (desig:a location (visible-for pepper) | ||
| + | (location ? | ||
| + | (object (desig:an object (type : | ||
| + | (desig: | ||
| + | </ | ||
| + | |||
| + | 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 " | ||
| + | |||
| + | <code lisp> | ||
| + | ;; This function moves the robot to a given pose | ||
| + | (defun move-to-location(? | ||
| + | (let ((? | ||
| + | (perform (desig:an action | ||
| + | (type going) | ||
| + | (target (desig:a location | ||
| + | (pose ? | ||
| + | </ | ||
| + | |||
| + | The function above moves the robot to the given location/ | ||
| + | |||
| + | 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(? | ||
| + | (cram-urdf-projection: | ||
| + | |||
| + | ;; | ||
| + | |||
| + | |||
| + | )) | ||
| + | </ | ||
| + | |||
| + | 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: | ||
| + | (reposition-robot) | ||
| + | (menu-one) | ||
| + | |||
| + | (let* ' | ||
| + | (? | ||
| + | (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 ? | ||
| + | (setf ? | ||
| + | (format t " | ||
| + | ((= proresponse 2) | ||
| + | (setf ? | ||
| + | (setf ? | ||
| + | (format t " | ||
| + | ) | ||
| + | ;;second conditional statement end | ||
| + | (print ? | ||
| + | (print ? | ||
| + | ) | ||
| + | ((= response 2 ) | ||
| + | (format t " | ||
| + | );;first conditional statement end | ||
| + | |||
| + | ) ;;let close brackets | ||
| + | ) ;; | ||
| + | </ | ||
| + | |||
| + | Compile all your edited files and run the " | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | As we can see from the image above, the " | ||
| + | Note: You can take out the print lines and replace those lines with the following code. | ||
| + | |||
| + | <code lisp> | ||
| + | (demo-one ? | ||
| + | </ | ||
| + | |||
| + | Update the "// | ||
| + | |||
| + | <code lisp> | ||
| + | (let* ((shelf-pose-lists-copy ? | ||
| + | (product-name-copy ? | ||
| + | (product-pose nil) | ||
| + | (found nil)) | ||
| + | |||
| + | ;;; | ||
| + | (dolist (? | ||
| + | |||
| + | (setf product-pose (generate-robot-pose ? | ||
| + | |||
| + | (cpl: | ||
| + | (cpl: | ||
| + | |||
| + | ((cram-common-failures: | ||
| + | |||
| + | |||
| + | (roslisp: | ||
| + | Could not move to location...retrying" | ||
| + | |||
| + | (cpl: | ||
| + | |||
| + | (setf product-pose (generate-robot-pose ? | ||
| + | |||
| + | (cpl: | ||
| + | (return))) | ||
| + | |||
| + | |||
| + | (move-to-location product-pose))) | ||
| + | |||
| + | )) ;;endof let* and dolist | ||
| + | </ | ||
| + | |||
| + | 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, | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | 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 (? | ||
| + | |||
| + | (generate-look-directions ? | ||
| + | |||
| + | (let* ((? | ||
| + | (possible-look-directions `(, | ||
| + | , | ||
| + | , | ||
| + | (? | ||
| + | |||
| + | (setf possible-look-directions (cdr possible-look-directions)) | ||
| + | |||
| + | (move-to-location (calculate-robot-navigation-goal-towards-target ? | ||
| + | |||
| + | (look-at-product ? | ||
| + | |||
| + | (cpl: | ||
| + | ((cram-common-failures: | ||
| + | ;; Try different look directions until there is none left. | ||
| + | (when possible-look-directions | ||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | |||
| + | (cram-executive: | ||
| + | (desig: | ||
| + | (type detecting) | ||
| + | (object (desig:an object | ||
| + | (type ? | ||
| + | ))) | ||
| + | </ | ||
| + | |||
| + | From the code above, we first generate three different looking directions based on the initial/ | ||
| + | |||
| + | <code lisp> | ||
| + | (generate-look-directions ? | ||
| + | </ | ||
| + | |||
| + | For every direction generated, we adjust the base of the robot to face that direction, and then we turn the robot' | ||
| + | |||
| + | <code lisp> | ||
| + | (setf possible-look-directions (cdr possible-look-directions)) | ||
| + | |||
| + | (move-to-location (calculate-robot-navigation-goal-towards-target ? | ||
| + | |||
| + | (look-at-product ? | ||
| + | </ | ||
| + | |||
| + | 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: | ||
| + | ((cram-common-failures: | ||
| + | ;; Try different look directions until there is none left. | ||
| + | (when possible-look-directions | ||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | |||
| + | (cram-executive: | ||
| + | (desig: | ||
| + | (type detecting) | ||
| + | (object (desig:an object | ||
| + | (type ? | ||
| + | ) | ||
| + | </ | ||
| + | |||
| + | From the code above, we can see that there some helper functions. These include "// | ||
| + | |||
| + | 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) | ||
| + | </ | ||
| + | |||
| + | Next, we add the following functions. | ||
| + | |||
| + | <code lisp> | ||
| + | ;; This function generates possible looking directions for the robot | ||
| + | (defun generate-look-directions (? | ||
| + | (setf *look-center* (cl-transforms-stamped: | ||
| + | (setf *look-right* (get-right-pose ? | ||
| + | (setf *look-left* (get-left-pose ? | ||
| + | </ | ||
| + | |||
| + | 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(? | ||
| + | (let* ((mapTshelf-pose ? | ||
| + | (mapTshelf-trans (cl-transforms: | ||
| + | |||
| + | (cl-transforms-stamped: | ||
| + | (cl-transforms: | ||
| + | (cl-transforms: | ||
| + | |||
| + | ;; This function generates the right possible looking direction | ||
| + | ;; It takes the product name as an argument | ||
| + | (defun get-right-pose(? | ||
| + | (let* ((mapTshelf-pose ? | ||
| + | (mapTshelf-trans (cl-transforms: | ||
| + | (cl-transforms-stamped: | ||
| + | (cl-transforms: | ||
| + | (cl-transforms: | ||
| + | </ | ||
| + | |||
| + | 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: | ||
| + | |||
| + | (defparameter *left-offset* | ||
| + | (cl-tf: | ||
| + | </ | ||
| + | |||
| + | Next, | ||
| + | |||
| + | <code lisp> | ||
| + | ;; This function queries our reasoning base and returns the type of | ||
| + | ;; product based on the name | ||
| + | (defun get-product-type(? | ||
| + | (cdaar (cut: | ||
| + | |||
| + | ;; This function helps the robot to look at the given | ||
| + | ;; location. | ||
| + | (defun look-at-product (? | ||
| + | (cpl: | ||
| + | (cpl: | ||
| + | |||
| + | ((cram-common-failures: | ||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | | ||
| + | |||
| + | | ||
| + | | ||
| + | |||
| + | |||
| + | (cram-executive: | ||
| + | (type looking) | ||
| + | (target (desig:a location | ||
| + | (pose ? | ||
| + | </ | ||
| + | |||
| + | 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-cur-trans (cl-transforms: | ||
| + | |||
| + | (cl-transforms-stamped: | ||
| + | (cl-transforms: | ||
| + | (cl-transforms: | ||
| + | </ | ||
| + | |||
| + | 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: | ||
| + | </ | ||
| + | |||
| + | Next, | ||
| + | |||
| + | <code lisp> | ||
| + | ;;;; | ||
| + | (defun calculate-robot-navigation-goal-towards-target (? | ||
| + | (calculate-pose-towards-target | ||
| + | ? | ||
| + | (cram-tf: | ||
| + | </ | ||
| + | |||
| + | This function also contains another helper function. Let’s add it. | ||
| + | |||
| + | <code lisp> | ||
| + | ;;" | ||
| + | ;; calculate the new robot-pose-stamped, | ||
| + | ;; the `look-pose-stamped' | ||
| + | (defun calculate-pose-towards-target (look-pose-stamped robot-pose-stamped) | ||
| + | |||
| + | (let* ((world-> | ||
| + | (cram-tf: | ||
| + | (robot-> | ||
| + | (cl-transforms: | ||
| + | (world-> | ||
| + | (cl-transforms: | ||
| + | (look-pose-in-robot-frame | ||
| + | (cl-transforms: | ||
| + | robot-> | ||
| + | world-> | ||
| + | (rotation-angle | ||
| + | (atan | ||
| + | (cl-transforms: | ||
| + | (cl-transforms: | ||
| + | (cram-tf: | ||
| + | </ | ||
| + | |||
| + | 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 ? | ||
| + | </ | ||
| + | |||
| + | If we run "// | ||
| + | |||
| + | <code lisp> | ||
| + | (if (not (null found)) | ||
| + | (return)) | ||
| + | </ | ||
| + | |||
| + | This line of code will stop the search for the product after it has been found. If we try to run " | ||
| + | |||
| + | |||
| + | 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 " | ||
| + | |||
| + | <code lisp> | ||
| + | (defun point-front-right() ; Point forward using the right arm | ||
| + | (cram-executive: | ||
| + | (desig:an action | ||
| + | (type positioning-arm) | ||
| + | (left-configuration park) | ||
| + | (right-configuration point-ahead)))) | ||
| + | </ | ||
| + | |||
| + | Let's call this function when the product has been found. That's when " | ||
| + | |||
| + | <code lisp> | ||
| + | (if (not (null found)) | ||
| + | (progn | ||
| + | (point-front-right) | ||
| + | (return))) | ||
| + | </ | ||
| + | |||
| + | 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. | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | 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* ((? | ||
| + | (setf (btr:pose (btr:object btr: | ||
| + | </ | ||
| + | |||
| + | 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-cur-trans (cl-transforms: | ||
| + | (cl-transforms-stamped: | ||
| + | (cl-transforms: | ||
| + | (cl-transforms: | ||
| + | </ | ||
| + | We have a global variable in this function as well. Let’s add it at the top. | ||
| + | |||
| + | <code lisp> | ||
| + | (defparameter *humanPoseOffset* | ||
| + | (cl-tf: | ||
| + | </ | ||
| + | |||
| + | 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))) | ||
| + | </ | ||
| + | |||
| + | You should get the following results. | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | 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))) | ||
| + | </ | ||
| + | |||
| + | 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." | ||
| + | </ | ||
| + | If we try running the " | ||
| + | |||
| + | {{ : | ||
| + | |||
| + | So we need a function to reset the human back to its original position. Update the " | ||
| + | |||
| + | <code lisp> | ||
| + | ;; | ||
| + | (defun reposition-human() | ||
| + | (prolog: | ||
| + | (assert (btr: | ||
| + | </ | ||
| + | Let's call this function in the " | ||
| + | |||
| <code lisp> | <code lisp> | ||
| - | (and this-is | + | ;; Interaction execution |
| + | (defun interaction() | ||
| + | (cram-urdf-projection: | ||
| + | (reposition-human) | ||
| + | (reposition-robot) | ||
| + | (menu-one) | ||
| + | …. | ||
| </ | </ | ||
| - | - This is an ordered list | + | Let's end the previous process using ctrl+c twice, compile the edited files, and run " |
| - | * And this is a bullet list | + | Congratulations!!! You just created |
| - | Below is a horizontal rule: | + | You can access the entire project from [[https:// |
| - | ---------------------------------- | + | |
| - | And this is '' | ||
