knowrob_actions: knowrob_actions: PlanExporter.java Source File

00001 package edu.tum.cs.ias.knowrob;
00002 
00003 import java.io.BufferedInputStream;
00004 import java.io.File;
00005 import java.io.FileInputStream;
00006 import java.io.IOException;
00007 import java.util.ArrayList;
00008 import java.util.Arrays;
00009 import java.util.HashMap;
00010 import java.util.Vector;
00011 
00012 import com.google.common.base.CaseFormat;
00013 import com.google.common.base.Joiner;
00014 
00015 import net.sf.json.*;
00016 import edu.tum.cs.ias.knowrob.prolog.PrologInterface;
00017 import edu.tum.cs.ias.knowrob.utils.ros.RosUtilities;
00018 
00019 public class PlanExporter {
00020 
00021 
00022     public static final String MAPPING_CONFIG_FILE = "etc/knowrob2cpl.json";
00023 
00028     private ArrayList<String> plan;
00029 
00033     private JSONObject knowrobToCPL;
00034     private HashMap<String, String> designators;
00035     private Vector<String> designator_order;
00036     private int inst_counter = 100;
00037 
00038     public PlanExporter() throws IOException {
00039 
00040         designators = new HashMap<String, String>();
00041         designator_order = new Vector<String>();
00042         knowrobToCPL = readCplMapping(RosUtilities.rospackFind("knowrob_actions") + "/" +MAPPING_CONFIG_FILE);
00043         plan = new ArrayList<String>();
00044     }
00045 
00046     public String exportPlanToCPL(String plan_name) {
00047         
00048         plan_name = PrologInterface.addSingleQuotes(plan_name);
00049                 String res = "";
00050                 
00051                 HashMap<String, Vector<String>> plan_steps = 
00052                         PrologInterface.executeQuery("plan_subevents("+ plan_name +", Steps)");
00053 
00054                 
00055                 for(String dottedpairs : (plan_steps.get("Steps"))) {
00056                     
00057 
00058                     for(String[] actions: PrologInterface.dottedPairsToArrayList(dottedpairs)) {
00059                         
00060                         for(String action : new ArrayList<String>(Arrays.asList(actions))) {
00061                     
00062                             HashMap<String, Vector<String>> params = 
00063                                 PrologInterface.executeQuery("class_properties("+action+", Prop, Val)");
00064 
00065                             String obj_desig="", loc_desig="", act_desig="", device="", bodypart="";
00066                             String cplAction = knowrobToCpl(action);
00067 
00068                             for(int i=0;i<params.get("Prop").size();i++) {
00069 
00070                                 String prop = PrologInterface.removeSingleQuotes(params.get("Prop").get(i));
00071                                 String val  = PrologInterface.removeSingleQuotes(params.get("Val").get(i));
00072 
00073 
00074                                 if(prop.endsWith("objectActedOn")) {
00075 
00076                                     obj_desig = objectDesignatorFromOWLclass(val);
00077 
00078 
00079                                 } else if(prop.endsWith("toLocation") || prop.endsWith("fromLocation")) {
00080 
00081                                         // location specification
00082                                         loc_desig = locationDesignatorFromOWLclass(val);
00083                                     
00084                                         // trajectory specification for reaching motions
00085                                         act_desig = actionDesignatorFromOWLclass(action, prop, loc_desig);
00086                                         
00087                                 } else if(prop.endsWith("deviceUsed")) {
00088                                         device = val;
00089                                         
00090                                 } else if(prop.endsWith("bodyPartsUsed")) {
00091                                         bodypart = val;
00092                                 }
00093 
00094                             }
00095                             
00096                             // create action goal
00097                             String action_spec = "";
00098                             if(cplAction.endsWith("object-in-hand")) {
00099                                 
00100                                 if(bodypart!=null && bodypart.contains("left")) {
00101                                         action_spec = "(achieve `(object-in-hand ,"+ obj_desig +" :left))";
00102                                 } else {
00103                                         action_spec = "(achieve `(object-in-hand ,"+ obj_desig +" :right))";
00104                                 }
00105 
00106                                 
00107                             } else if(cplAction.endsWith("object-placed-at")) {
00108                                 
00109                                 if(bodypart!=null && bodypart.contains("left")) {
00110                                         action_spec = "(achieve `(object-placed-at ,"+ obj_desig +" ,"+ loc_desig +" :left))";
00111                                 } else {
00112                                         action_spec = "(achieve `(object-placed-at ,"+ obj_desig +" ,"+ loc_desig +" :right))";
00113                                 }
00114 
00115                                 
00116                             } else if(cplAction.endsWith("arm-parked")) {
00117                                 action_spec = "(achieve `(arm-parked ,"+device+"))";
00118 
00119                                 
00120                             } else if(cplAction.endsWith("open-gripper")) {
00121 
00122                                 String act_desig2 = "unhand-action"+(inst_counter++);
00123                                 String act_desig2_content = "";
00124                                 if(bodypart!=null && bodypart.contains("left")) {
00125                                         act_desig2_content = "("+act_desig2+" (action `((to open-gripper) (side :left))))\n";
00126                                 } else {
00127                                         act_desig2_content = "("+act_desig2+" (action `((to open-gripper) (side :right))))\n";
00128                                 }
00129                                 
00130                                 action_spec = "(achieve `(arms-at ," + act_desig2 + "))";
00131                                 
00132                                 if(!designator_order.contains(act_desig2))
00133                                         designator_order.add(act_desig2);
00134                                 designators.put(act_desig2, act_desig2_content);
00135                               
00136                             } else if(cplAction.endsWith("close-gripper")) {
00137 
00138                                 String act_desig2 = "unhand-action"+(inst_counter++);
00139                                 String act_desig2_content = "";
00140                                 if(bodypart!=null && bodypart.contains("left")) {
00141                                         act_desig2_content = "("+act_desig2+" (action `((to close-gripper) (side :left))))\n";
00142                                 } else {
00143                                         act_desig2_content = "("+act_desig2+" (action `((to close-gripper) (side :right))))\n";
00144                                 }
00145                                 
00146                                 action_spec = "(achieve `(arms-at ," + act_desig2 + "))";
00147                                 
00148                                 if(!designator_order.contains(act_desig2))
00149                                         designator_order.add(act_desig2);
00150                                 designators.put(act_desig2, act_desig2_content);
00151 
00152                                 
00153                             } else if(cplAction.endsWith("arms-at")) {
00154                                 action_spec = "(achieve `(arms-at ,"+ act_desig +"))";
00155 
00156                                 
00157                             } else if(cplAction.endsWith("looking-at")) {
00158                                 action_spec = "(achieve `(looking-at ,"+ loc_desig +"))";
00159 
00160                                 
00161                             } else if(cplAction.endsWith("perceive-all")) {
00162                                 action_spec = "(perceive-all ,"+ obj_desig +")";
00163 
00164                                 
00165                             } else if(cplAction.endsWith("perceive")) {
00166                                 action_spec = "(perceive ,"+ obj_desig +")"; 
00167 
00168                                 
00169                             }  else if(cplAction.endsWith("loc")) {
00170                                 action_spec = "(at-location ("+ loc_desig +"))"; // TODO: extend the closing parenthesis around all actions to be performed here 
00171                             }
00172 
00173                             plan.add(action_spec);
00174 
00175                         }
00176 
00177                     }
00178                 }
00179         
00180 
00181                 ArrayList<String> orderedDesigValues = new ArrayList<String>();
00182                 
00183                 for(String key : designator_order)
00184                         orderedDesigValues.add(designators.get(key));
00185 
00186 
00187                 // export to string
00188                 res += "(def-top-level-plan " + lispify(plan_name) + " () \n";
00189                 res += "(with-designators (\n      ";
00190                 res += Joiner.on("\n      ").join(orderedDesigValues);
00191                 
00192                 res += ")\n\n";
00193                 
00194                 res += Joiner.on("\n").join(plan);
00195                 
00196                 res += "))";
00197                 
00198                 return res;
00199         }
00200 
00201     
00202     
00203     
00210     private String objectDesignatorFromOWLclass(String objdef) {
00211 
00212         // create the appropriate object designators
00213         HashMap<String, Vector<String>> types = PrologInterface
00214                 .executeQuery("rdf_has(" + PrologInterface.addSingleQuotes(objdef) + ", rdf:type, T)");
00215 
00216         String obj_type = "";
00217         String obj_inst = "";
00218         String obj_desig = "";
00219         
00220         // first case: objdef is an instance
00221         if (types.get("T") != null && types.get("T").size() > 0) {
00222                 
00223             obj_type = knowrobToCpl(types.get("T").firstElement());
00224             obj_inst = knowrobToCpl(objdef);
00225             obj_desig = "(" + lispify(obj_inst) + " (object `((name " + knowrobToCpl(obj_inst) + ") (type "+lispify(obj_type)+"))))";
00226 
00227         } else { // second case: objdef is a class
00228             
00229             obj_type = knowrobToCpl(objdef);
00230             obj_inst = knowrobToCpl(instanceFromClass(obj_type));
00231             obj_desig = "(" + lispify(obj_inst) + " (object  `((type " + knowrobToCpl(obj_type) + "))))";
00232         }
00233 
00234         if(!designator_order.contains(obj_inst))
00235                 designator_order.add(obj_inst);
00236         
00237         designators.put(obj_inst, obj_desig);
00238         
00239         return obj_inst;
00240     }
00241 
00242     
00243     
00244     
00245     
00246     private String locationDesignatorForObject(String objClass) {
00247         
00248         // first create object designator:
00249         String obj_desig = objectDesignatorFromOWLclass(objClass);
00250 //      
00251 //      String loc_desig="";
00252 //      loc_desig = "(location-" + obj_desig + " (location `((of ," + obj_desig + "))))";
00253 //      
00254 //      if(!designator_order.contains(obj_desig))
00255 //              designator_order.add(obj_desig);
00256 //      designators.put("location-" + obj_desig, loc_desig);
00257 //      return "location-" + obj_desig;
00258         
00259         return obj_desig;
00260     }
00261     
00262     
00263     
00264     
00265     
00273     private String locationDesignatorFromOWLclass(String loc) {
00274         
00275         HashMap<String, Vector<String>> params = PrologInterface
00276                 .executeQuery("class_properties(" + PrologInterface.addSingleQuotes(loc) + ", Prop, Val); " +
00277                                       "owl_has("+PrologInterface.addSingleQuotes(loc)+", Prop, Val)");
00278         
00279         
00280         loc = knowrobToCpl(loc);
00281         
00282         // recursively build the location designator
00283         String loc_desig = "(" + loc + " ";
00284         loc_desig += "(location `(";
00285         
00286         
00287         if(params!=null && params.get("Prop")!=null) {
00288         
00289             for (int i = 0; i < params.get("Prop").size(); i++) {
00290     
00291                 String prop = PrologInterface.removeSingleQuotes(params.get("Prop").get(i));
00292                 String val = params.get("Val").get(i);
00293     
00294                 // check if referring to object instance or another location
00295                 HashMap<String, Vector<String>> objtype = PrologInterface
00296                         .executeQuery("rdf_has("+PrologInterface.addSingleQuotes(val)+", rdf:type, Type)");
00297                 
00298                 boolean isObject = true;
00299                 if(objtype!=null && objtype.get("Type")!=null) {
00300                         for (int k = 0; k < objtype.get("Type").size(); k++) {
00301                                 
00302                                 String type = PrologInterface.removeSingleQuotes(objtype.get("Type").get(k));
00303                                 if( (type.endsWith("Point3D")) || (type.endsWith("Point2D"))) {
00304                                         isObject = false;
00305                                 }
00306                         }
00307                 }
00308                 
00309                 String desig = "";                               
00310                 if ((prop.endsWith("in-ContGeneric")
00311                         || prop.endsWith("in-UnderspecifiedContainer")
00312                         || prop.endsWith("into-UnderspecifiedContainer") 
00313                         || prop.endsWith("from-UnderspecifiedLocation"))) {
00314                         
00315                     if(isObject) {
00316                         desig = knowrobToCpl(locationDesignatorForObject(val));
00317                     } else {
00318                         desig = knowrobToCpl(locationDesignatorFromOWLclass(val));
00319                     }
00320                     
00321                     loc_desig += "(in ," + desig + ")";
00322                     
00323                 } else if (prop.endsWith("to-UnderspecifiedLocation")
00324                         || prop.endsWith("on-Physical")
00325                         || prop.endsWith("aboveOf")
00326                         || prop.endsWith("inCenterOf")) {
00327                     if(isObject) {
00328                         desig = knowrobToCpl(locationDesignatorForObject(val));
00329                     } else {
00330                         desig = knowrobToCpl(locationDesignatorFromOWLclass(val));
00331                     }
00332                     
00333                     loc_desig += "(on ," + desig + ")";
00334     
00335                 } else if (prop.endsWith("inReachOf") ||
00336                         prop.endsWith("inFrontOf-Generally")) {
00337                     if(isObject) {
00338                         desig = knowrobToCpl(locationDesignatorForObject(val));
00339                     } else {
00340                         desig = knowrobToCpl(locationDesignatorFromOWLclass(val));
00341                     }
00342                     
00343                     loc_desig += "(to reach) (side :right) (loc ," + desig + ")";
00344     
00345                 } else if (prop.endsWith("visibleFrom")) {
00346                     if(isObject) {
00347                         desig = knowrobToCpl(locationDesignatorForObject(val));
00348                     } else {
00349                         desig = knowrobToCpl(locationDesignatorFromOWLclass(val));
00350                     }
00351                     
00352                     loc_desig += "(to see ," + desig + ")";
00353     
00354                 } else if (prop.endsWith("orientation")) {
00355                     if(isObject) {
00356                         desig = knowrobToCpl(locationDesignatorForObject(val));
00357                     } else {
00358                         desig = knowrobToCpl(locationDesignatorFromOWLclass(val));
00359                     }
00360                     
00361                     loc_desig += "(pose ," + desig + ")";
00362                 }
00363     
00364             }
00365         }
00366         loc_desig += ")))";
00367 
00368         if(!designator_order.contains(loc))
00369                 designator_order.add(loc);
00370         designators.put(loc, loc_desig);
00371         return loc;
00372     }
00373 
00374     
00375     
00376     private String actionDesignatorFromOWLclass(String action, String prop, String loc_desig) {
00377         
00378         String cplAction = knowrobToCpl(action);
00379         String act_desig = cplAction+(inst_counter++);
00380         String act_descr="";
00381         act_descr += "("+act_desig+" (action `(";
00382         
00383         // check action type: only Reaching
00384         
00385         if(cplAction.endsWith("arms-at")) {
00386                 
00387                 act_descr += "(to trajectory) (pose ," + loc_desig + ")";
00388 
00389                 // check if side is set
00390                 HashMap<String, Vector<String>> params = 
00391                     PrologInterface.executeQuery("class_properties("+action+", Prop, Val)");
00392 
00393                 int idx = params.get("Prop").lastIndexOf(PrologInterface.
00394                                 addSingleQuotes("http://ias.cs.tum.edu/kb/knowrob.owl#bodyPartsUsed"));
00395                 
00396             if(idx>-1) {
00397                 if(params.get("Val").get(idx).contains("right") ||
00398                                 params.get("Val").get(idx).contains("Right")) {
00399                         act_descr+=" (side :right)";
00400                 } else if(params.get("Val").get(idx).contains("left") ||
00401                                 params.get("Val").get(idx).contains("Left")) {
00402                         act_descr+=" (side :left)";     
00403                 }
00404             }
00405             act_descr+=")))";
00406 
00407             if(!designator_order.contains(act_desig))
00408                 designator_order.add(act_desig);
00409                 designators.put(act_desig, act_descr);
00410 
00411                 }
00412                 return act_desig;               
00413                 
00414     }
00415     
00427     private String knowrobToCpl(String val) {
00428         
00429         if(knowrobToCPL.containsKey(PrologInterface.valueFromIRI(val))) {
00430             return knowrobToCPL.getString(PrologInterface.valueFromIRI(val));
00431             
00432         } else {
00433             
00434             // check whether there are definitions for superclasses, take the first one
00435             HashMap<String, Vector<String>> supercl = PrologInterface.
00436                     executeQuery("owl_subclass_of(" + PrologInterface.addSingleQuotes(val) + ", Super)");
00437             
00438             for(String sup : supercl.get("Super")) {
00439                 if(knowrobToCPL.containsKey(PrologInterface.valueFromIRI(sup))) {
00440                     return knowrobToCPL.getString(PrologInterface.valueFromIRI(sup));
00441                 }
00442             }
00443             return lispify(val);
00444         }
00445     }
00446     
00454     private String lispify(String pl) {
00455         
00456         // extract value from IRI
00457         String val = PrologInterface.valueFromIRI(PrologInterface.removeSingleQuotes(pl));
00458         val = CaseFormat.UPPER_CAMEL.to(CaseFormat.LOWER_HYPHEN, val);
00459         val = val.replaceAll("--", "_");
00460         
00461         return val;
00462     }
00463     
00464     
00465     
00474     public JSONObject readCplMapping(String configFile)
00475             throws java.io.IOException {
00476 
00477         byte[] buffer = new byte[(int) new File(configFile).length()];
00478         BufferedInputStream f = null;
00479         try {
00480             f = new BufferedInputStream(new FileInputStream(configFile));
00481             f.read(buffer);
00482         } finally {
00483             if (f != null)
00484                 try {
00485                     f.close();
00486                 } catch (IOException ignored) {
00487                 }
00488         }
00489 
00490         return (JSONObject) JSONSerializer.toJSON(new String(buffer));
00491     }
00492 
00493     
00501     private String instanceFromClass(String cl) {
00502         return (cl + (inst_counter++)).toLowerCase();
00503     }
00504 
00505     
00511     public static void main(String[] args) {
00512         
00513         try {
00514             
00515             PrologInterface.initJPLProlog("knowrob_actions");
00516             PlanExporter pl = new PlanExporter();
00517             String plan = pl.exportPlanToCPL("http://www.roboearth.org/kb/roboearth.owl#ServeADrink");
00518             System.out.println(plan);
00519             
00520         } catch (IOException e) {
00521             e.printStackTrace();
00522         }
00523         
00524     }
00525     
00526 }