This is an old revision of the document!


How fetch and deliver plans are implemented

We will explain the whole plan implementation pipeline of CRAM bottom up. As the bottom-most level we will use the projection environment.

The package that we will use for examples is cram_pr2_pick_place_demo.

There is a launch file in the package, launch it before starting a node in Lisp.

To start the node in Lisp do:

CL-USER> (roslisp-utilities:startup-ros)

Low-level projection functionality

To move the robot in the Bullet world we use the low-level functions of projection:

cram_pr2_projection/src/low-level.lisp

All the low-level functions use Prolog API of Bullet world to change it.

For example, to drive:

CL-USER> (pr2-proj::drive (cl-transforms-stamped:make-pose-stamped
                           "map"
                           0.0
                           (cl-transforms:make-3d-vector 0.5 0 0)
                           (cl-transforms:make-identity-rotation)))

To move torso:

CL-USER> (pr2-proj::move-torso 0.3)

To call perception, let's spawn an object and perceive it:

CL-USER> (btr-utils:spawn-object 'cup-1 :cup
                                 :pose (cl-transforms:make-pose
                                        (cl-tf:make-3d-vector 1.5 0.0 1.3)
                                        (cl-tf:make-identity-rotation)))
CL-USER> (pr2-proj::detect (desig:an object (type cup)))
#<A OBJECT
    (TYPE CUP)
    (NAME CUP-1)
    (POSE ((:POSE
            #<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
   FRAME-ID: "base_footprint", STAMP: 1.531307883284417d9
   #<3D-VECTOR (1.0d0 0.0d0 1.299999998509884d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>)
           (:TRANSFORM
            #<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED 
   FRAME-ID: "base_footprint", CHILD-FRAME-ID: "cup_1", STAMP: 1.531307883284417d9
   #<3D-VECTOR (1.0d0 0.0d0 1.299999998509884d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>)))>

Motion designators for mobile manipulation

For any mobile manipulation robot the common motion designators of CRAM are supported. This is used, such that the pick and place plans could be applied to any robot in real world or simulation.

The definitions are in cram_common/cram_common_designators/src/motions.lisp

Motion designators are resolved (or ground) into one command symbol and arguments. The command symbol belongs to the cram_common_designators package namespace.

Projection process modules

The PMs are implemented in cram_pr2_projection/src/process-modules.lisp

To directly call a PM, use PM-EXECUTE function:

CL-USER> (pr2-proj:with-projected-robot
             (cram-process-modules:pm-execute
              'pr2-proj::pr2-proj-navigation
              (let ((?pose (cl-transforms-stamped:make-pose-stamped
                            "map" 0.0
                            (cl-transforms:make-3d-vector 0.75 0 0)
                            (cl-transforms:make-identity-rotation))))
                (desig:a motion
                         (type going)
                         (target (desig:a location
                                          (pose ?pose)))))))

What happens is, we send a motion designator to the specific process module.

To automatically dispatch motion designators to their correct PMs, there are the matching-process-module Prolog predicates (implementation is in the same process-modules.lisp file).

To execute a motion with automatic PM dispatching, use exe:perform:

CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (let ((?pose (cl-transforms-stamped:make-pose-stamped
                            "map" 0.0
                            (cl-transforms:make-3d-vector 0 0 0)
                            (cl-transforms:make-identity-rotation))))
                (desig:a motion
                         (type going)
                         (target (desig:a location
                                          (pose ?pose)))))))

pr2-proj:with-projected-robot is implemented in cram_pr2_projection/src/projection-environment.lisp

It activates all the PMs defined in PR2's projection and sets up some environment variables to distinguish between real robot and projected robot, for example the TF publishers and subscribers, in order not to mess up the real robot's TF while it is imagining things in projection.

Atomic actions

Implementation is in cram_common/cram_mobile_pick_place_plans/src/atomic-action-plans.lisp

One atomic action calls one motion designator + does some primitive failure handling + throws world state change events.

We can perform an action of type going, for example:

CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (let ((?pose (cl-transforms-stamped:make-pose-stamped
                            "map" 0.0
                            (cl-transforms:make-3d-vector 0 0 0)
                            (cl-transforms:make-identity-rotation))))
                (desig:an action
                          (type going)
                          (target (desig:a location
                                           (pose ?pose)))))))

Or actions for moving arms:

CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (let ((?pose (cl-transforms-stamped:make-pose-stamped
                            "base_footprint" 0.0
                            (cl-transforms:make-3d-vector 0.7 0.5 0.8)
                            (cl-transforms:make-identity-rotation))))
                (desig:an action
                          (type lifting)
                          (left-poses (?pose))))))
 
CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (let ((?pose (cl-transforms-stamped:make-pose-stamped
                            "base_footprint" 0.0
                            (cl-transforms:make-3d-vector 0.7 0.5 0.8)
                            (cl-transforms:make-identity-rotation))))
                (desig:an action
                          (type reaching)
                          (left-poses (?pose))))))

The atomic action plan for moving arm in cartesian space is move-arms-in-sequence from atomic-action-plans.lisp.

Composite actions: picking and placing

Picking up action does not have any navigation. The robot has to be standing in the correct place and looking at the object and having had perceived it already.

Let's pick up a cup:

CL-USER> (btr-utils:spawn-object 'cup-1 :cup
                                 :pose (cl-transforms:make-pose
                                        (cl-tf:make-3d-vector 0.5 0.3 1.0)
                                        (cl-tf:make-identity-rotation)))

First we need to navigate to (0 0 0), then we need to look at the object:

CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (let ((?pose (cl-transforms-stamped:make-pose-stamped
                            "base_footprint" 0.0
                            (cl-transforms:make-3d-vector 0.7 0.5 0.8)
                            (cl-transforms:make-identity-rotation))))
                (desig:an action
                          (type looking)
                          (target (desig:a location (pose ?pose)))))))

Then we perceive the object:

CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (desig:an action
                        (type detecting)
                        (object (desig:an object (type cup))))))

Then we pick it up. Picking up is implemented in pick-place-plans.lisp and it's corresponding -designators.lisp.

CL-USER> (pr2-proj:with-projected-robot
             (let ((?obj *))
               (cram-executive:perform
                (desig:an action
                          (type picking-up)
                          (object ?obj)))))

The missing information is inferred automatically.

But, we can specify the arm explicitly:

CL-USER> (pr2-proj:with-projected-robot
             (let ((?obj *))
               (cram-executive:perform
                (desig:an action
                          (type picking-up)
                          (object ?obj)
                          (arm left)))))

And the grasp type as well:

(pr2-proj:with-projected-robot
             (let ((?obj *))
               (cram-executive:perform
                (desig:an action
                          (type picking-up)
                          (object ?obj)
                          (arm left)
                          (grasp side)))))

Object got attached through an event, so now it will always follow the robot:

CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (let ((?pose (cl-transforms-stamped:make-pose-stamped
                            "map" 0.0
                            (cl-transforms:make-3d-vector -0.5 0 0)
                            (cl-transforms:make-identity-rotation))))
                (desig:an action
                          (type going)
                          (target (desig:a location
                                           (pose ?pose)))))))

To get rid of the object call:

CL-USER> (pr2-proj:with-projected-robot
             (cram-executive:perform
              (desig:an action
                        (type placing)
                        (arm left))))

High-level action plans: fetching and delivering and manipulating environment and ...

Most high-level plans at the moment are implemented in cram_pr2_fetch_deliver_plans. The transport function is a high order function which will use most of the functions in cram_fetch_deliver_plans. Therefore we'll explain this function with an example and show the below function calls. Our goal with the PR2 is to put the bowl from the sink area on the table. To explain the other functions we are going to follow the execution of the other high level functions in the deliver function. We do this by loading the packages cram_pr2_pick_and_place and cram_bullet_world_tutorial

(ros-load:load-system "cram_pr2_pick_and_place" :cram_pr2_pick_and_place)
(ros-load:load-system "cram_bullet_world_tutorial" :cram_bullet_world_tutorial)

… and call the following functions:

(in-package :cram-bullet-world-tutorial)
(roslisp-utilities:startup-ros)
(urdf-proj:with-projected-robot 
	(cram-pr2-pick-place-demo::demo-random nil '(:bowl)))

After that the robot gets correctly positioned and kitchen objects spawned before starting.

Since we passed (demo-random) the object of type bowl the actions searching and transporting will be executed:

(when (eq ?object-type :bowl)
  (cpl:with-failure-handling
    ((common-fail:high-level-failure (e)
      (roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping the search" e)
      (return)))
    (let ((?loc (cdr (assoc :breakfast-cereal object-fetching-locations))))
      (exe:perform
        (desig:an action
                  (type searching)
                  (object (desig:an object (type breakfast-cereal)))
                  (location ?loc))))))
 
(cpl:with-failure-handling
    ((common-fail:high-level-failure (e)
        (roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping..." e)
        (return)))
  (if (eq ?object-type :bowl)
      (exe:perform
       (desig:an action
                 (type transporting)
                 (object ?object-to-fetch)
                 (location ?fetching-location)
                 (target ?delivering-location)))

If any of the actions fails, we just skip them. In the above code snippet the action of type searching is first executed and then an action of type transporting. Since the transport action uses more high-level functions, we will explain its contents at the end and start with functions which are direct or indirect used by the transport action. Let's start with search-for-object first, which is called in the executing of the searching action.

search-for-object

(defun search-for-object (&key
                           ((:object ?object-designator))
                           ((:location ?search-location))
                           ((:robot-location ?robot-location))
                           (retries 3)
                          &allow-other-keys)
(declare (type desig:object-designator ?object-designator)
;; location desigs can turn NILL in the course of execution
;; but should not be passed as NILL to start with.
(type (or desig:location-designator null) ?search-location ?robot-location))
"Searches for `?object-designator' in its likely location `?search-location'.
If the object is not there or navigation location is unreachable,
retries with different search location or robot base location."

The input parameters are the object which should be searched from the location ?robot-location at the location ?search-location. retries says how often we should try again to find a object at a specific robot location.

(cpl:with-failure-handling
    ((desig:designator-error (e)
      (roslisp:ros-warn (fd-plans search-for-object)
                        "Desig ~a could not be resolved: ~a~%Propagating up."
                        ?search-location e)
      (cpl:fail 'common-fail:object-nowhere-to-be-found
                :description "Search location designator could not be resolved.")))

If the referencing of below designators fails the error object-nowhere-to-be-found will be thrown.

The basic idea is the following: First the robot tries to navigate to the location ?robot-location. If this is succeeded within a number of retries the robot tries now to turn towards the location ?search-location and detect the object within the number of specific retries too. Now if the robot cannot detect the object or cannot navigate to the ?robot-location, we jump above the navigation part, set a new ?search-location and try all above again. This is done maximal outer-search-location-retries times.

;; take new `?search-location' sample if a failure happens and retry
(cpl:with-retry-counters ((outer-search-location-retries 2))
  (cpl:with-failure-handling
    ((common-fail:object-nowhere-to-be-found (e)
      (common-fail:retry-with-loc-designator-solutions
          ?search-location
          outer-search-location-retries
          (:error-object-or-string e
          :warning-namespace (fd-plans search-for-object)
          :reset-designators (list ?robot-location)
          :rethrow-failure 'common-fail:object-nowhere-to-be-found)
       (roslisp:ros-warn (fd-plans search-for-object)
                         "Search is about to give up. Retrying~%"))))

So here we set a new ?search-location since we could not move to robot or/and find detect the object.

;; if the going action fails, pick another `?robot-location' sample and retry
(cpl:with-retry-counters ((robot-location-retries 10))
  (cpl:with-failure-handling
      (((or common-fail:navigation-goal-in-collision
            common-fail:looking-high-level-failure
            common-fail:perception-low-level-failure) (e)
         (common-fail:retry-with-loc-designator-solutions
             ?robot-location
             robot-location-retries
             (:error-object-or-string e
              :warning-namespace (fd-plans search-for-object)
              :reset-designators (list ?search-location)
              :rethrow-failure 'common-fail:object-nowhere-to-be-found))))
 
     ;; navigate
     (exe:perform (desig:an action
                            (type navigating)
                            (location ?robot-location)))

Here we try to navigate the robot to ?robot-location and if it fails we try robot-location-retries times again before throwing object-nowhere-to-be-found and jump in the above code snippet. To understand how this action will be resolved we take a look at an example action:

#<A ACTION
	(TYPE NAVIGATING)
	(LOCATION #<A LOCATION
		(VISIBLE-FOR PR2)
		(LOCATION #<A LOCATION
			(ON #<A OBJECT
				(TYPE COUNTER-TOP)
				(URDF-NAME SINK-AREA-SURFACE)
				(OWL-NAME kitchen_sink_block_counter_top)
				(PART-OF KITCHEN)>)
			(SIDE LEFT)
			(SIDE FRONT)
			(RANGE 0.5)>)>)>

To resolve the nested location designator ?robot-location in this action the function cram_manipulation_interfaces::get-location-poses will be called twice with the following location designators:

#<A LOCATION
(VISIBLE-FOR PR2)
(LOCATION #<A LOCATION
	(ON #<A OBJECT
		(TYPE COUNTER-TOP)
		(URDF-NAME SINK-AREA-SURFACE)
		(OWL-NAME "kitchen_sink_block_counter_top")
		(PART-OF KITCHEN)>)
	(SIDE LEFT)
	(SIDE FRONT)
	(RANGE 0.5)>)> 

This designator is a subset of the action designator above and …

#<A LOCATION
(ON #<A OBJECT
	(TYPE COUNTER-TOP)
	(URDF-NAME SINK-AREA-SURFACE)
	(OWL-NAME "kitchen_sink_block_counter_top")
	(PART-OF KITCHEN)>)
(SIDE LEFT)
(SIDE FRONT)
(RANGE 0.5)>

this location designator is the subset of the above location designator. Since we have two location designators to resolve the function cram_manipulation_interfaces::get-location-poses will be called twice returning both times a lazy list of poses:

(#<CL-TRANSFORMS:POSE 
#<3D-VECTOR (1.5d0 0.9000000953674316d0 0.8500000258286794d0)>
#<QUATERNION (0.0d0 0.0d0 0.9084313017742992d0 0.41803417319239833d0)>>
. #S(CRAM-UTILITIES::LAZY-CONS-ELEM
:GENERATOR #<CLOSURE (LAMBDA () :IN CRAM-UTILITIES:LAZY-MAPCAN)
{1018B51EBB}>)) 

Since the type of the action is navigating the function cram_fetch_deliver_plans::go-without-collisions can be evaluated with the above pose. The procedure can be visualized with the Bullet World too:

This is the costmap after we called get-location-poses the first time only with the object designator.

This is the costmap after we called the nested location designator with get-location-poses.

This is after the execution of the navigation action designator.

;; if perception action fails, try another `?search-location' and retry
(cpl:with-retry-counters ((search-location-retries retries))
  (cpl:with-failure-handling
      (((or common-fail:perception-low-level-failure
            common-fail:looking-high-level-failure) (e)
         (common-fail:retry-with-loc-designator-solutions
             ?search-location
             search-location-retries
             (:error-object-or-string e
              :warning-namespace (fd-plans search-for-object)
              :reset-designators (list ?robot-location)))))
 
       (exe:perform (desig:an action
                              (type turning-towards)
                              (target ?search-location)))
       (exe:perform (desig:an action
                              (type detecting)
                              (object ?object-designator)))))))))))

Here we try to turn towards the location to search for and try to detect the object. If on of these action fails, we try like the parameter retries says times again. If it stills fails we throw the error object-nowhere-to-be-found and try therefore then first to navigate the robot before detecting again the object. The detection action calls the perceive function in cram_mobile_pick_place_plans/src/atomic-action-plans to detect the object and returns a motion designator. The function turn-towards will create a looking action designator which calls the look-at function in the above file too. At the end the function search-for-object returns an object designator with a pose like this e. g.:

#<A OBJECT
(LOCATION #<A LOCATION
	(ON #<A OBJECT
		(TYPE COUNTER-TOP)
		(URDF-NAME SINK-AREA-SURFACE)
		(OWL-NAME "kitchen_sink_block_counter_top")
		(PART-OF KITCHEN)>)
	(SIDE LEFT)
	(SIDE FRONT)
	(RANGE-INVERT 0.5)>)
(TYPE BOWL)
(NAME BOWL-1)
(POSE ((:POSE
#<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
FRAME-ID: "base_footprint", STAMP: 1.560865249078827d9
#<3D-VECTOR (0.8618418535601576d0 0.028947115308607385d0 0.8886420529683431d0)>
#<QUATERNION (-0.004970445237876797d0 -0.0035147137040045353d0 0.10407392205337866d0 0.994550963155759d0)>>)
(:TRANSFORM
#<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED 
FRAME-ID: "base_footprint", CHILD-FRAME-ID: "bowl_1", STAMP: 1.560865249078827d9
#<3D-VECTOR (0.8618418535601576d0 0.028947115308607385d0 0.8886420529683431d0)>
#<QUATERNION (-0.004970445237876797d0 -0.0035147137040045353d0 0.10407392205337866d0 0.994550963155759d0)>>)
(:POSE-IN-MAP
#<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
FRAME-ID: "map", STAMP: 1.560865249078827d9
#<3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>
#<QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>)
(:TRANSFORM-IN-MAP
#<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED 
FRAME-ID: "map", CHILD-FRAME-ID: "bowl_1", STAMP: 1.560865249078827d9
#<3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>
#<QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>)))> 

go-without-collisions

(defun go-without-collisions (&key
                                ((:location ?navigation-location))
                              &allow-other-keys)
  (declare (type desig:location-designator ?navigation-location))
  "Check if navigation goal is in reach, if not propagate failure up,
if yes, perform GOING action while ignoring failures."
 
  (exe:perform (desig:an action
                         (type positioning-arm)
                         (left-configuration park)
                         (right-configuration park)))

First we move the arms in the park position, so they do not hit anything on their way.

(proj-reasoning:check-navigating-collisions ?navigation-location)

Then we check if the requested location is in collision with the bullet world.

(setf ?navigation-location (desig:current-desig ?navigation-location))
 
  (cpl:with-failure-handling
      ((common-fail:navigation-low-level-failure (e)
         (roslisp:ros-warn (pp-plans navigate)
                           "Low-level navigation failed: ~a~%.Ignoring anyway." e)
         (return)))
    (exe:perform (desig:an action
                           (type going)
                           (target ?navigation-location)))))

Then we try to go with the action designator to the desired location, if it fails, because it was an navigation-low-level-failure, we print a message and ignore it by returning.