ObjectInstance.java

Go to the documentation of this file.

package edu.tum.cs.ias.knowrob.owl;

import java.util.Vector;

import javax.vecmath.Matrix4d;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;

import edu.tum.cs.ias.knowrob.prolog.PrologInterface;
import edu.tum.cs.ias.knowrob.prolog.PrologQueryUtils;


public class ObjectInstance extends OWLIndividual {

        
        protected Vector3d dimensions;
        
        protected Matrix4d pose_matrix;
        
        protected Vector<ObjectInstance> physicalParts;
        
        

        protected ObjectInstance(String iri, String label) {
                
                super(iri, label);
                
                this.dimensions = new Vector3d(); 
                this.pose_matrix = new Matrix4d();
                this.pose_matrix.setIdentity();
                
                this.physicalParts = new Vector<ObjectInstance>();
        }


        protected ObjectInstance(OWLIndividual ind) {
                
                this(ind.getIRI(), ind.getLabel());
                this.types.addAll(ind.getTypes());
                this.data_props.putAll(ind.getDataProperties());
                this.obj_props.putAll(ind.getObjProperties());
        }
        
        
        public static ObjectInstance getObjectInstance(String iri, String label) {

                // return exact match if available
                if(identifiers.containsKey(iri) && identifiers.get(iri) instanceof ObjectInstance) {
                        return (ObjectInstance) identifiers.get(iri);                   
                }
                
                // create ObjectInstance from higher-level objects if the existing object for this IRI has a more abstract type
                ObjectInstance res = new ObjectInstance(OWLIndividual.getOWLIndividual(iri, label));
                identifiers.put(iri, res);
                return res;
        }
        
        public static ObjectInstance getObjectInstance(String iri) {
                return getObjectInstance(iri, null); 
        }
        
        
        

        public Vector<ObjectInstance> getPhysicalParts() {
                return physicalParts;
        }

        public void setPhysicalParts(Vector<ObjectInstance> physicalParts) {
                this.physicalParts = physicalParts;
        }


        public void addPhysicalPart(ObjectInstance part) {
                this.physicalParts.add(part);
        }


        public void removePhysicalPart(ObjectInstance part) {
                this.physicalParts.remove(part);
        }


        public boolean hasPhysicalPart(ObjectInstance part) {
                if(this.physicalParts.contains(part))
                        return true;
                else return false;
        }
        
        
        

        public Vector3d getDimensions() {
                return dimensions;
        }


        public void setDimensions(Vector3d dimensions) {
                this.dimensions = dimensions;
        }

        
        public Matrix4d getPoseMatrix() {
                return pose_matrix;
        }


        public void setPoseMatrix(Matrix4d poseMatrix) {
                pose_matrix = poseMatrix;
        }
        
        
        public Vector3d getPosition() {
                return new Vector3d(pose_matrix.m03, 
                                                        pose_matrix.m13, 
                                                        pose_matrix.m23);
        }


        public void setPosition(Vector3d position) {
                pose_matrix.setM03(position.x);
                pose_matrix.setM13(position.y);
                pose_matrix.setM23(position.z);
        }

        
        public Quat4d getPoseQuaternion() {
                Quat4d res = new Quat4d();
                res.set(this.pose_matrix);
                return res;
        }
        

        public void setPoseQuaternion(Vector3d translation, Quat4d orientation, double scale) {
                pose_matrix = new Matrix4d(orientation, translation, scale);
        }

        
        
        public Vector<Float> getPoseAsVector() {
                Vector<Float> r = new Vector<Float>();
                
                for(int i=0;i<4;i++)
                        for(int j=0;j<4;j++)
                                r.add((float) pose_matrix.getElement(i, j));
                
                return r;
        }
                        
                        
        
        public void readFromProlog() {
                
                // read dimensions, pose, children
        }
        
        public void readFromPrologWithChildren() {
                
                readFromProlog();
                
                for(ObjectInstance inst : physicalParts)
                        inst.readFromProlog();
        }
        
        public void writeToProlog() {

                // write general instance-related stuff
                super.writeToProlog();
                
                
                // write object pose (new pose automatically overrides old one)
                PrologQueryUtils.setObjPose(iri, "SemanticMapPerception",  getPoseAsVector());
                
                
                // write object dimensions
                PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#depthOfObject', _)");
                PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#widthOfObject', _)");
                PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#heightOfObject', _)");
                PrologQueryUtils.assertDataPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#depthOfObject",  dimensions.x+"", "http://www.w3.org/2001/XMLSchema#float");
                PrologQueryUtils.assertDataPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#widthOfObject",  dimensions.y+"", "http://www.w3.org/2001/XMLSchema#float");
                PrologQueryUtils.assertDataPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#heightOfObject", dimensions.z+"", "http://www.w3.org/2001/XMLSchema#float");
                
                
                // write child objects
                PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#properPhysicalParts', _)");
                
                for(ObjectInstance child : physicalParts) {
                        child.writeToProlog();
                        PrologQueryUtils.assertObjectPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#properPhysicalParts",  child.getIRI());
                }
        }
}