Towards a Domain Specific Language for a Scene Graph based Robotic World Model

Transcription

1 Towards a Domain Specific Language for a Scene Graph based Robotic World Model, Herman Bruyninckx Mechanical Engineering, KU Leuven, Belgium mech.kuleuven.be 1

2 Outline Introduction The Robot Scene Graph: RSG The RSG-DSL Conclusion 2

3 DSLs for robotic applications "knowledge" (models + configuration + functions) Task specification Platform capabilities Knowledge base World model offline online Application "processes" (runtime state) perception planning control learning r e a s o n i n g adaptation (Prior) Knowledge Robot, Task,... World Model Algorithms For various domains: planning, perception, control,... Framework for execution and communication 3

4 Existing World Models Control Kinematic chains Geometry Navigation Grids Perception 2D/3D images Features Poses Planning (Triangle) Meshes 4

5 Existing World Models Control Kinematic chains Geometry Navigation Grids Perception 2D/3D images Features Poses Planning (Triangle) Meshes Not well connected yet! 4

6 Shared world model for robot applications perception planning world model control coordination World Model: as shared (distributed) resource with dynamic scene, multi-resolution and uncertainty support 5

7 Shared world model for robot applications perception planning world model control coordination World Model: as shared (distributed) resource with dynamic scene, multi-resolution and uncertainty support Problem: Does not exist yet! Concept DSL 5

8 Contributions M3: Ecore framework M2: Xtext based DSL for a world model M1: Editor via Eclipse toolchain M0: Prior work: C++ implementation (BRICS 3D library), code generation from M2 level 6

9 The Robot Scene Graph: RSG 7

10 RSG: Robot Scene Graph perception scene graph planning control coordination 8

11 RSG: Robot Scene Graph RSG contains: Objects Low level like a point cloud High level scene objects Relations Both organized in a Directed Acyclic Graph (DAG) Example graph Scene with table Cup on table Robot observes cup GeometricNode: table mesh GeometricNode: handle mesh Transform Group: table Transform Group: cup Group: root Transform Group: robot Transform GeometricNode: cylinder 9

12 RSG: Nodes Nodes Unique IDs Attributes GeometricNodes Point Cloud Mesh Box Cylinder Groups Transforms Cache with time stamps 10

13 RSG: Function blocks scene graph function block IN OUT A function block is a generic computation hosted by the world model Consumes a part of the world model and might perform updates or add new data Inputs and outputs are based on (unique) IDs 11

14 RSG: Example ID [1] ID [2] Updates: 1 (name = roi_box_tf) ID [4] Updates: 1 (name = world_to_sensor_tf) ID [7] (name = sceneobjects) ID [946] (name = roi_box ) ID [6] (name = sensor) ID [911] Updates: 4 (name = cluster_tf) (tasktype = sceneobject) ID [952] ID [898] ID [947] (name = point_cloud) ID [948] (name = filtered_point_cloud)... Updates: 1 (name = cluster_tf) (name = cluster_bbox) (shapetype = Box) (tasktype = sceneobject) (tasktype = sceneobject) ID [953] (name = cluster_bbox)) (shapetype = Box) 12

15 The RSG-DSL 13

16 RSG-DSL Overview Primitives: Node types, function blocks Relationships: DAG structure Transformations: Model-to-text (C++) 14

17 DSL Primitives for RSG node types WorldModel Primitive AbstractNodeProperties Attribute primitives name : EString cardinality : EString attributes key : EString value : EString Geometry geometry Node AbstractNode properties child Child Box PointCloud type PointCloudType type : EString sharedptr : EString library : EString root Cylinder GeometricNode Group Transform TimeStamp value : EString HomogeneousTransformationMatrix children children transforms stamp value TransformCache history RigidTransform name : EString 15

18 DSL Primitives for RSG node types WorldModel Primitive AbstractNodeProperties Attribute primitives name : EString cardinality : EString attributes key : EString value : EString Geometry geometry Node AbstractNode properties child Child Box PointCloud type PointCloudType type : EString sharedptr : EString library : EString root Cylinder GeometricNode Group Transform TimeStamp value : EString HomogeneousTransformationMatrix children children transforms stamp value TransformCache history RigidTransform name : EString 15

19 DSL Primitives for RSG node types WorldModel Primitive AbstractNodeProperties Attribute primitives name : EString cardinality : EString attributes key : EString value : EString Geometry geometry Node AbstractNode properties child Child Box PointCloud type PointCloudType type : EString sharedptr : EString library : EString root Cylinder GeometricNode Group Transform TimeStamp value : EString HomogeneousTransformationMatrix children children transforms stamp value TransformCache history RigidTransform name : EString 15

20 DSL Primitives for RSG node types WorldModel Primitive AbstractNodeProperties Attribute primitives name : EString cardinality : EString attributes key : EString value : EString Geometry geometry Node AbstractNode properties child Child Box PointCloud type PointCloudType type : EString sharedptr : EString library : EString root Cylinder GeometricNode Group Transform TimeStamp value : EString HomogeneousTransformationMatrix children children transforms stamp value TransformCache history RigidTransform name : EString 15

21 DSL Primitives for RSG node types WorldModel Primitive AbstractNodeProperties Attribute primitives name : EString cardinality : EString attributes key : EString value : EString Geometry geometry Node AbstractNode properties child Child Box PointCloud type PointCloudType type : EString sharedptr : EString library : EString root Cylinder GeometricNode Group Transform TimeStamp value : EString HomogeneousTransformationMatrix children children transforms stamp value TransformCache history RigidTransform name : EString 15

