$search
00001 package edu.tum.cs.ias.knowrob.utils.owl; 00002 00003 import java.io.*; 00004 import java.util.ArrayList; 00005 import java.util.HashMap; 00006 import java.util.Map; 00007 import java.util.Set; 00008 00009 import javax.vecmath.Matrix4d; 00010 import javax.vecmath.Vector3d; 00011 00012 import org.semanticweb.owlapi.apibinding.OWLManager; 00013 import org.semanticweb.owlapi.io.RDFXMLOntologyFormat; 00014 import org.semanticweb.owlapi.model.*; 00015 import org.semanticweb.owlapi.util.DefaultPrefixManager; 00016 import org.semanticweb.owlapi.reasoner.*; 00017 import org.semanticweb.owlapi.reasoner.structural.*; 00018 00019 import edu.tum.cs.ias.knowrob.utils.ros.RosUtilities; 00020 00021 00022 00034 public class OWLImportExport { 00035 00036 00038 // Set IRIs for the ontologies used here 00039 // 00040 00041 // Base IRI for KnowRob ontology 00042 public final static String KNOWROB = "http://ias.cs.tum.edu/kb/knowrob.owl#"; 00043 00044 // Base IRI for OWL ontology 00045 public final static String OWL = "http://www.w3.org/2002/07/owl#"; 00046 00047 // Base IRI for RDFS 00048 public final static String RDFS = "http://www.w3.org/2000/01/rdf-schema#"; 00049 00050 // Base IRI for semantic map ontology 00051 public final static String IAS_MAP = "http://ias.cs.tum.edu/kb/ias_semantic_map.owl#"; 00052 00053 // ROS package name for KnowRob 00054 public final static String KNOWROB_PKG = "ias_knowledge_base"; 00055 00056 // OWL file of the KnowRob ontology (relative to KNOWROB_PKG) 00057 public final static String KNOWROB_OWL = "owl/knowrob.owl"; 00058 00059 // Prefix manager 00060 public final static DefaultPrefixManager PREFIX_MANAGER = new DefaultPrefixManager(KNOWROB); 00061 static { 00062 PREFIX_MANAGER.setPrefix("knowrob:", KNOWROB); 00063 PREFIX_MANAGER.setPrefix("owl:", OWL); 00064 PREFIX_MANAGER.setPrefix("rdfs:", RDFS); 00065 } 00066 00067 // mapping ROS-KnowRob identifiers 00068 protected static final HashMap<String, String> rosToKnowrob = new HashMap<String, String>(); 00069 00070 OWLDataFactory factory; 00071 OWLOntologyManager manager; 00072 DefaultPrefixManager pm; 00073 00074 int inst_counter=0; // counter to create unique instance identifiers 00075 00076 public OWLImportExport() { 00077 //readKnowRobObjectClasses(); 00078 } 00079 00080 00088 public OWLOntology createOWLMapDescription(String map_id, ArrayList<MapObject> map) { 00089 00090 OWLOntology ontology = null; 00091 HashMap<String, OWLNamedIndividual> idToInst = new HashMap<String, OWLNamedIndividual>(); 00092 00093 try { 00094 00095 // Create ontology manager and data factory 00096 manager = OWLManager.createOWLOntologyManager(); 00097 factory = manager.getOWLDataFactory(); 00098 00099 // Get prefix manager using the base IRI of the JoystickDrive ontology as default namespace 00100 pm = PREFIX_MANAGER; 00101 00102 // Create empty OWL ontology 00103 ontology = manager.createOntology(IRI.create(map_id)); 00104 PREFIX_MANAGER.setPrefix("map:", map_id); 00105 manager.setOntologyFormat(ontology, new RDFXMLOntologyFormat()); 00106 00107 // Import KnowRob ontology 00108 OWLImportsDeclaration oid = factory.getOWLImportsDeclaration(IRI.create(KNOWROB)); 00109 AddImport addImp = new AddImport(ontology,oid); 00110 manager.applyChange(addImp); 00111 00112 00113 // create SemanticMap object in the ontology 00114 idToInst.put(map_id, createSemMapInst(ontology)); 00115 00116 // create time point 00117 OWLNamedIndividual time_inst = createTimePointInst(ros.communication.Time.now(), ontology); 00118 00119 00120 // iterate over all objects and create the respective OWL representations 00121 for(MapObject map_obj : map) { 00122 idToInst.put(map_obj.id, createSemMapObjectDescription(map_obj, time_inst, ontology)); 00123 } 00124 00125 00126 // link to parent objects (second loop to avoid problems due to wrong ordering) 00127 for(MapObject map_obj : map) { 00128 00129 OWLNamedIndividual obj_inst = idToInst.get(map_obj.id); 00130 00131 // link high-level objects to the map 00132 if(map_obj.types.contains("Cupboard") || 00133 map_obj.types.contains("Drawer") || 00134 map_obj.types.contains("Oven") || 00135 map_obj.types.contains("Refrigerator") || 00136 map_obj.types.contains("Dishwasher") || 00137 map_obj.types.contains("Table") || 00138 map_obj.types.contains("CounterTop") || 00139 map_obj.types.contains("Cabinet-PieceOfFurniture") || 00140 map_obj.types.contains("Bed-PieceOfFurniture") || 00141 map_obj.types.contains("Sink")) { 00142 00143 // top-level object, link to map 00144 OWLObjectProperty describedInMap = factory.getOWLObjectProperty("knowrob:describedInMap", pm); 00145 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(describedInMap, obj_inst, idToInst.get(map_id))); 00146 } 00147 00148 00149 // link proper physical parts of an object 00150 for(MapObject p: map_obj.physicalParts) { 00151 00152 OWLIndividual part = idToInst.get(p.id); 00153 OWLObjectProperty properPhysicalParts = factory.getOWLObjectProperty("knowrob:properPhysicalParts", pm); 00154 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(properPhysicalParts, obj_inst, part)); 00155 } 00156 00157 00158 // link hinges in a special way (set child, parent, hingedTo) 00159 if(map_obj instanceof MapJoint) { 00160 00161 OWLIndividual child = idToInst.get(((MapJoint) map_obj).child.id); 00162 OWLIndividual parent = idToInst.get(((MapJoint) map_obj).parent.id); 00163 00164 // set joint connection between parent and child 00165 if(map_obj.types.contains("HingedJoint") && child!= null && parent!=null) { 00166 OWLObjectProperty hingedTo = factory.getOWLObjectProperty("knowrob:hingedTo", pm); 00167 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(hingedTo, parent, child)); 00168 00169 } else if(map_obj.types.contains("PrismaticJoint")) { 00170 OWLObjectProperty prismaticallyConnectedTo = factory.getOWLObjectProperty("knowrob:prismaticallyConnectedTo", pm); 00171 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(prismaticallyConnectedTo, parent, child)); 00172 } 00173 00174 // set rigid connection between joint and parent/child resp. 00175 OWLObjectProperty connectedTo = factory.getOWLObjectProperty("knowrob:connectedTo-Rigidly", pm); 00176 if(child!= null) { 00177 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(connectedTo, obj_inst, child)); 00178 } 00179 00180 if(parent!=null) { 00181 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(connectedTo, obj_inst, parent)); 00182 } 00183 } 00184 } 00185 00186 } catch (Exception e) { 00187 ontology = null; 00188 e.printStackTrace(); 00189 } 00190 00191 return ontology; 00192 } 00193 00194 00195 00205 public OWLNamedIndividual createSemMapObjectDescription(MapObject map_obj, OWLNamedIndividual timestamp, OWLOntology ontology) { 00206 00207 // create object instance 00208 OWLNamedIndividual obj_inst = createObjectInst(map_obj, ontology); 00209 00210 // create pose matrix instance 00211 OWLNamedIndividual pose_inst = createPoseInst(map_obj.pose_matrix, ontology); 00212 00213 // create perception instance 00214 createPerceptionInst("knowrob:SemanticMapPerception", obj_inst, pose_inst, timestamp, ontology); 00215 00216 return obj_inst; 00217 } 00218 00219 00220 00227 public OWLNamedIndividual createSemMapInst(OWLOntology ontology) { 00228 00229 OWLClass sem_map_class = factory.getOWLClass("knowrob:SemanticEnvironmentMap", pm); 00230 OWLNamedIndividual sem_map_inst = factory.getOWLNamedIndividual( 00231 instForClass("map:SemanticEnvironmentMap"), pm); 00232 manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(sem_map_class, sem_map_inst)); 00233 00234 return sem_map_inst; 00235 } 00236 00237 00238 00246 public OWLNamedIndividual createObjectInst(MapObject map_obj, OWLOntology ontology) { 00247 00248 OWLNamedIndividual obj_inst = factory.getOWLNamedIndividual("knowrob:"+map_obj.id, pm); 00249 00250 for(String t : map_obj.types) { 00251 OWLClass obj_class = factory.getOWLClass("knowrob:"+t, pm); 00252 manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(obj_class, obj_inst)); 00253 } 00254 00255 // set object dimensions 00256 OWLDataProperty width = factory.getOWLDataProperty("knowrob:widthOfObject", pm); 00257 OWLDataProperty depth = factory.getOWLDataProperty("knowrob:depthOfObject", pm); 00258 OWLDataProperty height = factory.getOWLDataProperty("knowrob:heightOfObject", pm); 00259 00260 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(depth, obj_inst, map_obj.dimensions.x)); 00261 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(width, obj_inst, map_obj.dimensions.y)); 00262 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(height, obj_inst, map_obj.dimensions.z)); 00263 00264 00265 // create hinge-specific properties 00266 if(map_obj instanceof MapJoint) { 00267 00268 OWLDataProperty minJointValue = factory.getOWLDataProperty("knowrob:minJointValue", pm); 00269 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(minJointValue, obj_inst, ((MapJoint) map_obj).q_min)); 00270 00271 OWLDataProperty maxJointValue = factory.getOWLDataProperty("knowrob:maxJointValue", pm); 00272 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(maxJointValue, obj_inst, ((MapJoint) map_obj).q_max)); 00273 00274 OWLDataProperty turnRadius = factory.getOWLDataProperty("knowrob:turnRadius", pm); 00275 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(turnRadius, obj_inst, ((MapJoint) map_obj).radius)); 00276 00277 // set direction for prismatic joints 00278 if(map_obj.types.contains("PrismaticJoint")) { 00279 00280 OWLNamedIndividual dir_vec = createDirVector(((MapJoint) map_obj).direction, ontology); 00281 00282 OWLObjectProperty direction = factory.getOWLObjectProperty("knowrob:direction", pm); 00283 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(direction, obj_inst, dir_vec)); 00284 } 00285 } 00286 00287 return obj_inst; 00288 } 00289 00290 00291 00299 public OWLNamedIndividual createTimePointInst(ros.communication.Time stamp, OWLOntology ontology) { 00300 00301 OWLNamedIndividual time_inst = factory.getOWLNamedIndividual("map:timepoint_"+stamp.secs, pm); 00302 OWLClass time_class = factory.getOWLClass("knowrob:TimePoint", pm); 00303 manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(time_class, time_inst)); 00304 00305 return time_inst; 00306 } 00307 00308 00309 00317 public OWLNamedIndividual createPoseInst(Matrix4d pose, OWLOntology ontology) { 00318 00319 // create pose matrix instance 00320 OWLClass pose_class = factory.getOWLClass("knowrob:RotationMatrix3D", pm); 00321 OWLNamedIndividual pose_inst = factory.getOWLNamedIndividual( 00322 instForClass("knowrob:RotationMatrix3D"), pm); 00323 manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(pose_class, pose_inst)); 00324 00325 // set pose properties 00326 for(int i=0;i<4;i++) { 00327 for(int j=0;j<4;j++) { 00328 OWLDataProperty prop = factory.getOWLDataProperty("knowrob:m"+i+j, pm); 00329 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(prop, pose_inst, pose.getElement(i,j))); 00330 } 00331 } 00332 00333 return pose_inst; 00334 } 00335 00343 public OWLNamedIndividual createDirVector(Vector3d dir_vec, OWLOntology ontology) { 00344 00345 // create pose matrix instance 00346 OWLClass vec_class = factory.getOWLClass("knowrob:Vector", pm); 00347 OWLNamedIndividual vec_inst = factory.getOWLNamedIndividual(instForClass("knowrob:Vector"), pm); 00348 manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(vec_class, vec_inst)); 00349 00350 // set vector dimensions 00351 OWLDataProperty vecX = factory.getOWLDataProperty("knowrob:vectorX", pm); 00352 OWLDataProperty vecY = factory.getOWLDataProperty("knowrob:vectorY", pm); 00353 OWLDataProperty vecZ = factory.getOWLDataProperty("knowrob:vectorZ", pm); 00354 00355 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(vecX, vec_inst, dir_vec.x)); 00356 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(vecY, vec_inst, dir_vec.y)); 00357 manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(vecZ, vec_inst, dir_vec.z)); 00358 00359 return vec_inst; 00360 } 00361 00372 public OWLNamedIndividual createPerceptionInst(String type, OWLNamedIndividual obj_inst, OWLNamedIndividual pose_inst, OWLNamedIndividual timestamp, OWLOntology ontology) { 00373 00374 // create perception instance 00375 OWLClass perc_class = factory.getOWLClass(type, pm); 00376 OWLNamedIndividual perc_inst = factory.getOWLNamedIndividual( 00377 instForClass(type), pm); 00378 manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(perc_class, perc_inst)); 00379 00380 // link to the object instance and the pose instance 00381 OWLObjectProperty objectActedOn = factory.getOWLObjectProperty("knowrob:objectActedOn", pm); 00382 OWLObjectProperty eventOccursAt = factory.getOWLObjectProperty("knowrob:eventOccursAt", pm); 00383 00384 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(objectActedOn, perc_inst, obj_inst)); 00385 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(eventOccursAt, perc_inst, pose_inst)); 00386 00387 // set time stamp 00388 OWLObjectProperty startTime = factory.getOWLObjectProperty("knowrob:startTime", pm); 00389 manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(startTime, perc_inst, timestamp)); 00390 00391 return perc_inst; 00392 } 00393 00394 00395 00403 static public HashMap<String, MapObject> readMapObjectFromOWL(String filename) { 00404 00405 HashMap<String, MapObject> objects = new HashMap<String, MapObject>(); 00406 OWLOntology ont = null; 00407 try { 00408 00409 OWLOntologyManager manager = OWLManager.createOWLOntologyManager(); 00410 OWLDataFactory factory = manager.getOWLDataFactory(); 00411 DefaultPrefixManager pm = OWLImportExport.PREFIX_MANAGER; 00412 00413 ont = OWLFileUtils.loadOntologyFromFile(filename); 00414 00415 if(ont!=null) { 00416 00417 // iterate over objects, add to objects hashmap 00418 for(OWLNamedIndividual instances : ont.getIndividualsInSignature()) { 00419 00420 Set<OWLClassExpression> types = instances.getTypes(ont); 00421 OWLClass semanticMapPerception = factory.getOWLClass("knowrob:SemanticMapPerception", pm); 00422 00423 if(types.contains(semanticMapPerception)) { 00424 00425 Map<OWLObjectPropertyExpression, Set<OWLIndividual>> perc_props = 00426 instances.getObjectPropertyValues(ont); 00427 00428 OWLObjectProperty objectActedOn = factory.getOWLObjectProperty("knowrob:objectActedOn", pm); 00429 OWLObjectProperty eventOccursAt = factory.getOWLObjectProperty("knowrob:eventOccursAt", pm); 00430 00431 Set<OWLIndividual> objs = perc_props.get(objectActedOn); 00432 Set<OWLIndividual> poses = perc_props.get(eventOccursAt); 00433 00434 00435 // assuming there is only one object and one pose per perception: 00436 for(OWLIndividual obj : objs) { 00437 for(OWLIndividual pose : poses) { 00438 00439 // create map object 00440 MapObject cur = new MapObject(); 00441 00442 // get types 00443 for(OWLClassExpression c: obj.getTypes(ont)) { 00444 cur.types.add(c.asOWLClass().toStringID().split("#")[1]); 00445 } 00446 00447 // special treatment for MapJoints 00448 if(cur.types.contains("HingedJoint") || cur.types.contains("PrismaticJoint")) { 00449 cur = new MapJoint(); 00450 for(OWLClassExpression c: obj.getTypes(ont)) { 00451 cur.types.add(c.asOWLClass().toStringID().split("#")[1]); 00452 } 00453 } 00454 00455 cur.id = obj.toStringID().split("#")[1]; 00456 00457 00458 if(cur.types.contains("SemanticEnvironmentMap")) 00459 continue; 00460 00461 // get dimensions 00462 Map<OWLDataPropertyExpression, Set<OWLLiteral>> data_props = 00463 obj.getDataPropertyValues(ont); 00464 Map<OWLObjectPropertyExpression, Set<OWLIndividual>> obj_props = 00465 obj.getObjectPropertyValues(ont); 00466 00467 OWLDataProperty width = factory.getOWLDataProperty("knowrob:widthOfObject", pm); 00468 OWLDataProperty depth = factory.getOWLDataProperty("knowrob:depthOfObject", pm); 00469 OWLDataProperty height = factory.getOWLDataProperty("knowrob:heightOfObject", pm); 00470 00471 if(data_props.get(depth) != null) { 00472 for(OWLLiteral d : data_props.get(depth)) { 00473 cur.dimensions.x = Double.valueOf(d.getLiteral()); 00474 } 00475 } 00476 00477 if(data_props.get(width) != null) { 00478 for(OWLLiteral w : data_props.get(width)) { 00479 cur.dimensions.y = Double.valueOf(w.getLiteral()); 00480 } 00481 } 00482 00483 if(data_props.get(height) != null) { 00484 for(OWLLiteral h : data_props.get(height)) { 00485 cur.dimensions.z = Double.valueOf(h.getLiteral()); 00486 } 00487 } 00488 00489 // read hinge-specific properties 00490 if(cur.types.contains("HingedJoint") || cur.types.contains("PrismaticJoint")) { 00491 00492 OWLDataProperty q_min = factory.getOWLDataProperty("knowrob:minJointValue", pm); 00493 OWLDataProperty q_max = factory.getOWLDataProperty("knowrob:maxJointValue", pm); 00494 00495 if(data_props.containsKey(q_min)) { 00496 for(OWLLiteral qm : data_props.get(q_min)) { 00497 ((MapJoint) cur).q_min = Double.valueOf(qm.getLiteral()); 00498 } 00499 } 00500 00501 if(data_props.containsKey(q_max)) { 00502 for(OWLLiteral qm : data_props.get(q_max)) { 00503 ((MapJoint) cur).q_max = Double.valueOf(qm.getLiteral()); 00504 } 00505 } 00506 00507 if(cur.types.contains("HingedJoint")) { 00508 OWLDataProperty radius = factory.getOWLDataProperty("knowrob:turnRadius", pm); 00509 if(data_props.containsKey(radius)) { 00510 for(OWLLiteral rad : data_props.get(radius)) 00511 ((MapJoint) cur).radius = Double.valueOf(rad.getLiteral()); 00512 } 00513 } 00514 00515 if(cur.types.contains("PrismaticJoint")) { 00516 00517 OWLObjectProperty direction = factory.getOWLObjectProperty("knowrob:direction", pm); 00518 00519 if(obj_props.containsKey(direction)) { 00520 for(OWLIndividual dir : obj_props.get(direction)) { 00521 00522 Map<OWLDataPropertyExpression, Set<OWLLiteral>> vec_props = 00523 dir.getDataPropertyValues(ont); 00524 00525 OWLDataProperty vectorx = factory.getOWLDataProperty("knowrob:vectorX", pm); 00526 OWLDataProperty vectory = factory.getOWLDataProperty("knowrob:vectorY", pm); 00527 OWLDataProperty vectorz = factory.getOWLDataProperty("knowrob:vectorZ", pm); 00528 00529 if(vec_props.containsKey(vectorx)) { 00530 for(OWLLiteral x : vec_props.get(vectorx)) { 00531 ((MapJoint) cur).direction.x = Double.valueOf(x.getLiteral()); 00532 } 00533 } 00534 if(vec_props.containsKey(vectory)) { 00535 for(OWLLiteral y : vec_props.get(vectory)) { 00536 ((MapJoint) cur).direction.y = Double.valueOf(y.getLiteral()); 00537 } 00538 } 00539 if(vec_props.containsKey(vectorz)) { 00540 for(OWLLiteral z : vec_props.get(vectorz)) { 00541 ((MapJoint) cur).direction.z = Double.valueOf(z.getLiteral()); 00542 } 00543 } 00544 } 00545 } 00546 } 00547 } 00548 00549 00550 // get pose elements 00551 Map<OWLDataPropertyExpression, Set<OWLLiteral>> matrix_elems = 00552 pose.getDataPropertyValues(ont); 00553 00554 for(int i=0;i<4;i++) { 00555 for(int j=0;j<4;j++){ 00556 OWLDataProperty m_ij = factory.getOWLDataProperty("knowrob:m"+i+j, pm); 00557 Set<OWLLiteral> elem = matrix_elems.get(m_ij); 00558 00559 for(OWLLiteral e : elem) { 00560 cur.pose_matrix.setElement(i, j, Double.valueOf(e.getLiteral()) ); 00561 } 00562 00563 00564 } 00565 } 00566 objects.put(cur.id, cur); 00567 00568 } 00569 } 00570 } 00571 } 00572 00573 00574 // link objects to their physical parts 00575 for(OWLNamedIndividual instances : ont.getIndividualsInSignature()) { 00576 00577 Set<OWLClassExpression> types = instances.getTypes(ont); 00578 OWLClass semanticMapPerception = factory.getOWLClass("knowrob:SemanticMapPerception", pm); 00579 00580 if(types.contains(semanticMapPerception)) { 00581 00582 Map<OWLObjectPropertyExpression, Set<OWLIndividual>> perc_props = 00583 instances.getObjectPropertyValues(ont); 00584 00585 OWLObjectProperty objectActedOn = factory.getOWLObjectProperty("knowrob:objectActedOn", pm); 00586 00587 Set<OWLIndividual> objs = perc_props.get(objectActedOn); 00588 00589 00590 for(OWLIndividual obj : objs) { 00591 00592 // get proper physical parts 00593 Map<OWLObjectPropertyExpression, Set<OWLIndividual>> obj_props = 00594 obj.getObjectPropertyValues(ont); 00595 00596 OWLObjectProperty parts = factory.getOWLObjectProperty("knowrob:properPhysicalParts", pm); 00597 if(obj_props.containsKey(parts)) { 00598 for(OWLIndividual p : obj_props.get(parts)) { 00599 MapObject part = objects.get(p.toStringID().split("#")[1]); 00600 if(part!=null) { 00601 objects.get(obj.toStringID().split("#")[1]).physicalParts.add(part); 00602 } 00603 } 00604 } 00605 } 00606 } 00607 } 00608 00609 // read parent and child 00610 for(OWLNamedIndividual instances : ont.getIndividualsInSignature()) { 00611 00612 Set<OWLClassExpression> types = instances.getTypes(ont); 00613 OWLClass semanticMapPerception = factory.getOWLClass("knowrob:SemanticMapPerception", pm); 00614 00615 if(types.contains(semanticMapPerception)) { 00616 00617 Map<OWLObjectPropertyExpression, Set<OWLIndividual>> perc_props = 00618 instances.getObjectPropertyValues(ont); 00619 00620 OWLObjectProperty objectActedOn = factory.getOWLObjectProperty("knowrob:objectActedOn", pm); 00621 00622 Set<OWLIndividual> objs = perc_props.get(objectActedOn); 00623 00624 00625 00626 for(OWLIndividual obj : objs) { 00627 00628 Map<OWLObjectPropertyExpression, Set<OWLIndividual>> obj_props = 00629 obj.getObjectPropertyValues(ont); 00630 OWLObjectProperty connectedTo = factory.getOWLObjectProperty("knowrob:connectedTo-Rigidly", pm); 00631 00632 if(obj_props.containsKey(connectedTo)) { 00633 00634 MapObject cur = objects.get(obj.toStringID().split("#")[1]); 00635 00636 for(OWLIndividual rel : obj_props.get(connectedTo)) { 00637 00638 MapObject connectedObj = objects.get(rel.toStringID().split("#")[1]); 00639 00640 if(connectedObj!=null) { 00641 00642 // connectedObj is a child if it is also contained 00643 // in the physicalParts list of the current object 00644 if(connectedObj.physicalParts.contains(cur)) { 00645 ((MapJoint) cur).parent = connectedObj; 00646 } else { 00647 ((MapJoint) cur).child = connectedObj; 00648 } 00649 00650 } 00651 00652 } 00653 } 00654 } 00655 00656 } 00657 } 00658 } 00659 00660 } catch (OWLOntologyCreationException e) { 00661 e.printStackTrace(); 00662 } 00663 return objects; 00664 } 00665 00666 00667 00672 protected static void readKnowRobObjectClasses() { 00673 00674 try { 00675 00676 // Create ontology manager, data factory, and prefix manager 00677 OWLOntologyManager manager = OWLManager.createOWLOntologyManager(); 00678 OWLDataFactory factory = manager.getOWLDataFactory(); 00679 DefaultPrefixManager pm = PREFIX_MANAGER; 00680 00681 // Find ros package holding the knowrob ontology 00682 String knowrob_pkg = RosUtilities.rospackFind(KNOWROB_PKG); 00683 String knowrob_owl = knowrob_pkg + "/" + KNOWROB_OWL; 00684 00685 00686 // Load the knowrob ontology 00687 OWLOntology ont = manager.loadOntologyFromOntologyDocument(new File(knowrob_owl)); 00688 00689 // Retrieve only subclasses of SpatialThing-Localized 00690 OWLReasoner reasoner = new StructuralReasoner(ont, new SimpleConfiguration(), BufferingMode.NON_BUFFERING); 00691 OWLClass spatialThing = factory.getOWLClass("knowrob:SpatialThing-Localized", pm); 00692 NodeSet<OWLClass> ns = reasoner.getSubClasses(spatialThing, false); 00693 00694 java.util.Set<Node<OWLClass>> set = ns.getNodes(); 00695 00696 // Iterate over all subclasses and put them into the mapping hashmap 00697 for(Node<OWLClass> n : set) { 00698 OWLClass c = (OWLClass) n.getRepresentativeElement(); 00699 00700 String iri = c.toStringID().replaceAll(KNOWROB, "knowrob:"); 00701 String key = c.toStringID().substring(c.toStringID().lastIndexOf('#') + 1).toLowerCase(); 00702 00703 rosToKnowrob.put(key, iri); 00704 } 00705 // to support backward compatibility (should be removed) 00706 rosToKnowrob.put("hinge", "knowrob:HingedJoint"); 00707 rosToKnowrob.put("knob", "knowrob:ControlKnob"); 00708 rosToKnowrob.put("horizontal_plane", "knowrob:CounterTop"); 00709 00710 } 00711 catch (Exception e) { 00712 e.printStackTrace(); 00713 } 00714 } 00715 00716 00722 protected String instForClass(String cl) { 00723 return cl+(inst_counter++); 00724 } 00725 00726 00730 protected static void printObjectTypes() { 00731 00732 for(Object o : rosToKnowrob.values()) { 00733 System.out.println(((String)o).replaceAll("knowrob:","")); 00734 } 00735 } 00736 00737 00738 }