$search
00001 package edu.tum.cs.ias.knowrob.owl; 00002 00003 import java.util.Vector; 00004 00005 import javax.vecmath.Vector3d; 00006 00007 import edu.tum.cs.ias.knowrob.prolog.PrologInterface; 00008 import edu.tum.cs.ias.knowrob.prolog.PrologQueryUtils; 00009 00010 00011 public class JointInstance extends ObjectInstance { 00012 00013 private final static String prop_qmin = "http://ias.cs.tum.edu/kb/knowrob.owl#minJointValue"; 00014 private final static String prop_qmax = "http://ias.cs.tum.edu/kb/knowrob.owl#maxJointValue"; 00015 private final static String prop_radius = "http://ias.cs.tum.edu/kb/knowrob.owl#turnRadius"; 00016 00020 public Vector3d direction; 00021 00025 public ObjectInstance child; 00026 00030 public ObjectInstance parent; 00031 00032 00033 00040 protected JointInstance(String iri, String label) { 00041 00042 super(iri, label); 00043 this.direction = new Vector3d(); 00044 } 00045 00046 00052 protected JointInstance(ObjectInstance ind) { 00053 00054 this(ind.getIRI(), ind.getLabel()); 00055 this.types.addAll(ind.getTypes()); 00056 this.data_props.putAll(ind.getDataProperties()); 00057 this.obj_props.putAll(ind.getObjProperties()); 00058 00059 this.dimensions.set(ind.getDimensions()); 00060 this.pose_matrix.set(ind.getPoseMatrix()); 00061 00062 this.physicalParts.addAll(ind.getPhysicalParts()); 00063 } 00064 00065 00075 public static JointInstance getMapJoint(String iri, String label) { 00076 00077 // return exact match if available 00078 if(identifiers.containsKey(iri) && identifiers.get(iri) instanceof JointInstance) { 00079 return (JointInstance) identifiers.get(iri); 00080 } 00081 00082 // create ObjectInstance from higher-level objects if the existing object for this IRI has a more abstract type 00083 JointInstance res = new JointInstance(ObjectInstance.getObjectInstance(iri, label)); 00084 identifiers.put(iri, res); 00085 return res; 00086 } 00087 00096 public static ObjectInstance getMapJoint(String iri) { 00097 return getMapJoint(iri, null); 00098 } 00099 00100 00101 00102 public double getQ_min() { 00103 if(data_props.containsKey(prop_qmin)) { 00104 return Double.valueOf(data_props.get(prop_qmin).firstElement()); 00105 } else return -1; 00106 } 00107 00108 00109 public void setQ_min(double q_min) { 00110 00111 if(data_props.containsKey(prop_qmin)) { 00112 data_props.get(prop_qmin).clear(); 00113 data_props.get(prop_qmin).add(q_min+""); 00114 } else { 00115 data_props.put(prop_qmin, new Vector<String>()).add(q_min+""); 00116 } 00117 } 00118 00119 00120 public double getQ_max() { 00121 if(data_props.containsKey(prop_qmax)) { 00122 return Double.valueOf(data_props.get(prop_qmax).firstElement()); 00123 } else return -1; 00124 } 00125 00126 00127 public void setQ_max(double q_max) { 00128 00129 if(data_props.containsKey(prop_qmax)) { 00130 data_props.get(prop_qmax).clear(); 00131 data_props.get(prop_qmax).add(q_max+""); 00132 } else { 00133 data_props.put(prop_qmax, new Vector<String>()).add(q_max+""); 00134 } 00135 } 00136 00137 00138 public Vector3d getDirection() { 00139 return direction; 00140 } 00141 00142 00143 public void setDirection(Vector3d direction) { 00144 this.direction = direction; 00145 } 00146 00147 00148 public ObjectInstance getChild() { 00149 return child; 00150 } 00151 00152 00153 public void setChild(ObjectInstance child) { 00154 this.child = child; 00155 } 00156 00157 00158 public ObjectInstance getParent() { 00159 return parent; 00160 } 00161 00162 public void setParent(ObjectInstance parent) { 00163 this.parent = parent; 00164 } 00165 00166 public double getRadius() { 00167 if(data_props.containsKey(prop_radius)) { 00168 return Double.valueOf(data_props.get(prop_radius).firstElement()); 00169 } else return -1; 00170 } 00171 00172 public void setRadius(double radius) { 00173 00174 if(data_props.containsKey(prop_radius)) { 00175 data_props.get(prop_radius).clear(); 00176 data_props.get(prop_radius).add(radius+""); 00177 } else { 00178 data_props.put(prop_radius, new Vector<String>()).add(radius+""); 00179 } 00180 } 00181 00182 00183 public void writeToProlog() { 00184 00185 // write general object instance-related stuff 00186 super.writeToProlog(); 00187 00188 00189 // set joint connection between parent and child 00190 if(types.contains(OWLClass.getOWLClass("http://ias.cs.tum.edu/kb/knowrob.owl#HingedJoint")) && child!= null && parent!=null) { 00191 PrologInterface.executeQuery("rdf_retractall('" + parent.getIRI() + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#hingedTo', _)"); 00192 PrologQueryUtils.assertObjectPropertyForInst(parent.getIRI(), "http://ias.cs.tum.edu/kb/knowrob.owl#hingedTo", child.getIRI()); 00193 00194 } else if(types.contains(OWLClass.getOWLClass("http://ias.cs.tum.edu/kb/knowrob.owl#PrismaticJoint")) && child!= null && parent!=null) { 00195 PrologInterface.executeQuery("rdf_retractall('" + parent.getIRI() + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#prismaticallyConnectedTo', _)"); 00196 PrologQueryUtils.assertObjectPropertyForInst(parent.getIRI(), "http://ias.cs.tum.edu/kb/knowrob.owl#prismaticallyConnectedTo", child.getIRI()); 00197 } 00198 00199 // set rigid connection between joint and parent/child resp. 00200 if(child!= null) { 00201 PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#connectedTo-Rigidly', _)"); 00202 PrologQueryUtils.assertObjectPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#connectedTo-Rigidly", child.getIRI()); 00203 } 00204 00205 if(parent!=null) { 00206 PrologInterface.executeQuery("rdf_retractall('" + iri + "', 'http://ias.cs.tum.edu/kb/knowrob.owl#connectedTo-Rigidly', _)"); 00207 PrologQueryUtils.assertObjectPropertyForInst(iri, "http://ias.cs.tum.edu/kb/knowrob.owl#connectedTo-Rigidly", parent.getIRI()); 00208 } 00209 00210 // write direction vector 00211 if(direction!=null && !(direction.x==0 && direction.y==0 && direction.z==0)) { 00212 00213 PrologInterface.executeQuery("rdf_has('"+ iri + "', knowrob:'direction', DirVec),!, " + 00214 "rdf_retractall('"+ iri + "', knowrob:direction, DirVec), " + 00215 "rdf_retractall(DirVec, _, _)"); 00216 00217 PrologInterface.executeQuery( 00218 "rdf_instance_from_class(knowrob:'Vector', DirVec)," + 00219 "rdf_assert(DirVec, knowrob:'vectorX', literal(type(xsd:float, " + direction.x + ")))," + 00220 "rdf_assert(DirVec, knowrob:'vectorY', literal(type(xsd:float, " + direction.y + ")))," + 00221 "rdf_assert(DirVec, knowrob:'vectorZ', literal(type(xsd:float, " + direction.z + ")))," + 00222 "rdf_assert('"+ iri + "', knowrob:'direction', DirVec)"); 00223 } 00224 00225 00226 } 00227 00228 }