22 Example scene setup 1 root rootnode // application scene 2 3 Group rootnode { 4 child group1 5 child worldtocamera 6 } 7 8 Group group1 { 9 Attribute ("name ", " scene_objects ") 10 child kitchentable 11 } Transform worldtocamera { 14 Attribute ("name ", " wm_to_sensor_tf ") 15 child sensor 16 transforms { 17 RigidTransform t1 { 18 stamp TimeStamp ( 0.0 s) 19 value HomogeneousTransformationMatrix ( 20 [1.0, 0.0, 0.0, 0.0 m ], 21 [0.0, 1.0, 0.0, 0.0 m ], 22 [0.0, 0.0, 1.0, 1.0 m ], 23 [0.0, 0.0, 0.0, 0.0 ]) 24 } 25 } 26 } 27 Group sensor { 28 Attribute (" name ", " sensor ") 29 } 16

23 Generated code (excerpt) 1 std :: vector <rsg :: Attribute > attributes ; // Instantiation of list of attributes. 2 unsigned int rootnodeid ; // IDs correspond to names in model on M1 level. 3 unsigned int group1id ; 4 unsigned int worldtocameraid ; 5 // [...] 6 7 /* Add group1 as a new node to the scene graph */ 8 attributes. clear (); 9 attributes. push_back ( Attribute ("name ", " scene_objects ")); 10 wm ->scene. addgroup ( rootnodeid, group1id, attributes ); // group1id = output 11 // [...] // -> returns a unique ID /* Add worldtocamera as a new node to the scene graph */ 14 attributes. clear (); 15 attributes. push_back ( Attribute ("name ", " wm_to_sensor_tf ")); 16 brics_3d :: IHomogeneousMatrix44 :: IHomogeneousMatrix44Ptr worldtocamerainitialtf ( 17 new brics_3d :: HomogeneousMatrix44 ( , 0.0, 0.0, , 1.0, 0.0, , 0.0, 1.0, * 1.0, 0.0 * 1.0, 1.0 * 1.0 // Values are scaled to SI unit [m ]. 22 )); wm ->scene. addtransformnode ( rootnodeid, worldtocameraid, attributes, 25 worldtocamerainitialtf, brics_3d :: rsg :: TimeStamp (0.0, Units :: Second ) 26 ); // Value is scaled to SI unit [s ]. 17

24 Resulting scene graph ID [1] ID [2] ID [3] (name = scene_objects) ID [14] T = (0, 0, 1) Updates: 1 (name = wm_to_sensor_tf) ID [15] (name = sensor) ID [71] (name = sensor_point_cloud) 18

25 DSL Primitives for RSG function blocks 19

26 Example function block 1 PointCloudType PointCloudPCL { 2 type "pcl :: PointCloud <PointType >" 3 sharedptr "pcl :: PointCloud <PointType >:: Ptr " 4 library " pcl " 5 } 6 7 PointCloud inputcloud type PointCloudPCL 8 PointCloud planecloud type PointCloudPCL 9 10 GeometricNode pointcloud { 11 Attribute ("name ", " point_cloud ") 12 geometry inputcloud 13 } Group planes { 16 Attribute (" name ", " planes ") 17 child tftoplanecentroid 18 } 1 GeometricNode horizontalplane { 2 Attribute (" name ", " plane ") 3 geometry planecloud 4 } 5 6 FunctionBlock horizontalplanesegmentation { 7 inputstructure pointcloud 8 inputhook sensorpointcloud 9 outputstructure planes 10 outputhook sensor 11 } 20

27 Generated code (excerpt) 1 /* Interface stub for HorizontalPlaneSegmentation function block */ 2 class HorizontalPlaneSegmentation : public rsg :: IFunctionBlock { 3 public : 4 5 /* used input IDs */ 6 unsigned int pointcloudid ; 7 8 /* used output IDs */ 9 unsigned int planesid ; 10 unsigned int cloudcentertoplanecenterid ; 11 unsigned int horizontalplaneid ; HorizontalPlaneSegmentation ( brics_3d :: WorldModel * wmhandle ) : IFunctionBlock ( wmhandle ) {}; 14 virtual ~HorizontalPlaneSegmentation () {}; virtual void configure ( brics_3d :: ParameterSet parameters ){} 17 virtual void execute () {} protected : /* data conventions */ 22 const static unsigned int horizontalplanesegmentationinputhookid = 0; 23 const static unsigned int horizontalplanesegmentationoutputhookid = 1; 21

28 Generated code (excerpt) 1 pcl :: PointCloud <PointType >:: Ptr getinputcloudpointcloud () { 2 pcl :: PointCloud <PointType >:: Ptr inputclouddata (new pcl :: PointCloud < PointType >() ); 3 4 /* Query the world model based on the input id pointcloudid */ 5 brics_3d :: rsg :: TimeStamp resulttimestamp ; 6 brics_3d :: rsg :: Shape :: ShapePtr shape ; 7 wm ->scene. getgeometry ( pointcloudid, shape, resulttimestamp ); 8 9 /* Resolve ( raw ) data type known by model */ 10 brics_3d :: rsg :: PointCloud <pcl :: PointCloud <PointType > >:: PointCloudPtr inputcloudcontainer (new brics_3d :: rsg :: PointCloud <pcl :: PointCloud < PointType > >()); 11 inputcloudcontainer = boost :: dynamic_pointer_cast <brics_3d :: rsg :: PointCloud < pcl :: PointCloud <PointType > > >( shape ); 12 inputclouddata = inputcloudcontainer -> data ; return inputclouddata ; 15 } }; 22

29 Conclusion Robot applications require models for different aspects the world model is one of them Different existing world models for various domains RSG is shared world model RSG-DSL Primitives: Nodes, function blocks Relations: DAG structure Transforms: Model-to-text (C++) Expresses Prior knowledge: scene setups, known objects Interfaces for function blocks Future works Uncertainty, multi resolution support, triggers Composition with other DSLs 23