$search
00001 package edu.tum.cs.ias.knowrob.owl; 00002 00003 import java.util.Vector; 00004 00005 import javax.vecmath.Matrix4d; 00006 import javax.vecmath.Quat4d; 00007 import javax.vecmath.Vector3d; 00008 00009 import edu.tum.cs.ias.knowrob.prolog.PrologInterface; 00010 import edu.tum.cs.ias.knowrob.prolog.PrologQueryUtils; 00011 00012 00013 public class ObjectInstance extends OWLIndividual { 00014 00015 00019 protected Vector3d dimensions; 00020 00024 protected Matrix4d pose_matrix; 00025 00029 protected Vector<ObjectInstance> physicalParts; 00030 00031 00032 00039 protected ObjectInstance(String iri, String label) { 00040 00041 super(iri, label); 00042 00043 this.dimensions = new Vector3d(); 00044 this.pose_matrix = new Matrix4d(); 00045 this.pose_matrix.setIdentity(); 00046 00047 this.physicalParts = new Vector<ObjectInstance>(); 00048 } 00049 00050 00056 protected ObjectInstance(OWLIndividual ind) { 00057 00058 this(ind.getIRI(), ind.getLabel()); 00059 this.types.addAll(ind.getTypes()); 00060 this.data_props.putAll(ind.getDataProperties()); 00061 this.obj_props.putAll(ind.getObjProperties()); 00062 } 00063 00064 00074 public static ObjectInstance getObjectInstance(String iri, String label) { 00075 00076 // return exact match if available 00077 if(identifiers.containsKey(iri) && identifiers.get(iri) instanceof ObjectInstance) { 00078 return (ObjectInstance) identifiers.get(iri); 00079 } 00080 00081 // create ObjectInstance from higher-level objects if the existing object for this IRI has a more abstract type 00082 ObjectInstance res = new ObjectInstance(OWLIndividual.getOWLIndividual(iri, label)); 00083 identifiers.put(iri, res); 00084 return res; 00085 } 00086 00095 public static ObjectInstance getObjectInstance(String iri) { 00096 return getObjectInstance(iri, null); 00097 } 00098 00099 00100 00101 00106 public Vector<ObjectInstance> getPhysicalParts() { 00107 return physicalParts; 00108 } 00109 00114 public void setPhysicalParts(Vector<ObjectInstance> physicalParts) { 00115 this.physicalParts = physicalParts; 00116 } 00117 00118 00123 public void addPhysicalPart(ObjectInstance part) { 00124 this.physicalParts.add(part); 00125 } 00126 00127 00132 public void removePhysicalPart(ObjectInstance part) { 00133 this.physicalParts.remove(part); 00134 } 00135 00136 00141 public boolean hasPhysicalPart(ObjectInstance part) { 00142 if(this.physicalParts.contains(part)) 00143 return true; 00144 else return false; 00145 } 00146 00147 00148 00149 00155 public Vector3d getDimensions() { 00156 return dimensions; 00157 } 00158 00159 00165 public void setDimensions(Vector3d dimensions) { 00166 this.dimensions = dimensions; 00167 } 00168 00169 00175 public Matrix4d getPoseMatrix() { 00176 return pose_matrix; 00177 } 00178 00179 00185 public void setPoseMatrix(Matrix4d poseMatrix) { 00186 pose_matrix = poseMatrix; 00187 } 00188 00189 00195 public Vector3d getPosition() { 00196 return new Vector3d(pose_matrix.m03, 00197 pose_matrix.m13, 00198 pose_matrix.m23); 00199 } 00200 00201 00207 public void setPosition(Vector3d position) { 00208 pose_matrix.setM03(position.x); 00209 pose_matrix.setM13(position.y); 00210 pose_matrix.setM23(position.z); 00211 } 00212 00213 00219 public Quat4d getPoseQuaternion() { 00220 Quat4d res = new Quat4d(); 00221 res.set(this.pose_matrix); 00222 return res; 00223 } 00224 00225 00233 public void setPoseQuaternion(Vector3d translation, Quat4d orientation, double scale) { 00234 pose_matrix = new Matrix4d(orientation, translation, scale); 00235 } 00236 00237 00238 00239 public Vector<Float> getPoseAsVector() { 00240 Vector<Float> r = new Vector<Float>(); 00241 00242 for(int i=0;i<4;i++) 00243 for(int j=0;j<4;j++) 00244 r.add((float) pose_matrix.getElement(i, j)); 00245 00246 return r; 00247 } 00248 00249 00250 00251 public void readFromProlog() { 00252 00253 // read dimensions, pose, children 00254 } 00255 00256 public void readFromPrologWithChildren() { 00257 00258 readFromProlog(); 00259 00260 for(ObjectInstance inst : physicalParts) 00261 inst.readFromProlog(); 00262 } 00263 00264 public void writeToProlog() { 00265 00266 // write general instance-related stuff 00267 super.writeToProlog(); 00268 00269 00270 // write object pose (new pose automatically overrides old one) 00271 PrologQueryUtils.setObjPose(iri, "SemanticMapPerception", getPoseAsVector()); 00272 00273 00274 // write object dimensions 00275 PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#depthOfObject', _)"); 00276 PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#widthOfObject', _)"); 00277 PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#heightOfObject', _)"); 00278 PrologQueryUtils.assertDataPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#depthOfObject", dimensions.x+"", "http://www.w3.org/2001/XMLSchema#float"); 00279 PrologQueryUtils.assertDataPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#widthOfObject", dimensions.y+"", "http://www.w3.org/2001/XMLSchema#float"); 00280 PrologQueryUtils.assertDataPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#heightOfObject", dimensions.z+"", "http://www.w3.org/2001/XMLSchema#float"); 00281 00282 00283 // write child objects 00284 PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#properPhysicalParts', _)"); 00285 00286 for(ObjectInstance child : physicalParts) { 00287 child.writeToProlog(); 00288 PrologQueryUtils.assertObjectPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#properPhysicalParts", child.getIRI()); 00289 } 00290 } 00291 } 00292