Differences

This shows you the differences between two versions of the page.

--- doc:beginner:simple_plans [2013/12/17 13:44] – gkazhoya
+++ doc:beginner:simple_plans [2015/05/11 16:59] (current) – removed gkazhoya
@@ Line 1: / Line 1: @@
-====== 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.
-**Next Tutorial:**
-===== 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. Open the file ''tutorial.lisp'' of the previous tutorial and add the following code:
-<code lisp>
-(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)
-  "Given a pose as 3d-pose msg and a goal as 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))
-                     goal)))
-    (atan
-      (cl-transforms:y diff-pose)
-      (cl-transforms:x diff-pose))))
-(defun calculate-angular-cmd (goal &optional (ang-vel-factor 4))
-  "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*))))
-</code>
-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 y axis direction ("north"), and the goal is at ''(1, 2)''. Then, we transform ''(1, 2)'' into coordinates relative to the turtles frame, which is 4 "behind" the turtle and 3 to its "left", meaning ''(-4, 3)''. The ''atan'' function then gives us the angle by which the tutle 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:
-<code lisp>
-TUT> (dotimes (i 100)
-       (send-vel-cmd
-.5 ; linear speed
-         (calculate-angular-cmd (cl-transforms:make-3d-vector 1 1 0)))
-       (wait-duration 0.1))
-</code>
-(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 each select the function and press C-c C-c 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:
-<code lisp>
-(def-cram-function move-to (goal &optional (distance-threshold 0.1))
-  (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
-.5
-               (calculate-angular-cmd goal))
-             (wait-duration 0.1)))
-      (send-vel-cmd 0 0))))
-</code>
-When we want to use CRAM language features such as ''pursue'', it is good practice to use ''def-cram-function'' instead of ''defun''. The reason is that 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.
-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 continuing driving into walls. Therefore, it is in general necessary to wrap you code in an ''unwind-protect'' to stop a robot in all these cases.
-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 Language code 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. Let's try it out. Enter the following in the REPL:
-<code lisp>
-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))))
-</code>
-The turtle should now move along a rectangle.