Tutorial 1: Simple Fetch and Place

For this tutorial, we need to set up the robot environment similar to how it is done for the real system, so we will need:

• a so-called occupancy map, which is the floor plan of the room that helps with navigation,
• an Inverse Kinematics solver, which deals with the mathematics related to arm movements,
• a 3D description of the environment with all the furniture,
• as well as a 3D description of the robot itself.

Let's load all these by running a script called the launch file. Open a new terminal and run the following command:

$ roslaunch cram_pick_place_tutorial world.launch

Do not close this terminal instance throughout this tutorial.

Open a new terminal window and start Emacs with the Lisp command line, if you don't already have it open. For that type in the terminal:

$ roslisp_repl &

An Emacs window with a Lisp command line will have launched. At this point we can execute some simple Lisp code. Try:

CL-USER> (print "Hello World")

Note - “CL-USER>” is a part of your prompt and should not be typed in

Now let's load our pick and place tutorial package. Type in the following two commands into the Lisp command line:

CL-USER> (ros-load:load-system "cram_pick_place_tutorial" :cram-pick-place-tutorial)
CL-USER> (in-package :cram-pick-place-tutorial)

To paste something from, e.g., a browser, into Emacs, press “Ctrl - Y”. Alternatively, in Linux you can select any piece of text in any application and paste it by pressing the middle click.

Your prompt will now change to PP-TUT> which indicates that you have loaded and are now working inside the package called “cram-pick-place-tutorial”.

Now to launch a simulation with a kitchen environment and our robot, type the following command into your Lisp command line:

PP-TUT> (roslisp-utilities:startup-ros)

You will get a pop-up window with the environment and the robot inside. We call this environment the Bullet World. The robot we have there is a simulation of the real-life PR2 robot, and we're going to use it to perform various tasks inside the Bullet World.

Note:Please be patient as the startup may take as long as 3 minutes.

If you get an error saying “Could not contact master at …”, it's because you forgot to start the launch file. Once you've done that, press “q” to exit the debugger and try again.

At any point during the tutorial, if you want to clean/reset the Bullet World, enter the following command into the Lisp command line:

PP-TUT> (init-projection)

You can change the viewpoint in the window by pressing and dragging the left mouse button. Pressing and dragging the right mouse button translates the camera. Pressing and dragging the middle click doesn't do anything, but you can zoom in and zoom out by scrolling.

If the bullet simulation is stuck call the following function in the REPL to reset the window:

PP-TUT> (btr-utils:reset-debug-window)

First, let's try moving our PR2 robot around the kitchen. Before that let's try to create a pose, which can be given as the target destination for the robot. Execute the following function in the Lisp command line:

