This is an old revision of the document!
Zero prerequisites demo tutorial: Simple fetch and place
This tutorial is from the “Demo Tutorials” category, which assumes no prior knowledge of Ubuntu, ROS, Emacs, Lisp or robotics.
Setting Up
You will need a virtual machine to run this tutorial. We use VirtualBox to run the VM that contains all the preinstalled requisites needed. First, download and install VirtualBox on your computer, then download and configure the VM.
Install
Here are some hints on how to install VirtualBox on Ubuntu, Arch, Windows and MacOS
Ubuntu 14.04-18.04.2
You can install VirtualBox via the Ubuntu Software center. If you prefer the bash console VirtualBox can be installed from the multiverse repositories like so:
sudo add-apt-repository multiverse sudo apt update sudo apt install virtualbox
Arch-Linux
Follow this tutorial for the install via pacman.
Windows
Download version 5.2.32 for Windows Systems, execute the .exe file and follow the installation instructions.
The CPU needs to support virtualization to run a VM on Windows. The virtualization can be enabled in the BIOS. To get into the BIOS settings restart your machine, hit the settings button while booting (try F2, F8 and F10) and go into the CPU settings. There is something like 'Intel Virtual Technology' or 'CPU Virtualization' which needs to be enabled.
MacOS
Download the .dmg installer for OS X systems, execute the file and follow the instructions.
VirtualBox Setup
- Get the Virtual Disk Image cram_vm.vdi here. You need to enter your Uni-Bremen credentials to access the ZfN file-sharing platform. It contains an Ubuntu 16.04 with ROS kinetic and CRAM v7.0 pre-installed.
- Launch VirtualBox.
First we want to create a new virtual machine. Click on the 'New' button:
Usually the simplified menu will open. Change to expert mode, it won't get too complicated.
In this view the VM gets
- the name 'cram-vm'
- the OS as 'Linux'
- the version as 'Ubuntu (64-bit)'
Choose the VMs memory size (RAM), depending on your machines capacity, but less than 4GB could cause some performance issues.
The Virtual Disc Image downloaded before can now be used. Instead of creating a new one, the VM is set to 'Use an existing virtual hard disk file'. Over the button at the right the program opens a file explorer, where the cram-vm.vdi needs to be found (usually in Downloads) and selected. Finally 'Create' the VM.
Settings (optional)
For better performance the VM could use some of the CPU's power. Choose the newly created VM and hit 'Settings'. Go to 'System' and 'Processor' and choose how many processor cores you want to give the VM.
Everything is set up. The VM can now be started. The Ubuntu 16.04 system should start and automatically log into the only account, with the username and password 'cram'.
Crash Course on Ubuntu, ROS, Emacs, Lisp and Robotics :D
Launching the terminal
To launch the terminal press the Super Key (The windows key on the keyboard) to launch search and type in “terminal” and launch the program. The keyboard shortcut is “Ctrl + Alt + T”
Simple Fetch and Place
Environment setup
For this tutorial we need to set up the environment similar to how it is done for the real system: we will need an occupancy map which helps with navigation, an Inverse Kinematics solver for which deals with the mathematics related to manipulation, a map of the static parts of the environment with the furniture, etc.
Let's load all these into memory and running by running a script called the launch file. Open a new terminal and run the following command.
$ roslaunch cram_pick_place_tutorial world.launch
Do not close this terminal instance throughout this tutorial.
REPL Setup
Create a new instance of the terminal and start the LISP REPL from the terminal by typing:
$ roslisp_repl
An emacs window with the REPL will have launched. Now let's load the package which holds the pick and place tutorial. Type in the following two commands into the REPL :
Note - “CL-USER>” is a part of your prompt and should not be typed in
CL-USER> (ros-load:load-system "cram_pick_place_tutorial" :cram-pick-place-tutorial) CL-USER> (in-package :cram-pick-place-tutorial)
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”.
Launching the Bullet World
Now to launch a simulation of the map and the robot, type the following command into the REPL:
PP-TUT> (roslisp-utilities:startup-ros)
You will get a pop-up window which will load a kitchen environment with a robot inside, called the Bullet world. This robot is a simulation of a robot called PR2, and we're going to use it to perform various tasks inside the bullet world.
At any point during the tutorial, if you want to clean/reset the bullet world, enter the following command into the REPL:
PP-TUT> (init-projection)
Moving Around
First, Let's try moving our Robot, a simulation of the PR2 model, around the kitchen. Before that let's try to create a pose, which can be given as the target destination for the robot. Run the following method in the REPL
PP-TUT> (make-pose "map" '((0 1 0) (0 0 0 1)))
The make-pose method creates the pose and returns it. The “map” is the reference frame from 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 [1)].
Let's use this to move around our robot:
PP-TUT> (pr2-proj:with-simulated-robot (let ((?navigation-goal (make-pose "map" '((0 1 0) (0 0 0 1))))) (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?navigation-goal)))))))
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, etc.).
Note:- The let keyword in LISP is a handy way to define local variables within its scope. Eg:
CL-USER> (let ((a 1) (b 2)) (print a) b) 1 2 CL-USER> (print b) The variable B is unbound.
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 in the kitchen. Since we are going to use this pose a lot, we'll save this in a variable as well.
PP-TUT> (defparameter *base-pose-near-table* (make-pose "map" '((-1.447d0 -0.150d0 0.0d0) (0.0d0 0.0d0 -0.7071067811865475d0 0.7071067811865476d0)))) ;; The orientation in this pose corresponds to a rotation of -(pi/2) around the z-axis. ;; To check it for yourself or to create other orientations you can use the following functions ;; (cl-transforms:axis-angle->quaternion (cl-transforms:make-3d-vector 0 0 1) (/ pi -2)) ;; or ;; (cl-transforms:euler->quaternion :az (/ pi -2)) PP-TUT> (pr2-proj:with-simulated-robot (let ((?navigation-goal *base-pose-near-table*)) (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?navigation-goal)))))))
Note:- You'll notice that we're using pr2-proj: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.
Spawning Objects
We have written and provided you with a method to easily spawn objects into the world. Let's try using this:
PP-TUT> (spawn-object '((-1.6 -0.9 0.82) (0 0 0 1)))
You will see that a 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 supported objects 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)
Detecting Objects
The objective of this tutorial would be to enable you to write methods to pick an object (for us, the bottle) from one place and transport and place it on another. 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, which are not always trying to detect the objects in its view. We have to explicitly command the robot to detect 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.
- Look towards the direction where you expect the object to be in.
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 in the REPL:
PP-TUT> (pr2-proj:with-simulated-robot (let ((?navigation-goal *base-pose-near-table*)) (cpl:par ;; Increasing the height of the torso by setting the joint angle to 0.3 meters (exe:perform (desig:a motion (type moving-torso) (joint-angle 0.3))) ;; Parking the arms (park-arms) ;; Moving the robot near the table. (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?navigation-goal))))))))
The robot will do all the said actions parallely.
Now the next thing to do is to look towards the direction where the bottle will be in. We have found the corresponding coordinates for you and now let's save it in a variable and use 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. PP-TUT> (pr2-proj:with-simulated-robot (let ((?looking-direction *downward-look-coordinate*)) (exe:perform (desig:a motion (type looking) (target (desig:a location (pose ?looking-direction)))))))
Notice that the robot now tilts it's head downwards, somewhat in the direction of where the bottle is at.
Now the only thing remaining is to detect, or “perceive” the bottle. We use a low-level motion called “detecting” to do this. After the object has been perceived successfully, our method returns the designator of the bottle with its complete information, we will save this in a variable as this will be an argument to the methods we will call downstream.
PP-TUT> (pr2-proj:with-simulated-robot (setf *perceived-bottle* (exe:perform (desig:a motion (type detecting) (object (desig:an object (type bottle)))))))
Picking up Objects
Once the object has been found picking up is very straightforward:
PP-TUT> (pr2-proj:with-simulated-robot (let ((?perceived-bottle *perceived-bottle)) (exe:perform (desig: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 the designator to pick up the object with 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). The robot has successfully picked up the bottle with its right arm.
Placing the object
Now that the robot has the object in its gripper, the next task is to place the object in the destination. Let us use the dining area counter on the left side as our destination. So, to place the bottle the robot has to do two things - travel to where the robot can reach the destination of the bottle, and the placing action itself. The placing action is very similar to the picking-up action, but here we will provide the target destination instead. So before this, let's define two more locations again, which are the destination of the bottle and the position of the robot to be able to reach it respectively:
PP-TUT> (defparameter *final-object-destination* (make-pose "map" '((-0.8 2 0.9) (0 0 0 1)))) PP-TUT> (defparameter *base-pose-near-counter* (make-pose "base_footprint" '((-0.150d0 2.0d0 0.0d0) (0.0d0 0.0d0 -1.0d0 0.0d0))))
And finally:
PP-TUT> (pr2-proj:with-simulated-robot ;; Moving the robot near the counter. (let ((?nav-goal *base-pose-near-counter*)) (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?nav-goal)))))) (coe:on-event (make-instance 'cpoe:robot-state-changed)) ;; Setting the bottle down on the counter (let ((?drop-pose *final-object-destination*) (?perceived-bottle *perceived-bottle*)) (exe:perform (desig:an action (type placing) (arm right) (object ?perceived-bottle) (target (desig:a location (pose ?drop-pose)))))) (park-arm :right))
The robot has finally placed the object in the destination.
Simple Plan
Even though we have achieved the goal we had set, the entire process would have felt a little tedious, especially if you want to perform it multiple times, when you have made a mistake, etc. So let's bundle what we have done so far into a single method called, move-bottle which can easily be reused:
(defparameter *base-pose-near-table* (make-pose "map" '((-1.447d0 -0.150d0 0.0d0) (0.0d0 0.0d0 -0.7071067811865475d0 0.7071067811865476d0)))) (defparameter *downward-look-coordinate* (make-pose "base_footprint" '((0.65335 0.076 0.758) (0 0 0 1)))) (defparameter *base-pose-near-counter* (make-pose "base_footprint" '((-0.15 2 0) (0 0 -1 0)))) (defparameter *final-object-destination* (make-pose "map" '((-0.8 2 0.9) (0 0 0 1)))) (defun move-bottle (bottle-spawn-pose) (spawn-object bottle-spawn-pose) (pr2-proj:with-simulated-robot (let ((?navigation-goal *base-pose-near-table*)) (cpl:par (exe:perform (desig:a motion (type moving-torso) (joint-angle 0.3))) (park-arms) ;; Moving the robot near the table. (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?navigation-goal))))))) ;; Looking towards the bottle before perceiving. (let ((?looking-direction *downward-look-coordinate*)) (exe:perform (desig:a motion (type looking) (target (desig:a location (pose ?looking-direction)))))) ;; Detect the bottle on the table. (let ((?grasping-arm :right) (?perceived-bottle (exe:perform (desig:a motion (type detecting) (object (desig:an object (type :bottle))))))) ;; Pick up the bottle (exe:perform (desig: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*)) (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?nav-goal)))))) (coe:on-event (make-instance 'cpoe:robot-state-changed)) ;; Setting the bottle down on the counter (let ((?drop-pose *final-object-destination*)) (exe:perform (desig:an action (type placing) (arm ?grasping-arm) (object ?perceived-bottle) (target (desig:a location (pose ?drop-pose)))))) (park-arm ?grasping-arm))))
move-bottle accepts the position the bottle will spawn in and then move it to the final destination, which is the counter.
Now run move-bottle with the arguments we initially used to spawn the bottle.
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.
You have now written your first rudimentary plan to pick and place a bottle.
Recovering from Failures
The previous example worked perfectly, because we knew the exact coordinates to look for the bottle. This is hardly true for the real life scenario, especially since we are dealing with a kitchen environment, far from being precise compared to a factory floor. What would happen if the bottle was moved a little bit to the right of its previous spawn position? For this, let's change the arguments to reflect this on our method, and let's see what happens.
PP-TUT> (move-bottle '((-2 -0.9 0.860) (0 0 0 1)))
And the output will look like this.
PP-TUT> ; Evaluation aborted on #<CRAM-COMMON-FAILURES:PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>.
Clearly the robot cannot find the object anymore because even though the robot is near the bottle, it is out of the field of vision of the robot due to the predefined base and object pose in our code. Let's try fixing this issue.
To understand the syntax and get a refresher on failure handling, you can refer to Failure Handling. We're going to add some code into fixing perception in our case, so that the robot would still be able to find the bottle. Let's list down a plan of action for this.
- Tilt the head of the robot downwards
- Try to detect the bottle
- If,
- successful in finding the bottle - continue with the rest of the code.
- failed to find the bottle - turn the head to a different configuration (eg., left/right) and try detecting again.
- When all possible directions fail, error out.
Let's define some additional parameters to aid us:
PP-TUT> (defparameter *left-downward-look-coordinate* (cl-transforms-stamped:make-pose-stamped "base_footprint" 0.0 (cl-transforms:make-3d-vector 0.65335d0 0.76d0 0.758d0) (cl-transforms:make-identity-rotation))) (defparameter *right-downward-look-coordinate* (cl-transforms-stamped:make-pose-stamped "base_footprint" 0.0 (cl-transforms:make-3d-vector 0.65335d0 -0.76d0 0.758d0) (cl-transforms:make-identity-rotation)))
We defined two coordinates *left-downward-look-coordinate* and *right-downward-look-coordinate* with respective to our robot base footprint as alternative directions to look for when downward look fails.
Now we define a method find-object which encapsulates our plan with failure handling :
(defun get-preferred-arm-for-direction (direction-looked) (let ((preferred-arm :RIGHT)) (when (eq direction-looked *left-downward-look-coordinate*) (setf preferred-arm :LEFT)) preferred-arm)) (defun find-object (?object-type) (let* ((possible-look-directions `(,*downward-look-coordinate* ,*left-downward-look-coordinate* ,*right-downward-look-coordinate*)) (?looking-direction (first possible-look-directions))) (setf possible-look-directions (cdr possible-look-directions)) (exe:perform (desig:a motion (type looking) (target (desig:a location (pose ?looking-direction))))) (cpl:with-failure-handling ((cram-common-failures:perception-object-not-found (e) ;; Try different look directions until there is none left. (when possible-look-directions (roslisp:ros-warn (perception-failure) "~a~%Turning head." e) (exe:perform (desig:a motion (type looking) (direction forward))) (setf ?looking-direction (first possible-look-directions)) (setf possible-look-directions (cdr possible-look-directions)) (exe:perform (desig:a motion (type looking) (target (desig:a location (pose ?looking-direction))))) (cpl:retry)) (cpl:fail 'common-fail:looking-high-level-failure))) (let ((?perceived-bottle (exe:perform (desig:a motion (type detecting) (object (desig:an object (type ?object-type))))))) (values ?perceived-bottle (get-preferred-arm-for-direction ?looking-direction))))))
Let's see what this method does. The with-failure-handling clause here deals with cram-common-failures:perception-object-not-found which if you remember, was the error raised when the robot couldn't find the bottle. So instead of only looking downwards, the code will now iterate between downwards, left and right, and only upon a failure in all these, will an error be bubbled up - thus increasing our effective field of view.
Also note that the find-object suggests an arm to use, depending on the direction it looked.
Let us also update our move-bottle to use this method.
(defun move-bottle (bottle-spawn-pose) (spawn-bottle bottle-spawn-pose) (pr2-proj:with-simulated-robot (let ((?navigation-goal *base-pose-near-table*)) (cpl:par (exe:perform (desig:a motion (type moving-torso) (joint-angle 0.3))) (pp-plans::park-arms) ;; Moving the robot near the table. (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?navigation-goal))))))) ;; Find and detect the bottle on the table. (multiple-value-bind (?perceived-bottle ?grasping-arm) (find-object :bottle) (exe:perform (desig:an action (type picking-up) (arm ?grasping-arm) (grasp left-side) (object ?perceived-bottle))) (pp-plans::park-arms :arm ?grasping-arm) ;; Moving the robot near the counter. (let ((?nav-goal *base-pose-near-counter*)) (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?nav-goal)))))) (coe:on-event (make-instance 'cpoe:robot-state-changed)) ;; Setting the object down on the counter (let ((?drop-pose *final-object-destination*)) (exe:perform (desig:an action (type placing) (arm ?grasping-arm) (object ?perceived-bottle) (target (desig:a location (pose ?drop-pose)))))) (pp-plans::park-arms :arm ?grasping-arm))))
Run move-bottle again, and this time, you'll find that the robot succeeds in transporting the bottle.
Expanding Failure Management Capabilities
Everything is good so far, but let's call this a lucky coincidence. For the robot, knowing which arm to use to pick up the bottle is not always enough. There are many positions with which we can grasp objects - from the object's front, back, left, right, etc. One might think that since the bottle is a rotationally symmetric object, it doesn't matter which side you approach from. But consider the bottle as an object model, which has a specific front, back and sides according to its orientation with respect to the bullet world. In which case, the side with which the arm approaches the bottle greatly matters, accounting for the configuration of the joints the robot arm will make while trying to grasp - some poses may be unachievable, while others may result in the collision of the arm with the table.
Let's try to visualize this issue, by spawning the bottle in yet another position:
PP-TUT> (spawn-bottle '((-1.1 -0.75 0.860) (0 0 0 1)))
Now clean up and run move-bottle once more, and the output should be something similar to as below:
PP-TUT> (move-bottle '((-1.1 -0.75 0.860) (0 0 0 1))) [(PICK-PLACE PICK-UP) INFO] 1550502686.470: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1550502686.470: Reaching [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1550502686.797: #<POSE-STAMPED FRAME-ID: "torso_lift_link", STAMP: 0.0 #<3D-VECTOR (0.6819813024319948d0 0.4206671881870251d0 -0.11278482277792945d0)> #<QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>> is unreachable for EE. Ignoring. [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1550502687.092: #<POSE-STAMPED FRAME-ID: "torso_lift_link", STAMP: 0.0 #<3D-VECTOR (0.6797228574484719d0 0.4210222500057509d0 -0.2627674055849005d0)> #<QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>> is unreachable for EE. Failing. [(PP-PLANS PICK-UP) WARN] 1550502687.092: Manipulation messed up: #<POSE-STAMPED FRAME-ID: "torso_lift_link", STAMP: 0.0 #<3D-VECTOR (0.6797228574484719d0 0.4210222500057509d0 -0.2627674055849005d0)> #<QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>> is unreachable for EE. Ignoring. ; Evaluation aborted on #<CRAM-COMMON-FAILURES:MANIPULATION-POSE-UNREACHABLE {10100E64C3}>.
The robot has failed to grasp again, even though the bottle is well within perception and grasping range.
So what went wrong? If you go back to definition of get-preferred-arm-for-direction:
(defun get-preferred-arm-for-direction (direction-looked) (let ((preferred-arm :RIGHT)) ;; Always prefers right arm unless looking at the left side (when (eq direction-looked *left-downward-look-coordinate*) (setf preferred-arm :LEFT)) preferred-arm))
And the part where we pick up the object:
(exe:perform (desig:an action (type picking-up) (arm ?grasping-arm) (grasp left-side) (object ?perceived-bottle)))
We see that the robot defaults the right arm when the object is in the center and will always try to grasp the left side of the bottle, even when the object is slightly favoring the left side under it (Refer the figure below).
Note: We have been using left-side here a lot. In the context of (grasp left-side), it refers to the left-side of the bottle. But what defines the “left-side”?
Before we understand it, we have to acknowledge that every object spawned in the bullet world has its own coordinate axes - even rotationally symmetric objects like the bottle here. And the definition for left-side is the side of the -ve Y-axis in the bottle coordinate frame. Similarly right-side is +ve Y-axis, front is denoted by +ve X-axis and the back by -ve X-axis.
With this the “left-side” grasp can change based on how the object is oriented in the world. But it remains the w.r.t the object coordinate frame.
The file at <your workspace>/cram/cram_common/cram_object_knowledge/src/household.lisp holds some definitions for grasps supported by the household objects supporte in CRAM.
Once again, let's formulate a strategy like the previous case here. The plan we are going for will look something like this:
- Choose the favored arm.
- Get all possible grasp poses for the given type of the object and the arm.
- Choose one grasp from the possible grasp list.
- Try to grasp the object.
- If,
- Grasp succeeds - Continue with the rest of the code
- Grasp Fails - Choose a different grasp pose from the list.
- When no more possible grasp poses, try changing the arm used to grasp Once and try resuming from Step (2)
- When attempted with all arms and grasps, error out.
Let's encapsulate all this in a method called pick-up-object:
(defun pick-up-object (?perceived-object ?object-type ?grasping-arm) (let ((?possible-arms '(:right :left))) ;;Retry by changing the arm (cpl:with-retry-counters ((arm-change-retry 1)) (cpl:with-failure-handling ((common-fail:object-unreachable (e) (roslisp:ros-warn (arm-failure) "Manipulation failed: ~a~%" e) (cpl:do-retry arm-change-retry (setf ?grasping-arm (car (remove ?grasping-arm ?possible-arms))) (cpl:retry)) (roslisp:ros-warn (arm-failures) "No more retries left"))) ;; Retry by changing the grasp (let* ((?possible-grasp (cram-object-interfaces:get-object-type-grasps ?object-type nil nil nil ?grasping-arm)) (?grasp (cut:lazy-car ?possible-grasp))) (cpl:with-retry-counters ((grasp-retries 3)) (cpl:with-failure-handling (((or cram-common-failures:manipulation-pose-unreachable cram-common-failures:gripper-closed-completely) (e) (roslisp:ros-warn (grasp-failure) "~a~%Failed to grasp from ~a using ~a arm " e ?grasp ?grasping-arm) (cpl:do-retry grasp-retries (when (cut:lazy-car ?possible-grasp) (roslisp:ros-info (trying-new-grasp) "Trying to grasp from ~a using ~a arm" ?grasp ?grasping-arm) (setf ?possible-grasp (cut:lazy-cdr ?possible-grasp)) (pp-plans::park-arms) (setf ?grasp (cut:lazy-car ?possible-grasp)) (cpl:retry))) (roslisp:ros-warn (grasp-failures) "No more retries left") (cpl:fail 'common-fail:object-unreachable))) ;; Perform the grasp (exe:perform (desig:an action (type picking-up) (arm ?grasping-arm) (grasp ?grasp) (object ?perceived-object))))))))) ?grasping-arm)
With this, the pick-up-object can now iterate through all possible grasp configurations stored in ?possible-grasp. The cram-object-interaces:get-object-type-grasps is a useful method which will give these values as a lazy list, provided the grasping arm and the object type.
There are two nested failure-handling clauses here. The inner failure-handling part will take care of cram-common-failures:manipulation-pose-unreachable and cram-common-failures:gripper-closed-completely - both of which are errors which can occur during a failed grasp. The recovery method will iterate through all possible grasp configurations for a given arm. Upon failure of this, the outer failure handling clause will take care of the common-fail:object-unreachable error thrown by the inner part and change the arm it's trying to grasp the object with and then retry the entire procedure again.
And only upon the failure of all these will the error be bubbled up. The method also returns the grasping arm it used to pick the object up so that the rest of the code is aware of the arm in which the bottle rests. Also, note that the arm we decided on earlier in find-object now acts as a bias to reduce the number of searches we do to find a successful configuration. We have also written appropriate warning statements to be informed about the actions the robot is taking.
Also let's redefine move-bottle again to include these:
(defun move-bottle (bottle-spawn-pose) (spawn-bottle bottle-spawn-pose) (pr2-proj:with-simulated-robot (let ((?navigation-goal *base-pose-near-table*)) (cpl:par (exe:perform (desig:a motion (type moving-torso) (joint-angle 0.3))) (pp-plans::park-arms) ;; Moving the robot near the table. (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?navigation-goal))))))) (multiple-value-bind (?perceived-bottle ?grasping-arm) (find-object :bottle) (setf ?grasping-arm (pick-up-object ?perceived-bottle :bottle ?grasping-arm)) (pp-plans::park-arms :arm ?grasping-arm) ;; Moving the robot near the counter. (let ((?nav-goal *base-pose-near-counter*)) (exe:perform (desig:a motion (type going) (target (desig:a location (pose ?nav-goal)))))) (coe:on-event (make-instance 'cpoe:robot-state-changed)) ;; Setting the object down on the counter (let ((?drop-pose *final-object-destination*)) (exe:perform (desig:an action (type placing) (arm ?grasping-arm) (object ?perceived-bottle) (target (desig:a location (pose ?drop-pose)))))) (pp-plans::park-arms :arm ?grasping-arm))))
You should see a result that looks like the one below. [Some messages that would come up are suppressed here for readability.]
PP-TUT> (init-projection) PP-TUT> (move-bottle) [(PICK-PLACE PICK-UP) INFO] 1550504321.279: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1550504321.279: Reaching [(GRASP-FAILURE) WARN] Failed to grasp from LEFT-SIDE using RIGHT arm [(TRYING-NEW-GRASP) INFO] 1550504800.749: Trying to grasp from RIGHT-SIDE using RIGHT arm [(PICK-PLACE PICK-UP) INFO] 1550504800.789: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1550504800.789: Reaching [(GRASP-FAILURE) WARN] Failed to grasp from RIGHT-SIDE using RIGHT arm [(TRYING-NEW-GRASP) INFO] 1550504801.577: Trying to grasp from BACK using RIGHT arm [(PICK-PLACE PICK-UP) INFO] 1550504801.601: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1550504801.602: Reaching [(PICK-PLACE PICK-UP) INFO] 1550504801.939: Gripping [(PICK-PLACE PICK-UP) INFO] 1550504801.973: Assert grasp into knowledge base [(PICK-PLACE PICK-UP) INFO] 1550504801.974: Lifting [(PICK-PLACE PLACE) INFO] 1550504802.356: Reaching [(PICK-PLACE PLACE) INFO] 1550504802.508: Putting [(PICK-PLACE PLACE) INFO] 1550504802.619: Opening gripper [(PICK-PLACE PLACE) INFO] 1550504802.655: Retract grasp in knowledge base [(PICK-PLACE PLACE) INFO] 1550504802.660: Retracting
The robot has once again succeeded in grasping the object.
