Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revisionLast revisionBoth sides next revision | ||
tutorials:intermediate:costmaps [2019/03/07 11:51] – [Visualization of costmaps] gkazhoya | tutorials:intermediate:costmaps [2022/04/27 13:55] – [Setup] schimpf | ||
---|---|---|---|
Line 3: | Line 3: | ||
This tutorial demonstrates the location costmap library of CRAM, including its API, examples of existing costmaps and how to write your own new costmaps. | This tutorial demonstrates the location costmap library of CRAM, including its API, examples of existing costmaps and how to write your own new costmaps. | ||
+ | ===== Introduction ===== | ||
+ | If you recall, the resolution of a location designator is divided into two steps - a generation step and a verification step. The generators yield sequences of solution candidates in the form of lazy lists and verification functions accept or reject solutions. The constraints specified with designators normally restrict the solution space but still leave many potentially valid solutions. The process of resolving location designator properties and generating poses in space is split up into three parts in order to deal with the computational complexity of the six-dimensional space of poses and to make a sampling-based approach feasible. Informally, the generation of a pose can be resolved as follows: | ||
+ | - Generate a two-dimensional grid based on location designator constraints with values greater than zero for all potentially valid solutions. It is a map representing the solution density of a specific cell where greater values represent more solutions for a specific constraint. | ||
+ | - Use the grid as a probability density function to generate random samples. | ||
+ | - Use Heuristics to resolve the orientation and Z coordinate of the generated pose. | ||
+ | - Use the physics-based reasoning system to prove that the solution is valid. | ||
+ | As can be seen, the first three steps generate a solution candidate while the last step verifies it. | ||
+ | |||
+ | Costmaps are density maps of two-dimensional matrices of positive real numbers which allow for representing spacial constraints such as '' | ||
+ | |||
+ | For further reading, refer to section 4 of [[https:// | ||
===== Setup ===== | ===== Setup ===== | ||
+ | *set up the environment in our terminal by calling the launch file: | ||
+ | |||
+ | roslaunch cram_bullet_world_tutorial world.launch | ||
- | | + | |
- | | + | |
- | * start a node in REPL with (roslisp-utilities: | ||
* define costmap parameters: | * define costmap parameters: | ||
Line 16: | Line 29: | ||
<code lisp> | <code lisp> | ||
(prolog: | (prolog: | ||
- | (prolog:< | + | (prolog:< |
- | (prolog:< | + | (prolog:< |
- | (prolog:< | + | (prolog:< |
- | (prolog:< | + | (prolog:< |
</ | </ | ||
Line 92: | Line 105: | ||
(abs (/ (cl-transforms: | (abs (/ (cl-transforms: | ||
| | ||
- | </ | + | </ |
* define order for your costmap function and give it a name: | * define order for your costmap function and give it a name: | ||
Line 98: | Line 111: | ||
<code lisp> | <code lisp> | ||
(defmethod location-costmap: | (defmethod location-costmap: | ||
- | </ | + | </ |
* define the prolog rule for generating costmaps: | * define the prolog rule for generating costmaps: | ||
Line 114: | Line 127: | ||
(make-behind-cost-function ?ref-x ?ref-y) | (make-behind-cost-function ?ref-x ?ref-y) | ||
?costmap))) | ?costmap))) | ||
- | </ | + | </ |
* resolve a designator with your new awesome costmap: | * resolve a designator with your new awesome costmap: | ||
Line 132: | Line 145: | ||
(car (prolog: | (car (prolog: | ||
</ | </ | ||
+ | The code can also be used to visualize the other costmap functions in this tutorial. | ||
- | {{: | + | {{: |
===== Using other (pre-defined) cost-functions ===== | ===== Using other (pre-defined) cost-functions ===== | ||
In the location-costmap package there are other cost function already available for generating costmaps. Here are two examples: | In the location-costmap package there are other cost function already available for generating costmaps. Here are two examples: | ||
Line 153: | Line 166: | ||
</ | </ | ||
+ | The visualized costmap looks like this: | ||
+ | |||
+ | {{: | ||
* using range costmap function: | * using range costmap function: | ||
Line 164: | Line 180: | ||
| | ||
? | ? | ||
+ | </ | ||
+ | |||
+ | The visualized costmap looks like this: | ||
+ | |||
+ | {{: | ||
+ | * using range costmap function inverted: | ||
+ | <code lisp> | ||
(prolog: | (prolog: | ||
(prolog:< | (prolog:< | ||
Line 174: | Line 197: | ||
</ | </ | ||
+ | The visualized costmap looks like this: | ||
+ | |||
+ | {{: | ||
+ | ===== Using your own Z coordinate function ===== | ||
+ | So far the costmaps that you have generated/ | ||
+ | |||
+ | ==== 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: | ||
+ | (prolog:< | ||
+ | (desig: | ||
+ | (location-costmap: | ||
+ | (location-costmap: | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | Notice that the only thing that we added is this particular line: | ||
+ | <code lisp> | ||
+ | (costmap: | ||
+ | (costmap: | ||
+ | ?costmap) | ||
+ | </ | ||
+ | The end result is that all the poses generated will now have a z-coordinate of 1.0 units. | ||
+ | |||
+ | The visualized costmap using the bullet world looks like this: | ||
+ | |||
+ | {{: | ||
+ | |||
+ | ==== Configuring height according to conditions ==== | ||
+ | In a real scenario, not all the z coordinate would be constant, due to terrain differences, | ||
+ | <code lisp> | ||
+ | (defun positive-y-height-generator () | ||
+ | (lambda (x y) | ||
+ | (if (> y 0.0) | ||
+ | '(1.0) ' | ||
+ | </ | ||
+ | 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: | ||
+ | (prolog:< | ||
+ | (desig: | ||
+ | (location-costmap: | ||
+ | (location-costmap: | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | Note that we only replaced the constant height generator with our own function. | ||
+ | |||
+ | ===== Using your own Orientation Generator ===== | ||
+ | As mentioned in the introduction, | ||
+ | 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 | ||
+ | | ||
+ | | ||
+ | (cl-transforms: | ||
+ | angle)) | ||
+ | `(0.0 ,(/ pi 2) ,pi ,(- (/ pi 2)))))) | ||
+ | </ | ||
+ | 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: | ||
+ | (prolog:< | ||
+ | (desig: | ||
+ | (location-costmap: | ||
+ | (location-costmap: | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | |||
+ | The visualized costmap using the bullet world looks like this: | ||
+ | |||
+ | {{: |