Creating motion designators for the TurtleSim

Description: In this tutorial you will learn what designators are, and in particular, work with motion designators: you will learn how to define one and how to use it.

Previous Tutorial: Using Prolog for reasoning
Next Tutorial: Creating process modules

To run the code in the tutuorial the roscore and the turtlesim need to be started in the terminal. Each in their own tab.

$ roscore

$ rosrun turtlesim turtlesim_node

From a user's point of view, a designator is a Common Lisp object that contains a sequence of key-value pairs representing a high-level, symbolic description of some aspect of robot's activity. The power and usefulness of the designator concept comes from the ability to infer concrete parameters when needed, based on user-specified rules, from the context in which the robot operates and the symbolic description in the designator. This is known as 'resolving' or '[de]referencing' a designator, and it returns an object containing the newly resolved values that can then be used by the robot to specify some task.

Currently there are the following types of designators defined in CRAM:

• location designators: describe locations taking various constraints into account (for example, reachability, visibility etc)
• object designators: describe objects on a symbolic level (what they are, what they may be used for etc)
• motion designators: describe a low-level motion that a robot should take and serve as input to process modules
• action designators: describe an high-level action, which can't be done with a simple (single) motion

These types should cover most use cases in robotics, however, if needed, new designator types can be defined as subclasses of the designator class.

As a simple example of a designator creation, suppose we want to create a designator for a location from which some object (for which we already have a designator) can be seen. We would then write

(defparameter *spy-location* (desig:a location (to see) (object ?prime-minister)))

(Do not add this code to your tutorial files, it is meant simply for illustration here.)

This line of code creates a location designator (*spy-location*) which “knows” of the given object (?prime-minister, which we assume is an already defined object designator), and knows that its purpose is to see the object. We do not actually specify a location in terms of coordinates at this moment. When we do want to find a suitable set of coordinates for this location, we would call

(reference *spy-location*)

and, assuming we have set up rules to take into account knowledge of the associated object, the environment, and the vision capabilities of the robot, we would have a process which can enumerate possible coordinates for the location that would satisfy its purpose and return these as coordinate vectors.

For the rest of this tutorial, we will work with motion designators and show a few basics of their creation and use. For a more detailed look at designators, it is recommended that you also read the cram_designators page.

First, you will need to add a few more dependencies to the tutorial package files you created previously. To package.xml, add dependencies on cram_prolog and cram_designators, but this time only with '<depend>:

  <depend>cram_prolog</depend>
  <depend>cram_designators</depend>

In your cram-my-beginner-tutorial.asd file, on the :depends-on line, add dependencies to cram-designators and cram-prolog. Let's also create a new source file for the code in this tutorial (under src directory), call it motion-designators.lisp. It can be empty for now. We will need to add the file to the :components of the src module in your cram-my-beginner-tutorial.asd, which should now look something like this:

(defsystem cram-my-beginner-tutorial
  :depends-on (cram-language roslisp turtlesim-msg turtlesim-srv
                             geometry_msgs-msg cl-transforms
                             cram-designators cram-prolog)
  :components
  ((:module "src"
            :components
            ((:file "package")
             (:file "control-turtlesim" :depends-on ("package"))
             (:file "simple-plans" :depends-on ("package" "control-turtlesim"))
             (:file "motion-designators" :depends-on ("package"))))))

Finally, we add cram-designators namespace to the use list of our package, for convenience, and import some of the symbols of the cram-prolog package. We don't use cram-prolog, as it causes conflicts with some of the similarly named symbols of the cram-language package:

(defpackage :cram-my-beginner-tutorial
  (:nicknames :tut)
  (:use :cpl :roslisp :cl-transforms :cram-designators)
  (:import-from :cram-prolog :def-fact-group :<- :lisp-fun))

Now, reload the tutorial in roslisp_repl (which also loads the newly added dependencies):

PROLOG> (ros-load:load-system "cram_my_beginner_tutorial" :cram-my-beginner-tutorial)
PROLOG> (in-package :tut)

Let's try to create a motion designator in the REPL command line:

TUT> (defparameter *my-desig* (desig:a motion (type driving) (speed 1.5)))
*MY-DESIG*
TUT> (desig-prop-value *my-desig* :speed)
1.5

We call the variable *my-desig* with asterisks as this is the common convention in Common Lisp for naming global variables.

We use the a macro to create designators. Internally it uses the make-designator function to create a designator of the specified type. The macro allows us to write cleaner code for designator creation.

As we can see, the a macro simply returns a designator object with the properties we specify. We can also read those already specified property values with the desig-prop-value function. So far, so good, but the interesting thing about designators is their resolution. Let's try that in the REPL command line:

TUT> (reference *my-desig*)
Cannot resolve motion designator #<MOTION-DESIGNATOR ((TYPE
                                                       DRIVING)
                                                      (SPEED
                                                       1.5)) {100888CED3}>.
   [Condition of type DESIGNATOR-ERROR]

This tells you there are no rules in place to resolve this designator, so let's try to provide a few such rules.

The function reference uses the CRAM Prolog engine to ground a motion designator into specific motion parameters. CRAM Prolog engine is a Prolog interpreter / compiler implemented as a domain-specific language within Lisp. To learn more about CRAM Prolog look at the Using Prolog for reasoning tutorial. To reference a motion designator, motion-grounding Prolog rule is being called on the given designator and the rule binds a tuple of command and specific motion parameters to its second argument.

Append the following to your motion-designators.lisp file:

(in-package :tut)
 
(defstruct turtle-motion
  "Represents a motion."
  speed
  angle)
 
(def-fact-group turtle-motion-designators (motion-grounding)
  ;; for each kind of motion, check for and extract the necessary info
 
  ;; drive and turn
  (<- (desig:motion-grounding ?desig (drive ?motion))
    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:speed ?speed))
    (desig-prop ?desig (:angle ?angle))
    (lisp-fun make-turtle-motion :speed ?speed :angle ?angle ?motion))
 
  ;; drive
  (<- (desig:motion-grounding ?desig (drive ?motion))
    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:speed ?speed))
    (lisp-fun make-turtle-motion :speed ?speed ?motion))
 
  ;; turn
  (<- (desig:motion-grounding ?desig (drive ?motion))
    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:angle ?angle))
    (lisp-fun make-turtle-motion :angle ?angle ?motion)))

Let's see what this code does. The defstruct declares a structure type to hold values resulting from the inference. It's an instantiation of our motion in a space of (possibly) continuous parameters, which we deduce from a symbolic description of the designator via rules given in the def-fact-group.

As for the inference rules themselves, these are Prolog code embedded in Lisp, for it is Prolog that powers the inference behind designator resolution. The def-fact-group is a collection of several rules, each of similar structure, so it helps to look at one of them in more detail:

  (<- (desig:motion-grounding ?desig (drive ?motion))
    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:speed ?speed))
    (desig-prop ?desig (:angle ?angle))
    (lisp-fun make-turtle-motion :speed ?speed :angle ?angle ?motion))

The first parameter to ← is the “rule head”

  (<- (desig:motion-grounding ?desig (drive ?motion))

while all other parameters comprise the “rule body”

    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:speed ?speed))
    (desig-prop ?desig (:angle ?angle))
    (lisp-fun make-turtle-motion :speed ?speed :angle ?angle ?motion)

Prolog's inference semantics is, in a nutshell, 'IF there is some assignment to variables such that all elements of the body are true, THEN use that assignment of variables to evaluate the head'. Note that in CRAM Prolog for a symbol to be considered a variable, its name must begin with a ?.

For the example rule we've selected, Prolog looks at the designator and asks “does it contain a key-value pair that is (:type :driving)?” (so no variables here except the designator itself). Assuming this is true, it then asks “does it contain a key-value pair that is (speed <some variable value>)?”. This last question has the effect of extracting the value of the speed parameter that we created the designator with and making it available to Prolog. This is done again to extract the value of the angle parameter. Lastly it uses lisp-fun to call a lisp function and store it's result in a variable available to Prolog. In this case it calls make-turtle-motion, which is the default function to instantiate our previously defined struct turtle-motion. It then stores the struct in ?motion.

If all of the above body rules are true (or can be executed sucessfully) then the head is itself evaluated, and what it does is glue the newly created ?motion, the object containing the concrete values, to the designator ?desig. Or in more concrete terms, it defines what the value of the motion-grounding predicate is if applied to ?desig, and that value is a list containing the instruction drive and the value of ?motion.

If in any step of the inference Prolog stumbles upon false (or NIL) it exists the current search branch and continues searching somewhere else.

For this tutorial, the inference is very simple: in fact, it is just extracting the parameters from the designators.

Reload the tutorial package in roslisp_repl. This will also load the newly defined inference rules.

Let's try to create, then reference a designator in the REPL command line again:

TUT> (defparameter *my-desig2* (desig:a motion (type driving) (speed 1.5) (angle 2)))
*MY-DESIG2
TUT> (reference *my-desig2*)
(DRIVE #S(TURTLE-MOTION :SPEED 1.5 :ANGLE 2))

No more resolution errors this time, as we have specified inference rules which allow us to have speed and angle parameters returned, which we could then pass to a process module that will actually move the turtle.

In the previous tutorial we added a simple plan to move to a location. This is a low level motion, so we should add a rule for it to our fact-group turtle-motion-designators:

(def-fact-group turtle-motion-designators (motion-grounding)
  ;; for each kind of motion, check for and extract the necessary info
 
  ;; drive and turn
  (<- (desig:motion-grounding ?desig (drive ?motion))
    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:speed ?speed))
    (desig-prop ?desig (:angle ?angle))
    (lisp-fun make-turtle-motion :speed ?speed :angle ?angle ?motion))
 
  ;; drive
  (<- (desig:motion-grounding ?desig (drive ?motion))
    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:speed ?speed))
    (lisp-fun make-turtle-motion :speed ?speed ?motion))
 
  ;; turn
  (<- (desig:motion-grounding ?desig (drive ?motion))
    (desig-prop ?desig (:type :driving))
    (desig-prop ?desig (:angle ?angle))
    (lisp-fun make-turtle-motion :angle ?angle ?motion))
 
  ;; move
  (<- (desig:motion-grounding ?desig (move ?motion))
    (desig-prop ?desig (:type :moving))
    (desig-prop ?desig (:goal ?goal))
    (lisp-fun apply make-3d-vector ?goal ?motion)))

This rule does almost the same as the others, but it only applies if the type of the designator is moving and the ?goal can be applied to make-3d-vector.

TUT> (desig:a motion (type moving) (goal (1 1 0)))
#<A MOTION
    (TYPE MOVING)
    (GOAL (1 1 0))>

Here we pass a list of numbers as the goal to the designator. The list represents a vector in 3D-space to describe the desired position of the turtle.

The turtles in the TurtleSim can also change the color and width of the pen to draw on the background. Append this to the motion-designators.lisp file:

(defstruct pen-motion
  "Represents a pen motion."
  (r 255) (g 255) (b 255)
  (width 1)
  (off 0))
 
(def-fact-group turtle-pen-motion-designators (motion-grounding)
  ;; for each kind of pen motion, check for and extract the necessary info
 
  ;; change color, width and on/off status
  (<- (desig:motion-grounding ?desig (set-pen ?motion))
    (desig-prop ?desig (:type :setting-pen))
    (desig-prop ?desig (:r ?r))
    (desig-prop ?desig (:g ?g))
    (desig-prop ?desig (:b ?b))
    (desig-prop ?desig (:width ?width))
    (desig-prop ?desig (:off ?off))
    (lisp-fun make-pen-motion :r ?r :g ?g :b ?b :width ?width :off ?off ?motion))
 
  ;; change color and width (implicates setting the pen to 'on')
  (<- (desig:motion-grounding ?desig (set-pen ?motion))
    (desig-prop ?desig (:type :setting-pen))
    (desig-prop ?desig (:r ?r))
    (desig-prop ?desig (:g ?g))
    (desig-prop ?desig (:b ?b))
    (desig-prop ?desig (:width ?width))
    (lisp-fun make-pen-motion :r ?r :g ?g :b ?b :width ?width :off 0 ?motion))
 
  ;; change on/off status (if set to 'on' the pen will have a default color and width)
  (<- (desig:motion-grounding ?desig (set-pen ?motion))
    (desig-prop ?desig (:type :setting-pen))
    (desig-prop ?desig (:off ?off))
    (lisp-fun make-pen-motion :off ?off ?motion)))

This is another fact-group for referencing motion designators of type setting-pen. We could have defined the rules in the other fact-group, but with a growing number of rules it is a good idea to organize them in different fact-groups.

The rules in this fact-group also just extract some info from the unreferenced designator and put it into a struct. It might seem a bit strange that we don't have a simple 'on'-switch for the pen, which doesn't change the color. That's because, when calling the SetPen service, we need to provide all the info (so also color and width) of the pen, which is also the reason why we give default values to the pen-motion's slots. To maintain the same color, one would need to store the color and width in variables when setting the pen to off. And then read them, when switching the pen back on.

We can test the referencing like this:

TUT> (defparameter *my-desig3* (desig:a motion (type setting-pen) (r 100) (g 150) (b 0) (width 5)))
*MY-DESIG3*
TUT> (reference *my-desig3*)
(SET-PEN #S(PEN-MOTION :R 100 :G 150 :B 0 :WIDTH 5 :OFF 0))

Seems to work fine. Like above with the drive designator, we now have a referenced designator, which we can pass to a process module to actually change the pen color and width.

Now that we know how to handle designators, we need to have the robot act on them in some way. CRAM process modules allow us to do just that …

Creating process modules