Differences

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

--- tutorials:intermediate:costmaps [2019/04/02 12:03] – amar
+++ tutorials:intermediate:costmaps [2019/06/13 11:01] – Added section about height and orientation generators amar
@@ Line 185: / Line 185: @@
 </code>
+===== Using your own Z coordinate function =====
+So far the costmaps that you have generated/used is set on the 2D XY plane and the Z is taken to be 0 by default. Height generators can also be defined along with the cost functions which determines the z coordinate of the sampled pose from the costmap. There are a couple of ways you can define a height generator for your costmap:
+==== Consistent Height ====
+With this, you can fix the z coordinate to one constant value for all the poses generated. CRAM provides you the method to do this easily. Let's take the example we used above and modify it a little bit.
+<code lisp>
+(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
+  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
+    (desig:desig-prop ?designator (:behind ?pose))
+    (location-costmap:costmap ?costmap)
+    (location-costmap:costmap-add-function
+     behind-cost-function
+     (location-costmap:make-range-cost-function ?pose 1.0)
+     ?costmap)
+    (costmap:costmap-add-cached-height-generator
+     (costmap:make-constant-height-function 1.0)
+     ?costmap)))
+</code>
+Notice that the only thing that we added is this particular line:
+<code lisp>
+(costmap:costmap-add-height-generator
+  (costmap:make-constant-height-function 1.0)
+  ?costmap)
+</code>
+The end result is that all the poses generated will now have a z-coordinate of 1.0 units.
+==== Configuring height according to conditions ====
+In a real scenario, not all the z coordinate would be constant, due to terrain differences, obstacles, etc. CRAM allows you to model this similar to how you created your own custom costmap. For this, let's define our own height-generator, as shown below:
+<code lisp>
+(defun positive-y-height-generator ()
+  (lambda (x y)
+    (if (> y 0.0)
+        '(1.0) '(0.0))))
+</code>
+As before, the lambda function will get both x and y as inputs but this time we are expected to return a list of possible heights and a random value out of the satisfied condition list is taken.
+In this example, if y is greater than 0, then the height generator returns 1, else 0 otherwise.
+To use it in our designator, we redefine our fact group again:
+<code lisp>
+(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
+  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
+    (desig:desig-prop ?designator (:behind ?pose))
+    (location-costmap:costmap ?costmap)
+    (location-costmap:costmap-add-function
+     behind-cost-function
+     (location-costmap:make-range-cost-function ?pose 1.0)
+     ?costmap)
+    (costmap:costmap-add-height-generator
+     (positive-y-height-generator)
+     ?costmap)))
+</code>
+Note that we only replaced the constant height generator with our own function.
+===== Using your own Orientation Generator =====
+As mentioned in the introduction, the resolution of the designator can also involve resolving an orientation in addition to resolving height and the pose from the costmap. In this section, we will see how we can employ orientation generators to give various poses when resolving a location costmap. This works similar to how we defined and used the height generator, with some minor changes. By default, if no custom orientation generators are used, the system will use it's own identity orientation generator where all the poses will have identity rotation aligned to the reference axis.
+Let's jump into defining and using our own version of an orientation generator:
+<code lisp>
+(defun make-4-orientations-generator ()
+  (lambda (x y previous-orientations)
+    (declare (ignore x y previous-orientations))
+    (mapcar
+     (lambda (angle)
+       (cl-transforms:axis-angle->quaternion
+        (cl-transforms:make-3d-vector 0 0 1)
+        angle))
+     `(0.0 ,(/ pi 2) ,pi ,(- (/ pi 2))))))
+</code>
+Note: Here we will receive a parameter called previous-orientations in addition to x and y, and this parameter can be used to incrementally modify the orientation that are generated. In this particular example, we are not using these parameters, but just creating 4 possible orientations (0, pi/2, pi and -pi/2) regardless of the previous orientation or the current position. The user can make use of these parameters according to their own use-case.
+And finally adding it to the fact-group
+<code lisp>
+(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
+  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
+    (desig:desig-prop ?designator (:behind ?pose))
+    (location-costmap:costmap ?costmap)
+    (location-costmap:costmap-add-function
+     behind-cost-function
+     (location-costmap:make-range-cost-function ?pose 1.0)
+     ?costmap)
+    (costmap:costmap-add-height-generator
+     (positive-y-height-generator)
+     ?costmap)
+    (costmap:costmap-add-orientation-generator
+     (make-4-orientations-generator)
+     ?costmap)))
+</code>