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:intermediate:semantic_environment_map [2016/01/15 13:20] gkazhoyatutorials:intermediate:semantic_environment_map [2016/06/14 13:15] (current) – [Setting up] gkazhoya
Line 62: Line 62:
 </code> </code>
  
-There are three things we need to setup in CRAM to use semantic maps from KnowRob: the fixed frame of our TF tree, the configuration of our location costmap and a new ROS node:+To use semantic maps from KnowRob in CRAM we first need to set our ''*fixed-frame*''configure the location costmap and start a new ROS node:
  
 <code lisp> <code lisp>
-SEMANTIC-MAP-COSTMAP> (setf cram-transforms-stamped:*fixed-frame* "map")+SEMANTIC-MAP-COSTMAP> (setf cram-tf:*fixed-frame* "map")
 SEMANTIC-MAP-COSTMAP>  SEMANTIC-MAP-COSTMAP> 
 (prolog:def-fact-group costmap-metadata () (prolog:def-fact-group costmap-metadata ()
Line 102: Line 102:
  
 If you inspect a ''semantic-map-geom'' you can see the slots of that object, e.g. the ''"kitchen_island"'' looks like this: If you inspect a ''semantic-map-geom'' you can see the slots of that object, e.g. the ''"kitchen_island"'' looks like this:
-<code> +{{ :tutorials:intermediate:kitchen_island_geom.png?direct&1000 |}}
-All Slots: +
-[ ]  ALIASES    = NIL +
-[ ]  DIMENSIONS = #<CL-TRANSFORMS:3D-VECTOR (0.800000011920929d0 2.450000047683716d0 0.8500000238418579d0)> +
-[ ]  NAME       = "kitchen_island" +
-[ ]  OWL-NAME   = "http://knowrob.org/kb/knowrob.owl#kitchen_island" +
-[ ]  PARENT     = NIL +
-[ ]  POSE       = #<CL-TRANSFORMS:POSE  +
-   #<3D-VECTOR (-1.0528899431228638d0 1.6562440395355225d0 0.42500001192092896d0)> +
-   #<QUATERNION (0.0d0 0.0d0 1.0d0 0.0d0)>> +
-[ ]  SUB-PARTS  = (#<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {1005529EC3}> #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {10055A58F3}> #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {100560AC53}> #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {100566E7D3}> #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {10056D2CD3}> #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {1005736723}>) +
-[ ]  TYPE       = "Cupboard" +
-[ ]  URDF-NAME  = #<unbound> +
-</code>+
  
-Most of the classes and utility functions are defined in the ''cram_semantic_map_utils'' package, mostly in the ''semantic-map.lisp'' file: to get a list of all the parts use ''semantic-map-parts'', for a specific part use ''semantic-map-part'', you can get the pose and dimensions of the part using ''pose'' and ''dimensions'' functions, etc., see the examples below:+Most of the classes and utility functions are defined in the ''cram_semantic_map_utils'' package, mostly in the ''semantic-map.lisp'' file: to get a list of all the parts use ''semantic-map-parts'', for a specific part use ''semantic-map-part'', you can get the pose and dimensions of the part using ''pose'' and ''dimensions'' functions, if your semantic map contains joints there are functions to access them as well, etc., see the examples below:
  
 <code lisp> <code lisp>
Line 130: Line 117:
 #<CL-TRANSFORMS:3D-VECTOR (0.800000011920929d0 2.450000047683716d0 0.8500000238418579d0)> #<CL-TRANSFORMS:3D-VECTOR (0.800000011920929d0 2.450000047683716d0 0.8500000238418579d0)>
 </code> </code>
 +
 +
 +===== Designators =====
 +
 +Now let's create a symbolic object description to correspond to a part of the semantic map.
 +We cannot resolve object designators using the ''reference'' function currently, as the object designator resolving mechanism is a big flimsy right now - only perception can resolve object designators - so we will use an explicit function ''designator->semantic-map-objects'' to do that. Note, that you can have a location designator with the same property and it will work just as well:
 +
 +<code lisp>
 +SEMANTIC-MAP-COSTMAP> 
 +(cram-semantic-map-designators:designator->semantic-map-objects 
 + (cram-designators:make-designator :object '((:name "kitchen_sink_block"))))
 +(#<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {100FB08C23}>)
 +SEMANTIC-MAP-COSTMAP> 
 +(cram-semantic-map-designators:designator->semantic-map-objects 
 + (cram-designators:make-designator :location '((:name "kitchen_island"))))
 +(#<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {100FE7E4A3}>)
 +</code>
 +
 +We can also ask for a location of an semantic map part:
 +<code lisp>
 +SEMANTIC-MAP-COSTMAP> (make-designator :location `((:of
 +                                                    ,(make-designator :object '((:name "kitchen_island"))))))
 +#<LOCATION-DESIGNATOR ((:OF
 +                        #<OBJECT-DESIGNATOR ((:NAME "kitchen_island"))
 +                          {100E276F63}>)) {100E37ABA3}>
 +                          
 +SEMANTIC-MAP-COSTMAP> (reference *)
 +#<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
 +   FRAME-ID: "map", STAMP: 0.0
 +   #<3D-VECTOR (-1.0528899431228638d0 1.6562440395355225d0 0.42500001192092896d0)>
 +   #<QUATERNION (0.0d0 0.0d0 1.0d0 0.0d0)>>
 +</code>
 +
 +A location description can be ambiguous, e.g., if we would like to have a location of an object of a certain semantic map part type:
 +<code lisp>
 +SEMANTIC-MAP-COSTMAP> 
 +(let* ((object-of-type-cupboard (make-designator :object '((:type "Cupboard"))))
 +       (location-of-cupboard (make-designator :location `((:of ,object-of-type-cupboard)))))
 +  (format t "one solution: ~a~%" (reference location-of-cupboard))
 +  (format t "another solution: ~a~%" (reference (next-solution location-of-cupboard))))
 +one solution: #<POSE-STAMPED 
 +   FRAME-ID: "map", STAMP: 0.0
 +   #<3D-VECTOR (-1.0528899431228638d0 -0.44999998807907104d0 0.36000001430511475d0)>
 +   #<QUATERNION (0.0d0 0.0d0 1.0d0 0.0d0)>>
 +another solution: #<POSE-STAMPED 
 +   FRAME-ID: "map", STAMP: 0.0
 +   #<3D-VECTOR (1.5159399509429932d0 0.30313000082969666d0 0.42500001192092896d0)>
 +   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
 +</code>
 +There are multiple objects of type cupboard, so we get multiple solutions.
 +
 +In the example semantic map '':type'' can be ''"Cupboard"'', ''"Drawer"'', ''"Refrigerator"'', etc., see the semantic map object for more detail.
 +
 +Now, let's use the location costmap mechanism to ground locations on or in objects:
 +<code lisp>
 +SEMANTIC-MAP-COSTMAP> (reference (make-designator :location '((:in "Refrigerator"))))
 +#<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
 +   FRAME-ID: "map", STAMP: 0.0
 +   #<3D-VECTOR (1.3000001907348633d0 -0.9499998092651367d0 0.53999999538064d0)>
 +   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
 +</code>
 +{{ :tutorials:intermediate:in-fridge.png?direct&600 |}}
 +The big red dot is the location sampled for this designator.
 +
 +If we want a location on a specific object, not on all objects of a certain type, we can specify that using the '':name'' key:
 +<code lisp>
 +SEMANTIC-MAP-COSTMAP> (reference (make-designator :location '((:on "Cupboard")
 +                                                              (:name "kitchen_island"))))
 +#<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
 +   FRAME-ID: "map", STAMP: 0.0
 +   #<3D-VECTOR (-0.6999998092651367d0 1.0500001907348633d0 0.8500000238418579d0)>
 +   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
 +</code>
 +{{ :tutorials:intermediate:location_on_kitchen_island.png?direct&1000 |}}
 +The marker array in RViz show a uniform distribution of all possible locations that satisfy the symbolic constraint in the designator.