Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision
Previous revision
tutorials:beginner:simple_plans [2016/01/22 14:25] – [Moving the turtle towards a point] gkazhoyatutorials:beginner:simple_plans [2022/02/25 23:15] (current) – [Implementing simple plans to move a turtle] schimpf
Line 4: Line 4:
  
 **Previous Tutorial:** [[tutorials:beginner:controlling_turtlesim_2|Controlling turtlesim from CRAM]]\\ **Previous Tutorial:** [[tutorials:beginner:controlling_turtlesim_2|Controlling turtlesim from CRAM]]\\
-**Next Tutorial:** [[tutorials:beginner:designators|Creating action designators for the turtlesim]]+**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.  
 +<code bash> 
 +$ roscore 
 +</code> 
 +<code bash> 
 +$ rosrun turtlesim turtlesim_node 
 +</code> 
 + 
 +And in the REPL the following commands need to be executed: 
 +<code lisp> 
 +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"
 +</code>
 ===== Moving the turtle towards a point ===== ===== Moving the turtle towards a point =====
  
Line 28: Line 45:
  
 (defun relative-angle-to (goal pose-msg) (defun relative-angle-to (goal pose-msg)
-  "Given a `pose-msg' as a geometry_msgs/Pose and a `goal' as cl-transforms:3d-vector,+  "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." calculate the angle by which the pose has to be turned to point toward the goal."
   (let ((diff-pose (cl-transforms:transform-point   (let ((diff-pose (cl-transforms:transform-point
Line 38: Line 55:
       (cl-transforms:x diff-pose))))       (cl-transforms:x diff-pose))))
  
-(defun calculate-angular-cmd (goal &optional (ang-vel-factor 4))+(defun calculate-angular-cmd (goal &optional (ang-vel-factor 8))
   "Uses the current turtle pose and calculates the angular velocity command   "Uses the current turtle pose and calculates the angular velocity command
 to turn towards the goal." to turn towards the goal."
Line 55: Line 72:
 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. 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&800 |}}+{{ :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: 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:
Line 80: Line 97:
 <code lisp> <code lisp>
 (def-cram-function move-to (goal &optional (distance-threshold 0.1)) (def-cram-function move-to (goal &optional (distance-threshold 0.1))
-  (let ((reached-fl (< (fl-funcall #'cl-transforms:v-dist+  "Sends velocity commands until `goal' is reached." 
 + (let ((reached-fl (< (fl-funcall #'cl-transforms:v-dist
                                    (fl-funcall                                    (fl-funcall
                                     #'cl-transforms:translation                                     #'cl-transforms:translation
Line 99: Line 117:
 </code> </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.+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 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'' 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. Thereforewe 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. 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:+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:
 <code lisp> <code lisp>
 TUT> (top-level TUT> (top-level
        (dolist (goal '((9 1 0) (9 9 0) (1 9 0) (1 1 0) (9 1 0)))        (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))))          (move-to (apply #'cl-transforms:make-3d-vector goal))))
-</code>         +</code> 
 + 
 +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. The turtle should now move along a rectangle.
Line 120: Line 140:
  
  
-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 ...+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:designators|Creating designators for the turtlesim]]+[[tutorials:beginner:cram_prolog|Using Prolog for reasoning]]