Implementing simple plans to move a turtle

Description: In this tutorial you will learn how to implement a simple plan to move a turtle from waypoint to waypoint.

Previous Tutorial: Controlling turtlesim from CRAM
Next Tutorial: Using Prolog for reasoning

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

$ roscore

$ rosrun turtlesim turtlesim_node

And in the REPL the following commands need to be executed:

CL-USER>(ros-load:load-system "cram_my_beginner_tutorial" :cram-my-beginner-tutorial)
...
CL-USER>(in-package :tut)
...
TUT>(start-ros-node "turtle1")
...
TUT>(init-ros-turtle "turtle1")

To move from one waypoint to the next one, we need to calculate the angular and linear velocity commands. We do that by transforming the goal pose into the turtle's coordinate frame and then calculating the angle from the current turtle position towards the goal.

Go to the src directory of your tutorial and create a new file, simple-plans.lisp. Open it and add the following code:

(in-package :tut)
 
(defun pose-msg->transform (msg)
  "Returns a transform proxy that allows to transform into the frame
given by x, y, and theta of `msg'."
  (with-fields (x y theta) msg
    (cl-transforms:make-transform
     (cl-transforms:make-3d-vector x y 0)
     (cl-transforms:axis-angle->quaternion
      (cl-transforms:make-3d-vector 0 0 1)
      theta))))
 
(defun relative-angle-to (goal pose-msg)
  "Given a `pose-msg' as a turtlesim-msg:pose and a `goal' as cl-transforms:3d-vector,
calculate the angle by which the pose has to be turned to point toward the goal."
  (let ((diff-pose (cl-transforms:transform-point
                     (cl-transforms:transform-inv
                       (pose-msg->transform pose-msg))
                     goal)))
    (atan
      (cl-transforms:y diff-pose)
      (cl-transforms:x diff-pose))))
 
(defun calculate-angular-cmd (goal &optional (ang-vel-factor 8))
  "Uses the current turtle pose and calculates the angular velocity command
to turn towards the goal."
  (* ang-vel-factor
     (relative-angle-to goal (value *turtle-pose*))))

You will also need to update your *.asd file by adding a dependency to the newly created simple-plans.lisp by adding this line to the :components field of the src module:

             (:file "simple-plans" :depends-on ("package" "control-turtlesim"))

Now lets see what the code does. The function pose-msg→transform returns a transform frame for a given pose message. In relative-angle-to we use it to calculate diff-pose, which is the transformed goal pose, and then we calculate the relative angle to the goal pose from the turtle pose, such that the result may be, e.g. “20 degrees to the left”. (calculate-angular-cmd goal) then just retrieves the current turtle pose from the fluent and multiplies the angle by a factor to make the turtle turn faster.

As an example, assume the turtle is at (5, 5) facing the direction of y axis (“north”), and the goal is at (1, 2). Then, we transform (1, 2) into coordinates relative to the turtle's frame, which is 3 “behind” the turtle and 4 to its “left”, meaning (-3, 4). The atan function then gives us the angle by which the turtle needs to turn to face this point. We multiply this by a factor to make the turtle turn faster or slower.

In order to move the turtle towards a point, we need to continuously recalculate and send the velocity command at a specific rate. The rate must be sufficiently high, since the sent command needs to change while the turtle moves. Let's see if our code does what it should do. Enter in the Lisp REPL:

TUT> (dotimes (i 100) 
       (send-vel-cmd 
         1.5 ; linear speed
         (calculate-angular-cmd (cl-transforms:make-3d-vector 1 1 0)))
       (wait-duration 0.1))

(Note: each time you add some new code to a .lisp file and want to try it out in REPL you need to recompile it. So either press C-c C-c with the cursor on the function which would recompile only the function, or press C-c C-k to recompile the file, or reload your complete ASDF system.)

The code calls send-vel-cmd in a loop 100 times.

The turtle should now move towards the bottom left corner and finally move along a circle around the goal until the loop finishes.

Now we need to write a simple plan that recalculates and executes the velocity command until we reach the goal. The easiest way to do that is to construct a fluent over the distance to the goal and use pursue to monitor it in parallel to the control loop. Here the code, which you should append to simple-plans.lisp:

(def-cram-function move-to (goal &optional (distance-threshold 0.1))
  "Sends velocity commands until `goal' is reached."
 (let ((reached-fl (< (fl-funcall #'cl-transforms:v-dist
                                   (fl-funcall
                                    #'cl-transforms:translation
                                    (fl-funcall
                                     #'pose-msg->transform
                                     *turtle-pose*))
                                   goal)
                       distance-threshold)))
    (unwind-protect
         (pursue
           (wait-for reached-fl)
           (loop do
             (send-vel-cmd
               1.5
               (calculate-angular-cmd goal))
             (wait-duration 0.1)))
      (send-vel-cmd 0 0))))

When we want to use CRAM language features such as pursue, we can use def-cram-function instead of defun. Code in def-cram-function leads to the creation of a task tree node, which makes it transparent for reasoning and enables online transformation of the code. But if you don't need these features you can just as well use defun.

The fluent network that we construct looks pretty complicated. We first transform the pose message into a cl-transforms:transform, then we access the translation slot over which we calculate the euclidean distance to the goal.

The pursue block returns as soon as the turtle is within a threshold to the goal. Without an additional message to the controller, the turtle might move on a bit, thus going beyond the goal. Therefore, we send a velocity command afterwards. For robotics it is also important to consider the cases of failures or interrupts of controllers by a developer to prevent robots from driving into walls. Therefore, it is in general necessary to wrap your code in an unwind-protect to implement emergency behaviors.

The pursue form terminates whenever one of the two body forms terminate. The second form is an endless loop just recalculating and re-sending the command. The first form terminates as soon as we reached the goal.

To execute CRAM Plan Language code (e.g. pursue or def-cram-function) we need to either call it from a function that was defined with def-top-level-cram-function or we need to wrap it in a top-level form, which will create a root for our task tree. Let's try it out. Enter the following in the REPL:

TUT> (top-level
       (dolist (goal '((9 1 0) (9 9 0) (1 9 0) (1 1 0) (9 1 0)))
         (move-to (apply #'cl-transforms:make-3d-vector goal))))

We pass the dolist macro a list of lists containing coordinates in the TurtleSim world. It iterates over this list, storing one of the coordinates in goal each iteration. We then use apply to pass the contents of goal to make-3d-vector and pass the resulting vector to move-to. This let's the turtle follow a trajectory described by the coordinates.

The turtle should now move along a rectangle.

Moving along predetermined points is all fine and good, but let's have a look at a more flexible way that CRAM provides to specify and reason about parameters. To learn about motion parameter, we, however, first need to understand how the Lisp Prolog works …

Using Prolog for reasoning