PP-TUT> (make-pose "map" '((0 1 0) (0 0 0 1)))

The make-pose function creates the pose and returns it. The “map” is the coordinate frame in which the pose is considered. The argument is a list of two sublists - the first one is the x y z position and the second one is the quaternion of the orientation [( (x y z) (x y z w) )].

The screenshot below shows axes where the “map” coordinate frame is located. Red is the +ve x-axis, Green is the +ve y-axis and Blue is the +ve z-axis. This is the pose that is referred by '( (0 0 0) (0 0 0 1) ) in “map”. We also call this pose the origin of “map”. All the poses that are in reference to the “map” are relative to this pose. Our robot is, per default, always spawned at the origin of “map”.

Please note that in our Emacs IDE the result of executing something is colored as red. It is not an error but simply a resulting object. You can always inspect the result object by right-clicking on it and saying “Inspect”. That shows the guts of the object. Press “q” to exist the inspector tab.

Let's use this to move around our robot:

PP-TUT>
(with-simulated-robot
  (let ((?navigation-goal (make-pose "map" '((0 1 0) (0 0 0 1)))))
    (perform (an action
                 (type going)
                 (target (a location 
                            (pose ?navigation-goal)))))))

To go onto a new line without executing, press “Ctrl-J”.

You can see that the robot has moved to a new location which is 1 meter from the origin along the y-axis in the “map” reference frame. Try to move around different locations on the map by changing the poses. You'll notice that you will get an error of type CRAM-COMMON-FAILURES:NAVIGATION-POSE-UNREACHABLE if you try to move the robot to positions, which are not valid (due to collisions).

Note:- Notice how we're using with-simulated-robot to enclose the code to perform actions on the robot. This method tells CRAM that our code needs to be run on the simulation and not on the actual PR2 itself.

Note:- The let keyword in Lisp is a handy way to define local variables within its scope. Eg:

PP-TUT> (let ((a 1)
               (b 2))
          (print a)
          b)
1
2
PP-TUT> (print b)
The variable B is unbound.

To exit the debugger, press “q”. The variables a and b have been declared with values under the scope of let and as soon as the body of let was over, the scope of these variables ended as well.

Now let's move the robot next to the table on the right-hand side of the kitchen. Since we are going to use this pose a lot, we'll save this in a global variable. In Lisp, a parameter is a kind of a global variable.

PP-TUT>
(defparameter *base-pose-near-table*
  (make-pose "map" '((-1.447d0 -0.150d0 0.0d0) (0.0d0 0.0d0 -0.7071067811865475d0 0.7071067811865476d0))))
 
(with-simulated-robot
  (let ((?navigation-goal *base-pose-near-table*))
    (perform (an action
                 (type going)
                 (target (a location 
                            (pose ?navigation-goal)))))))

Don't be surprised if you occasionally see “d0” at the end of the numbers, e.g., -0.150d0. This simply means that the number has a double precision. you can just as well write -0.15, if you don't require very high precision, which we don't in this tutorial.

To go up in the history of your Lisp command line, press “Ctrl-Up” while your cursor is at the prompt. You can scroll through the history by pressing “Ctrl-Up” repeatedly.

To clean all the input that is currently sitting in the prompt, instead of pressing “Backspace” many many times, try “Alt-Ctrl-Backspace”.

Now let's try to spawn a new object into the world:

PP-TUT> (spawn-object '((-1.6 -0.9 0.82) (0 0 0 1)))

You will see that a red bottle has been created on the table in front of the PR2. By default, this method is written to help you to always create a bottle. But if you want to spawn other objects from our library, like a cube, you can provide it as an argument. Eg:

PP-TUT> (spawn-object '((1.44 1.28 0.85) (0 0 0 1)) :cube 'cube-1)

Here, :cube is the type of the object and 'cube-1 is its name. For the default bottle, type is :bottle and name is 'bottle-1.

To see a list of all available objects, call:

PP-TUT> (list-available-objects)

You can also give a different color to the object in RGB encoding:

PP-TUT> (spawn-object '((0 0 0.1) (0 0 0 1)) :weisswurst 'wurst-1 '(0 1 0))

This should have spawned a green sausage on the floor near the origin of the “map” coordinate frame.

If you want to know if a coordinate you defined is correct, you can visualize the axis of the coordinate frame in the Bullet World and see for yourself. Try the following:

PP-TUT> (visualize-coordinates :bottle)

This will spawn a coordinate frame on our bottle object. The argument here is the object type for which we want to visualize the coordinates. Make sure you use : in front of the bottle because it's an object type.

You can also visualize the coordinates of an object with a specific name:

PP-TUT> (spawn-object '((1.5 1 1) (0 0 0 1)) :weisswurst 'wurst-1 '(0 1 0))
PP-TUT> (visualize-coordinates 'wurst-1)

This will spawn a green sausage on the sink counter and visualize its coordinate frame. Note how here we used the name of the object, 'wurst-1, instead of its type, :weisswurst. Note:The weisswurst might have a different coordinate axes than that shown in the picture below for you below because of its shape and the randomness of the simulation

You can also visualize poses that don't have any objects on them yet, for example:

PP-TUT> (init-projection)
PP-TUT> (visualize-coordinates *base-pose-near-table*)

You will see the coordinate near the table on the right side of the kitchen visualized.

You can also change the scale of the coordinate frame, if you don't quite see it:

PP-TUT> (visualize-coordinates *base-pose-near-table* 1.0)

That will make the axes 1 meter long.

The objective of this tutorial would be to enable you to write methods to pick an object (for us, the bottle) from one place and transport and place it somewhere else. But before the robot can pick up anything, it has to know where the object is, which is enabled by the cameras placed on the robot. We have to explicitly command the robot to perceive a bottle to find it.

The first step to enable this is to bring the object that has to be detected under the field of view of the robot's cameras. There are some actions that can be done for this

• Position the cameras a bit higher so that the robot can see things easily.
• Move the arms away from the field of view to a standard position - also called parking the arms.
• Move the neck such that the camera is directed towards where you expect the object to be.

Let's do the first two steps in this first. The torso of the robot has a prismatic joint which has limits from 0 to 30cm. So we will set that to the maximum and we'll also call the method to park arms. Since these are two actions that are independent and can be done parallelly, we can bundle them together under a clause called cpl:par. Even navigation can be bundled together with this. So try this:

PP-TUT>
(with-simulated-robot
  (let ((?navigation-goal *base-pose-near-table*))
    (cpl:par
      ;; Moving the robot near the table.
      (perform (an action
                   (type going)
                   (target (a location 
                              (pose ?navigation-goal)))))
      ;; Increasing the height of the torso by setting the joint angle to 0.3 meters 
      (perform (a motion 
                  (type moving-torso)
                  (joint-angle 0.3)))
      ;; Parking the arms                
      (park-arms))))

The robot will do all the said actions in parallel.

In Lisp, “;” is the symbol for commentaries. Traditionally, one uses “;” for inline comments, and “;;” for comments that take a full line.

Note how until now we were performing action-s, and moving-torso is a motion. The distinction is that motions are very low-level, and actions are more high-level. We will not go into detail here. For now, just remember that moving-torso is a motion, and everything else is an action.

If you get a navigation pose unreachable error, it might be that there are objects on the floor. To clean up the environment, you can always call (init-projection).

Now, the robot is standing next to the table, the torso is up and the arms are out of the field of view. The next thing to do is to look in the direction where the bottle is expected to be found. We have found the corresponding coordinate for you to direct the robot's gaze, so let's save the pose in a variable and look at it:

PP-TUT>
(defparameter *downward-look-coordinate*
  (make-pose "base_footprint" '((0.65335d0 0.076d0 0.758d0) (0 0 0 1))))
;; This coordinate frame has base_footprint as reference, which is the reference
;; frame of PR2's base.
 
(with-simulated-robot
  (let ((?looking-direction *downward-look-coordinate*))
    (perform (an action
                 (type looking)
                 (target (a location 
                            (pose ?looking-direction)))))))

Notice that the robot now tilts it's head downwards, somewhat in the direction of where the bottle is.

“base_footprint” is a coordinate frame attached to the robot, at the very bottom of its base, on the floor, in the middle of the base square. Whenever the robot moves, the “base_footprint” moves with it. Here is a visualization of “base_footprint”, the X axis looks to the front of the robot, Y to the left, and Z looks up. Note, how the coordinate frame is located on the floor.

Now the only thing remaining is to detect, or “perceive” the bottle. We use an action called “detecting” to do this:

PP-TUT>
(with-simulated-robot
  (perform (an action
               (type detecting)
               (object (an object 
                           (type bottle))))))

If you get an error that an object could not be perceived, the robot is probably not looking at an object of type bottle. So press “q” to exit the debugger and tweak the location of the base or the direction of the gaze of the robot and try to perceive again.

After the object has been perceived successfully, our method returns a description of the bottle with its complete information. Let's save this in a variable, as this will be an argument to the methods we will call below.

PP-TUT>
(defparameter *perceived-bottle* nil)
(with-simulated-robot
  (setf *perceived-bottle* (perform (an action
                                        (type detecting)
                                        (object (an object 
                                                    (type bottle)))))))

setf is used to assign a value to a variable.

Once the object has been found, picking up is very straightforward:

PP-TUT>
(with-simulated-robot 
  (let ((?perceived-bottle *perceived-bottle*))
    (perform (an action
                 (type picking-up)
                 (arm right)
                 (grasp left-side)
                 (object ?perceived-bottle))))
   ;; Parking the right arm after grasping, bringing the bottle close to the robot
   ;; and freeing up the field of view.
   (park-arm :right))

You will notice that we have provided in our action description information like which arm to pick up with (right), which side the robot should be grasping from (left-side) and which object to pick up (?perceived-bottle).

By now, the robot should have successfully picked up the bottle with its right arm.

Now that the robot has the object in its gripper, the next task is to place the object in the destination. Let us use the dining area counter on the left side as our destination. So, to place the bottle the robot has to do two things - drive with the base to a location from where it can place the bottle on the dining table, and move the arm to place the object down.

So before this, let's define two more locations, which are the destination of the bottle and the position of the robot to be able to reach it respectively:

PP-TUT>
(defparameter *final-object-destination*
  (make-pose "map" '((-0.8 2 0.9) (0 0 0 1))))
 
(defparameter *base-pose-near-counter*
  (make-pose "map" '((-0.150d0 2.0d0 0.0d0) (0.0d0 0.0d0 -1.0d0 0.0d0))))

Now, finally, we are going to drive with the base, and the placing action is very similar to the picking-up action, but here we provide the target destination instead:

PP-TUT>
(with-simulated-robot 
  ;; Moving the robot near the counter.
  (let ((?nav-goal *base-pose-near-counter*))
    (perform (an action
                 (type going)
                 (target (a location 
                            (pose ?nav-goal))))))
  ;; Setting the bottle down on the counter
  (let ((?drop-pose *final-object-destination*)
        (?perceived-bottle *perceived-bottle*))
    (perform (an action
                 (type placing)
                 (arm right)
                 (object ?perceived-bottle)
                 (target (a location 
                            (pose ?drop-pose))))))
  (park-arm :right))

The robot has finally placed the object in the destination.

Even though we have achieved the goal we had set, the entire process would have felt a little tedious, especially if you want to perform it multiple times, when you have made a mistake, etc. So let's bundle what we have done so far into a single method called, move-bottle which can easily be reused. To save your work so that the changes you make are still available even if you exit Emacs, we will save the code for it in a file.

First let's open the file which we will write into. Press Ctrl-x Ctrl-f to open the “Find file” dialog on the bottom of Emacs. The folder will be set to “~/<workspace>/src/cram/cram_tutorials/cram_pick_place_tutorial/” by default (This is because it is the path of the project we loaded). We have already saved a file called “pick-and-place.lisp” under the src folder of this project as a workspace for you. So open the file by completing the folder name as below and pressing Enter.

Find File: ~/<workspace>/src/cram/cram_tutorials/cram_pick_place_tutorial/src/pick-and-place.lisp
;; Notice that <workspace> has to be replaced by the corresponding folder name and depends
;; on the machine

The file will open up and you'll see that it has only one line as of now:

(in-package :pp-tut)

This line just selects the namespace that we're going to work with and you'll notice that it's the name of the package of this demo. Do not remove this line throughout the demo.

After that you can copy paste the following code into the file:

(defparameter *base-pose-near-table*
  (make-pose "map" '((-1.447 -0.15 0.0) (0.0 0.0 -0.7071 0.7071))))
 
(defparameter *downward-look-coordinate*
  (make-pose "base_footprint" '((0.65335 0.076 0.758) (0 0 0 1))))
 
(defparameter *base-pose-near-counter*
  (make-pose "map" '((-0.15 2 0) (0 0 -1 0))))
 
(defparameter *final-object-destination*
  (make-pose "map" '((-0.8 2 0.9) (0 0 0 1))))
 
(defun move-bottle (bottle-spawn-pose)
  (spawn-object bottle-spawn-pose)
  (with-simulated-robot
    (let ((?navigation-goal *base-pose-near-table*))
      (cpl:par
        ;; Moving the robot near the table.
        (perform (an action
                     (type going)
                     (target (a location 
                                (pose ?navigation-goal)))))
        (perform (a motion 
                    (type moving-torso)
                    (joint-angle 0.3)))
        (park-arms)))
    ;; Looking towards the bottle before perceiving.
    (let ((?looking-direction *downward-look-coordinate*))
      (perform (an action
                   (type looking)
                   (target (a location 
                              (pose ?looking-direction))))))
    ;; Detect the bottle on the table.
    (let ((?grasping-arm :right)
          (?perceived-bottle (perform (an action
                                          (type detecting)
                                          (object (an object 
                                                      (type bottle)))))))
      ;; Pick up the bottle
      (perform (an action
                   (type picking-up)
                   (arm ?grasping-arm)
                   (grasp left-side)
                   (object ?perceived-bottle)))
      (park-arm ?grasping-arm)
      ;; Moving the robot near the counter.
      (let ((?nav-goal *base-pose-near-counter*))
        (perform (an action
                     (type going)
                     (target (a location 
                                (pose ?nav-goal))))))
      ;; Setting the bottle down on the counter
      (let ((?drop-pose *final-object-destination*))
        (perform (an action
                     (type placing)
                     (arm ?grasping-arm)
                     (object ?perceived-bottle)
                     (target (a location 
                                (pose ?drop-pose))))))
      (park-arm ?grasping-arm))))

This defines a function called move-bottle with the Lisp command defun. This function accepts one argument, bottle-spawn-pose, which is the position the bottle will be spawned in. After spawning the object, move-bottle commands the robot to pick the bottle and place it at its final destination, which is the counter.

First, save the file using the shortcut Ctrl-x Ctrl-s. To use these methods, we need to compile these functions and to do that press Ctrl-c Ctrk-k. You should compile the file any time changes have been made to it. Alternatively, if you only edited one method and want to compile just that, place the cursor on that particular function and press Ctrl-c Ctrl-c.

Now to go back to the REPL to run the code, press Ctrl-x b, type “repl” press Tab and then Enter. This will switch you back to the REPL. Any time you want to switch back to the file, you can type the name of the file instead of repl after Ctrl-x b .

Now, run move-bottle with the coordinates we initially used to spawn the bottle:

PP-TUT> (move-bottle '((-1.6 -0.9 0.82) (0 0 0 1)))

You will see that the robot successfully picks the bottle and places it on the counter. ​[If you're stuck, remember that you can always clean up the world using ​(init-projection)​.]

Congratulations! You have just written your first rudimentary plan to pick and place a bottle.