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.

To see the plans in action use the demo function from cram_pr2_pick_place_demo package:

(pr2-proj:with-simulated-robot
  (demo::demo-random nil))

Example transporting action:

CL-USER> (demo::spawn-objects-on-sink-counter)
CL-USER> (pr2-proj:with-projected-robot
             (exe:perform
              (desig:an action
                        (type transporting)
                        (object (desig:an object (type breakfast-cereal)))
                        (location (desig:a location
                                           (on (desig:an object
                                                         (type counter-top)
                                                         (urdf-name sink-area-surface)
                                                         (part-of kitchen)))))
                        (target (desig:a location
                                         (on (desig:an object
                                                       (type counter-top)
                                                       (urdf-name kitchen-island-surface)
                                                       (part-of kitchen))))))))