Both sides previous revisionPrevious revisionNext revision | Previous revisionLast revisionBoth sides next revision |
tutorials:beginner:simple_plans [2016/01/22 14:26] – [Moving the turtle towards a point] gkazhoya | tutorials:beginner:simple_plans [2022/02/25 23:11] – [Implementing simple plans to move a turtle] schimpf |
---|
| |
**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: |
| <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 ===== |
| |
| |
(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 |
(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." |
<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 |
</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. 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. | 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. |
| |
| |
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]] |
| |