Inference.java
Go to the documentation of this file.
00001 /* \file Inference.java
00002  * \brief A collection of inference tasks to be executed on the client (robot).
00003  *
00004  * This file is part of the RoboEarth ROS re_comm_core package.
00005  * 
00006  * It was originally created for <a href="http://www.roboearth.org/">RoboEarth</a>.
00007  * The research leading to these results has received funding from the 
00008  * European Union Seventh Framework Programme FP7/2007-2013 
00009  * under grant agreement no248942 RoboEarth.
00010  *
00011  * Copyright (C) 2011 by 
00012  * <a href=" mailto:perzylo@cs.tum.edu">Alexander Perzylo</a>
00013  * Technische Universitaet Muenchen
00014  * 
00015  * Redistribution and use in source and binary forms, with or without
00016  * modification, are permitted provided that the following conditions are met:
00017  * 
00018  *    <UL>
00019  *     <LI> Redistributions of source code must retain the above copyright
00020  *       notice, this list of conditions and the following disclaimer.
00021  *     <LI> Redistributions in binary form must reproduce the above copyright
00022  *       notice, this list of conditions and the following disclaimer in the
00023  *       documentation and/or other materials provided with the distribution.
00024  *     <LI> Neither the name of Willow Garage, Inc. nor the names of its
00025  *       contributors may be used to endorse or promote products derived from
00026  *       this software without specific prior written permission.
00027  *    </UL>
00028  * 
00029  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00030  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00031  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00032  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00033  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00034  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00035  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00036  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00037  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00038  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00039  * POSSIBILITY OF SUCH DAMAGE.
00040  *
00041  * \author Alexander Perzylo
00042  * \version 1.0
00043  * \date 2011
00044  */
00045 
00046 package roboearth.wp5.owl;
00047 
00048 import java.util.ArrayList;
00049 import java.util.Set;
00050 
00051 import javax.vecmath.Matrix4d;
00052 
00053 import org.semanticweb.HermiT.Reasoner;
00054 import org.semanticweb.owlapi.apibinding.OWLManager;
00055 import org.semanticweb.owlapi.model.IRI;
00056 import org.semanticweb.owlapi.model.OWLClass;
00057 import org.semanticweb.owlapi.model.OWLDataFactory;
00058 import org.semanticweb.owlapi.model.OWLDataProperty;
00059 import org.semanticweb.owlapi.model.OWLIndividual;
00060 import org.semanticweb.owlapi.model.OWLNamedIndividual;
00061 import org.semanticweb.owlapi.model.OWLObjectProperty;
00062 import org.semanticweb.owlapi.model.OWLOntology;
00063 import org.semanticweb.owlapi.model.OWLOntologyManager;
00064 import org.semanticweb.owlapi.reasoner.NodeSet;
00065 import org.semanticweb.owlapi.util.DefaultPrefixManager;
00066 
00067 public class Inference {
00068 
00069         public static Matrix4d getPose(OWLOntology srdl, String linkIRI) {
00070                 
00071                 if (srdl == null || linkIRI == null || linkIRI.length() == 0) {
00072                         System.out.println("  Error: parameter error at call to Inference.getPose()");
00073                         return null;
00074                 }
00075                 
00076                 Matrix4d pose = null;
00077                 
00078                 System.out.print("  Initializing reasoner... ");
00079                 Reasoner hermit = new Reasoner(srdl);
00080                 System.out.println("DONE");
00081                 
00082                 System.out.print("  Checking SRDL consistency... ");
00083                 if (!hermit.isConsistent()) {
00084                         System.out.println("FAILED");
00085                 } else {
00086                         System.out.println("DONE");
00087 
00088                         DefaultPrefixManager pm = IRIDepot.PREFIX_MANAGER;
00089                         OWLOntologyManager manager = OWLManager.createOWLOntologyManager();
00090                         OWLDataFactory factory = manager.getOWLDataFactory();
00091                         
00092                         // find robot instance
00093                         OWLClass owlClass = factory.getOWLClass("knowrob:Robot", pm);
00094                         NodeSet<OWLNamedIndividual> robotNodes;
00095                         robotNodes = hermit.getInstances(owlClass, false);
00096                         Set<OWLNamedIndividual> robotSet = robotNodes.getFlattened();
00097                         if (robotSet.size() > 1) {
00098                                 System.out.println("  Error: " + robotSet.size() + " robot instances found (expected only one)");
00099                         } else if (robotSet.size() == 0) {
00100                                 System.out.println("  Error: No robot instance found");
00101                         } else {
00102                                 OWLNamedIndividual robot = robotSet.iterator().next();
00103                                 System.out.println("  IRI of robot instance: "+robot.getIRI());
00104 
00105                                 // find laser scanner instance
00106                                 IRI slIRI = IRI.create(linkIRI);
00107                                 OWLNamedIndividual targetLink;
00108                                 targetLink = factory.getOWLNamedIndividual(slIRI);
00109                                 if (!hermit.isDefined(targetLink)) {
00110                                         System.out.println("  Error: individual for target link '" + linkIRI + "' doesn't exist");
00111                                 } else {
00112                                         System.out.println("  IRI of target link: " + slIRI);
00113                                         
00114                                         // find UrdfJoint instance, that has the laser scanner 
00115                                         // as its succeeding link 
00116                                         OWLObjectProperty succLink;
00117                                         succLink = factory.getOWLObjectProperty("srdl2-comp:succeedingLink", pm);
00118         
00119                                         OWLClass urdfJoint = factory.getOWLClass("srdl2-comp:UrdfJoint", pm);
00120                                         NodeSet<OWLNamedIndividual> joints;
00121                                         joints = hermit.getInstances(urdfJoint, false);
00122                                         
00123                                         OWLNamedIndividual targetJoint = null;
00124                                         for (OWLNamedIndividual joint : joints.getFlattened()) {
00125                                                 if (joint.hasObjectPropertyValue(succLink, targetLink, srdl)) {
00126                                                         targetJoint = joint;
00127                                                         break;
00128                                                 }
00129                                         }
00130                                         // target link exists but there's no joint connecting it
00131                                         if (targetJoint == null) {
00132                                                 Matrix4d identity = new Matrix4d();
00133                                                 identity.setIdentity();
00134                                                 return identity;
00135                                         }
00136                                         System.out.println("  Joint leading to target link: "+targetJoint);
00137 
00138                                         // find the related rotational matrix
00139                                         OWLObjectProperty orientation;
00140                                         orientation = factory.getOWLObjectProperty("knowrob:orientation", pm);
00141                                         
00142                                         Set<OWLIndividual> matrices;
00143                                         matrices = targetJoint.getObjectPropertyValues(orientation, srdl);
00144                                         if (matrices.size() > 1) {
00145                                                 System.out.println("  Error: " + matrices.size() + " rotational matrices found (expected only one)");
00146                                         } else if (matrices.size() == 0) {
00147                                                 System.out.println("  Error: No rotational matrix found");
00148                                         } else {
00149                                                 OWLIndividual matrix = matrices.iterator().next();
00150                                                 System.out.println("  Orientation of target link: " + matrix.toString());
00151                                                 
00152                                                 ArrayList<Matrix4d> rotMats = new ArrayList<Matrix4d>();
00153                                                 do {
00154 
00155                                                         // read in matrix
00156                                                         OWLDataProperty m00 = factory.getOWLDataProperty("knowrob:m00", pm);
00157                                                         OWLDataProperty m01 = factory.getOWLDataProperty("knowrob:m01", pm);
00158                                                         OWLDataProperty m02 = factory.getOWLDataProperty("knowrob:m02", pm);
00159                                                         OWLDataProperty m03 = factory.getOWLDataProperty("knowrob:m03", pm);
00160                                                         OWLDataProperty m10 = factory.getOWLDataProperty("knowrob:m10", pm);
00161                                                         OWLDataProperty m11 = factory.getOWLDataProperty("knowrob:m11", pm);
00162                                                         OWLDataProperty m12 = factory.getOWLDataProperty("knowrob:m12", pm);
00163                                                         OWLDataProperty m13 = factory.getOWLDataProperty("knowrob:m13", pm);
00164                                                         OWLDataProperty m20 = factory.getOWLDataProperty("knowrob:m20", pm);
00165                                                         OWLDataProperty m21 = factory.getOWLDataProperty("knowrob:m21", pm);
00166                                                         OWLDataProperty m22 = factory.getOWLDataProperty("knowrob:m22", pm);
00167                                                         OWLDataProperty m23 = factory.getOWLDataProperty("knowrob:m23", pm);
00168                                                         OWLDataProperty m30 = factory.getOWLDataProperty("knowrob:m30", pm);
00169                                                         OWLDataProperty m31 = factory.getOWLDataProperty("knowrob:m31", pm);
00170                                                         OWLDataProperty m32 = factory.getOWLDataProperty("knowrob:m32", pm);
00171                                                         OWLDataProperty m33 = factory.getOWLDataProperty("knowrob:m33", pm);
00172                                                         
00173                                                         double[] m = new double[16]; // TODO error handling
00174                                                         m[0] = matrix.getDataPropertyValues(m00, srdl).iterator().next().parseDouble();
00175                                                         m[1] = matrix.getDataPropertyValues(m01, srdl).iterator().next().parseDouble();
00176                                                         m[2] = matrix.getDataPropertyValues(m02, srdl).iterator().next().parseDouble();
00177                                                         m[3] = matrix.getDataPropertyValues(m03, srdl).iterator().next().parseDouble();
00178                                                         m[4] = matrix.getDataPropertyValues(m10, srdl).iterator().next().parseDouble();
00179                                                         m[5] = matrix.getDataPropertyValues(m11, srdl).iterator().next().parseDouble();
00180                                                         m[6] = matrix.getDataPropertyValues(m12, srdl).iterator().next().parseDouble();
00181                                                         m[7] = matrix.getDataPropertyValues(m13, srdl).iterator().next().parseDouble();
00182                                                         m[8] = matrix.getDataPropertyValues(m20, srdl).iterator().next().parseDouble();
00183                                                         m[9] = matrix.getDataPropertyValues(m21, srdl).iterator().next().parseDouble();
00184                                                         m[10] = matrix.getDataPropertyValues(m22, srdl).iterator().next().parseDouble();
00185                                                         m[11] = matrix.getDataPropertyValues(m23, srdl).iterator().next().parseDouble();
00186                                                         m[12] = matrix.getDataPropertyValues(m30, srdl).iterator().next().parseDouble();
00187                                                         m[13] = matrix.getDataPropertyValues(m31, srdl).iterator().next().parseDouble();
00188                                                         m[14] = matrix.getDataPropertyValues(m32, srdl).iterator().next().parseDouble();
00189                                                         m[15] = matrix.getDataPropertyValues(m33, srdl).iterator().next().parseDouble();
00190                                                         
00191                                                         Matrix4d mat4d = new Matrix4d(m);
00192                                                         rotMats.add(mat4d);
00193                                                         
00194                                                         // search for relativeTo object property
00195                                                         OWLObjectProperty relativeTo;
00196                                                         relativeTo = factory.getOWLObjectProperty("knowrob:relativeTo", pm);
00197                                                         
00198                                                         Set<OWLIndividual> precMatrices;
00199                                                         precMatrices = matrix.getObjectPropertyValues(relativeTo, srdl);
00200                                                         if (precMatrices.size() > 1) {
00201                                                                 System.out.println("  Error: " + precMatrices.size() + " 'knowrob:relativeTo' data properties found (expected only one)");
00202                                                                 matrix = null;
00203                                                         } else if (precMatrices.size() == 0) {
00204                                                                 matrix = null;
00205                                                         } else {
00206                                                                 matrix = precMatrices.iterator().next();
00207                                                                 System.out.println("    relative to " + matrix);
00208                                                         }
00209                                                         
00210                                                 } while (matrix != null);
00211                                                 
00212                                                 if (rotMats.size() > 0) {
00213                                                         Matrix4d result = rotMats.get(rotMats.size()-1);
00214                                                         for (int i = rotMats.size()-2; i>=0; i--) {
00215                                                                 result.mul(rotMats.get(i));
00216                                                         }
00217                                                         pose = result;
00218                                                 }
00219                                                 
00220                                         }
00221                                         
00222                                 }
00223                                 
00224                         }
00225 
00226                 }
00227 
00228                 return pose;
00229                 
00230         }
00231         
00232         public static double getZCoordinate(OWLOntology srdl, String linkIRI) {
00233 
00234                 Matrix4d pose = getPose(srdl, linkIRI);
00235                 if (pose == null) {
00236                         System.out.println("  z coordinate of '" +linkIRI+ "': unknown");
00237                         return Double.NaN;
00238                 } else {
00239                         System.out.println("  z coordinate of '" +linkIRI+ "': "+pose.m23);
00240                         return pose.m23;
00241                 }
00242                 
00243         }
00244         
00248         public static void main(String[] args) {
00249                 
00250                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_robot1";
00251                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_base_link";
00252                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_base_laser_link";
00253                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_head_plate_frame";
00254                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_double_stereo_link";
00255                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_l_wrist_roll_link";
00256                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_l_gripper_led_frame";
00257                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_l_forearm_cam_frame";
00258                 //String targetLink = "http://ias.cs.tum.edu/kb/PR2.owl#pr2_l_forearm_cam_optical_frame";
00259                 String targetLink = "http://ias.cs.tum.edu/kb/amigo.owl#amigo_base_laser";
00260                 
00261                 //String fileName = "/home/alex/re_urdf2srdl/example/pr2.owl";
00262                 String fileName = "/home/alex/re_urdf2srdl/example/amigo.owl";
00263                 
00264                 
00265                 System.out.print("  Loading SRDL file... ");
00266                 OWLOntology srdl = OWLIO.loadOntologyFromFile(fileName);
00267                 if (srdl == null) {
00268                         System.out.println("FAILED");
00269                 } else {
00270                         System.out.println("DONE");
00271                         double z = getZCoordinate(srdl, targetLink);
00272                         if (Double.isNaN(z)) {
00273                                 System.out.println("\n  Couldn't determine z coordinate of target link");
00274                         } else {
00275                                 System.out.println("\n  z coordinate of target link: "+z);      
00276                         }
00277                 }
00278                 
00279         }
00280         
00281 }


re_comm_core
Author(s): Alexander Perzylo
autogenerated on Sun Jan 5 2014 11:29:33