====== 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:** [[tutorials:beginner:controlling_turtlesim_2|Controlling turtlesim from CRAM]]\\
**Next Tutorial:** [[tutorials:beginner:cram_prolog|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")
===== Moving the turtle towards a point =====
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.
{{ :tutorials:beginner:turtlesim-tutorial.png?direct&600 |}}
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.
===== Writing a plan to move to a waypoint =====
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.
== Next ==
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 ...
[[tutorials:beginner:cram_prolog|Using Prolog for reasoning]]