Introducing a new robot to CRAM

To understand how to introduce a new robot to CRAM, let us first look at how PR2 is integrated into the system.

Motion designators

What are they?

First of all, we need to define the atomic actions that the robot is supposed to execute.

For mobile manipulation robots that would include driving, moving-arm, opening-gripper, moving-neck, etc.

For a flying robot with a distance sensor that would include flying, distance-sensing, etc.

For a human-robot interaction robot that would include saying, listening.

For mobile manipulation robots of CRAM the defined motion designators can be found in “cram_common_designators” package in motions.lisp file:

This is how we define motion designator grounding aka referencing for looking motions:

(def-fact-group ptu-motions (motion-grounding)
 
  (<- (desig:motion-grounding ?designator (move-head :pose ?pose))
    (desig:desig-prop ?designator (:type :looking))
    (desig:desig-prop ?designator (:target ?location-designator))
    (desig:designator-groundings ?location-designator ?poses)
    (member ?pose ?poses))
 
  (<- (desig:motion-grounding ?designator (move-head :frame ?frame))
    (desig:desig-prop ?designator (:type :looking))
    (desig:desig-prop ?designator (:frame ?frame)))
 
  (<- (desig:motion-grounding ?designator (move-head :direction ?direction))
    (desig:desig-prop ?designator (:type :looking))
    (desig:desig-prop ?designator (:direction ?direction))))

Motions are executed with PERFORM calls:

(on-real-pr2
  (perform
    (a motion
       (type going)
       (target (a location (pose xyz))))))
 
(on-simulated-pr2
  (perform
    (a motion
       (type going)
       (target (a location (pose xyz))))))
 
(on-real-boxy
  (perform
    (a motion
       (type going)
       (target (a location (pose xyz))))))     

As you can see, the PERFORM call stays exactly the same, only the environment setup commands (ON-REAL-BOXY etc) change, which completely change command execution.

Motion designators are the abstraction layer for plans from robot hardware platform

Executing motion designators on real PR2

Low-level ROS interfaces

We start with low-level ROS clients for our PR2: it's the cram_pr2_low_level package.

This package has clients for different ROS actions / services / topics that the real PR2 robot provides.

E.g., let's look at the looking action:

https://github.com/cram2/cram/blob/master/cram_pr2/cram_pr2_low_level/src/ptu.lisp

Connecting low-level functions to motion designators -- process modules

A process module is a software component that is responsible for one resource of the robot, e.g. it's base, it's camera, it's neck, it's arms or it's left arm etc.

When you call

(perform (a motion
            (type looking)
            (direction forward)))

The PTU (pan-tilt-unit) process module automatically finds the low-level Lisp function that should be executed for motions of type looking.

It also does primitive resource management of the robot's neck resource: it enqueues all PERFORM calls that go to the neck such that they don't conflict with each other when you have a concurrent program.

Defining process modules

Question: how does the PM (process module) know which low-level function to call?

For the PR2 we can see the code in the cram_pr2_process_modules package, e.g. in ptu.lisp file.

First, the PM references the motion designator that it gets (that's implemented in a file we looked at before – motions.lisp).

Then it says, if the command from the referenced motion designator is cram-common-designators:move-head then I'll call the low-level function pr2-ll:call-ptu-action.

Matching process modules to designators

Question: how does the PTU PM get only motion designators of type looking and not of type moving-arm?

That is done by assigning different types of designators to different types of PMs – let's look in the designators.lisp file.

Performing the motions

(cram-process-modules:with-process-modules-running
    (pr2-pms:pr2-grippers-pm pr2-pms:pr2-ptu-pm)
  (cpl:top-level
    (exe:perform
     (desig:a motion
              (type looking)
              (frame "r_gripper_tool_frame")))))
 
(cram-process-modules:with-process-modules-running
    (pr2-pms:pr2-grippers-pm pr2-pms:pr2-ptu-pm)
  (cpl:top-level
    (let ((?pose-stamped
            (cl-transforms-stamped:make-pose-stamped
             "map"
             0.0
             (cl-transforms:make-3d-vector 0 0 0)
             (cl-transforms:make-quaternion 0 0 0 1))))
      (exe:perform
       (desig:a motion
                (type looking)
                (target (desig:a location
                                 (pose ?pose-stamped))))))))

Executing motion designators on projected PR2

Now let's take a different robot – the projected PR2 – and do the exercise one more time.

Low-level

Let's define the low-level interfaces for projected PR2 – the low-level.lisp file.

Process modules

Defining process modules

Now let's define a PTU process module – process-modules.lisp file.

Matching process modules to designators

Now let's say which PM corresponds to which motion designator – same file bottom.

Performing motions

The first line is something very specific for projection, so ignore it, if you're not writing projection code you will not need this.

(let ((cram-projection:*projection-environment* 'pr2-proj:pr2-bullet-projection-environment))
  (cram-process-modules:with-process-modules-running
      (pr2-proj::pr2-proj-ptu)
    (cpl:top-level
      (let ((?pose-stamped
              (cl-transforms-stamped:make-pose-stamped
               "map"
               0.0
               (cl-transforms:make-3d-vector 1.0 0 0.5)
               (cl-transforms:make-quaternion 0 0 0 1))))
        (exe:perform
         (desig:a motion
                  (type looking)
                  (target (desig:a location
                                   (pose ?pose-stamped)))))))))

Summary: your ToDo checklist to implement performing motion designators on your robot

  • Create a new CRAM package with dependency on cram-prolog and cram-designators. In your namespace use cram-prolog for convenience. Look at creating a CRAM package tutorial for reference.
  • Add a new file (or create a new package) for your low-level functionality. It should add dependencies on all ROS message packages that you use for communication, on the ROS Lisp API roslisp, on actionlib if you use ActionLib, roslisp-utilities if you use the ROS startup functionality etc. Message packages in ASDF are defined by adding -msg suffix to the ROS package name, e.g. std_msgs-msg. See roslisp tutorials for more on this.
  • Define process modules for your low-level functions. This will add dependency on cram-process-modules. See PM tutorial. Also define Prolog predicates matching-process-module and available-process-module for your PMs (see corresponding tutorial.
  • Finally, to test, perform the motions through with-process-modules-running macro. This will add a dependency on cram-executive, which defines the perform command.

Action designators and plans

This is TODO…

Projection for your new robot

This is TODO…