def createDescription(self): #=======Params========= self.addParam("PlacingLocation", Element("skiros:Location"), ParamTypes.Required) self.addParam("Object", Element("skiros:Product"), ParamTypes.Required) self.addParam("Arm", Element("rparts:ArmDevice"), ParamTypes.Required) self.addParam("Gripper", Element("rparts:GripperEffector"), ParamTypes.Required) #=======PreConditions========= self.addPreCondition( self.getRelationCond("RobotAt", "skiros:at", "Robot", "PlacingLocation", True)) self.addPreCondition( self.getRelationCond("Holding", "skiros:contain", "Gripper", "Object", True)) #self.addPreCondition(self.getPropCond("LocationEmpty", ":ContainerState", "PlacingLocation", "=", "Empty", True)); #=======PostConditions========= self.addPostCondition( self.getRelationCond("NotHolding", "skiros:contain", "Gripper", "Object", False)) #self.addPostCondition(self.getPropCond("LocationEmpty", ":ContainerState", "PlacingLocation", "=", "Empty", False)); self.addPostCondition( self.getPropCond("EmptyHanded", "skiros:ContainerState", "Gripper", "=", "Empty", True)) self.addPostCondition( self.getRelationCond("InPlace", "skiros:contain", "PlacingLocation", "Object", True))
def createDescription(self): # =======Params========= self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Required) self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Required) self.addParam("Axis", float, ParamTypes.Required)
def createDescription(self): #=======Params========= self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Optional) self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Optional) self.addParam("Pose3", Element("skiros:TransformationPose"), ParamTypes.Optional)
def createDescription(self): #=======Params========= self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) self.addParam("Target", Element("sumo:Object"), ParamTypes.Required) self.addParam("Linear", float, ParamTypes.Optional) self.addParam("Angular", float, ParamTypes.Optional) self.addParam("MinVel", float, ParamTypes.Optional) self.addParam("MinDist", float, ParamTypes.Optional)
def createDescription(self): #=======Params========= #self.addParam("Container", Element(":Location"), ParamTypes.World) #self.addParam("Object", Element(":Product"), ParamTypes.Optional) self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Optional) self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Optional)
def createDescription(self): #=======Params========= self.addParam("StartLocation", Element("skiros:Location"), ParamTypes.Inferred) self.addParam("TargetLocation", Element("skiros:Location"), ParamTypes.Required) #=======PreConditions========= self.addPreCondition(self.getRelationCond("RobotAt", "skiros:at", "Robot", "StartLocation", True)) #=======PostConditions========= self.addPostCondition(self.getRelationCond("NoRobotAt", "skiros:at", "Robot", "StartLocation", False)) self.addPostCondition(self.getRelationCond("RobotAt", "skiros:at", "Robot", "TargetLocation", True))
def createDescription(self): #=======Params========= self.addParam("WorldModelObject", Element("skiros:TransformationPose"), ParamTypes.Required) self.addParam("WorldModelOptional", Element("skiros:TransformationPose"), ParamTypes.Optional) self.addParam("DictionaryOptional", dict, ParamTypes.Optional) self.addParam("Boolean", False, ParamTypes.Required) self.addParam("Number", 0.0, ParamTypes.Required)
def createDescription(self): #=======Params========= self.addParam("Container", Element("skiros:Location"), ParamTypes.Required) self.addParam("Object", Element("skiros:Product"), ParamTypes.Optional) self.addParam("Camera", Element("skiros:DepthCamera"), ParamTypes.Required, [ParamOptions.Lock]) #=======PreConditions========= self.addPreCondition(self.getRelationCond("RobotAt", "skiros:at", "Robot", "Container", True)) self.addPreCondition(self.getAbstractRelationCond("ContainerForObject", "skiros:partReference", "Container", "Object", True)) #=======PostConditions========= self.addPostCondition(self.getRelationCond("InContainer", "skiros:contain", "Container", "Object", True)) self.addPostCondition(self.getHasPropCond("HasPosition", "skiros:Position", "Object", True))
def createDescription(self): # =======Params========= self.addParam("StartLocation", Element("sumo:Object"), ParamTypes.Inferred) self.addParam("TargetLocation", Element("sumo:Object"), ParamTypes.Optional) self.addParam("Object", Element("sumo:Object"), ParamTypes.Required) self.addParam("Relation", "skiros:contain", ParamTypes.Required) # =======PreConditions========= self.addPreCondition( self.getRelationCond("StartContainObj", "skiros:spatiallyRelated", "StartLocation", "Object", True))
def generateDefParams(self): """ @brief Some default params are added automatically """ if not self._params.hasParam('Robot'): self._params.addParam("Robot", Element("sumo:Agent"), params.ParamTypes.Inferred)
def createDescription(self): #=======Params========= self.addParam("Name", str, ParamTypes.Required) self.addParam("X", 0.0, ParamTypes.Required) self.addParam("Y", 0.0, ParamTypes.Required) self.addParam("Rotation", 0.0, ParamTypes.Required) self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Optional)
def _register_agent(self, agent_name): res = self._wmi.resolve_element(Element("cora:Robot", agent_name)) if res: log.info("[{}]".format(self.__class__.__name__), "Found robot {}, skipping registration.".format(res)) self._robot = res for r in self._robot.getRelations("-1", "skiros:hasSkill"): self._wmi.remove_element(self._wmi.get_element(r['dst'])) self._robot = self._wmi.get_element(self._robot.id) else: self._robot = self._wmi.instanciate(agent_name, True) startLocUri = self._wmi.get_template_element( agent_name).getRelations(pred="skiros:hasStartLocation") if startLocUri: start_location = self._wmi.instanciate(startLocUri[0]["dst"], False, []) self._wmi.set_relation(self._robot._id, "skiros:at", start_location._id) self._robot = self._wmi.get_element(self._robot.id) log.info("[{}]".format(self.__class__.__name__), "Registered robot {}".format(self._robot)) self._robot.setProperty( "skiros:SkillMgr", self._agent_name[self._agent_name.rfind(":") + 1:]) self._wmi.update_element(self._robot)
def _autoParametrizeBB(self, skill): """ @brief ground undefined parameters with parameters in the Black Board """ to_resolve = [ key for key, param in skill._params.getParamMap().iteritems() if param.paramType != params.ParamTypes.Optional and param.dataTypeIs(Element) and param.getValue().getIdNumber() < 0 ] if not to_resolve: return True log.assertInfo(self._verbose, "[Autoparametrize]", "Resolving {}:{}".format(skill.type, to_resolve)) #self._importParentsConditions(skill, to_resolve) remap = {} cp = params.ParamHandler() cp.reset(skill._params.getCopy()) for c in skill._pre_conditions: c.setDesiredState(cp) for key in to_resolve: remap[key] = [] for k, p in self._params._params.iteritems(): if p.dataTypeIs(Element()): if p.getValue().isInstance(cp.getParamValue(key), self._wm): remap[key].append(k) else: pass # log.info("Not instance", "{} Model: {} Match: {}".format(key, cp.getParamValue(key).printState(True), p.getValue().printState(True))) return self._autoParametrizeWm(skill, to_resolve, cp)
def createDescription(self): # =======Params========= self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Required) self.addParam("Direction", 0, ParamTypes.Required, description="x: 0, y: 1, z: 2")
def test_remove_element(skiros_bot): "test that an element can be removed from wm through the gui" element = Element(":testType") element_inst = skiros_bot.add_object(element) #test that element is there assert skiros_bot.gui_has_element(element_inst) skiros_bot.remove_object(element_inst) #test that it was removed assert not skiros_bot.gui_has_element(element_inst)
def get_individual(self, name, context_id=""): """ @brief Builds an element from an individual @param context_id if defined look for the individual only in the context """ subject = self.lightstring2uri(name) if not self.uri_exists(subject, context_id): raise Exception( "Element {} doesn't exist in ontology. Uri: {}. Context: {}.". format(name, subject, context_id)) e = Element() for predicate, obj in self.ontology(context_id).predicate_objects( subject): if OWL.DatatypeProperty in self.ontology().objects( predicate, RDF.type) or predicate == RDFS.comment: e.setProperty(self.uri2lightstring(predicate), obj.value, self.uri2lightstring(obj.datatype), force_convertion=True) elif OWL.ObjectProperty in self.ontology().objects( predicate, RDF.type): e.addRelation("-1", self.uri2lightstring(predicate), self.uri2lightstring(obj)) elif predicate == RDF.type and obj != OWL.NamedIndividual: e._type = self.uri2lightstring(str(obj)) elif obj == OWL.NamedIndividual: pass elif predicate == RDFS.label: e._label = obj.value else: log.error( "[get_individual]", "Ignoring {}-{}-{}. Predicate is not defined in the ontology." .format(name, self.uri2lightstring(predicate), self.uri2lightstring(obj))) for subj, predicate in self.ontology(context_id).subject_predicates( subject): if (self.uri2lightstring(predicate) != "skiros:hasTemplate"): e.addRelation(self.uri2lightstring(subj), self.uri2lightstring(predicate), "-1") self._add_reasoners_prop(e) return e
def printParams(self, params): to_ret = "\n" for _, p in params.getParamMap().iteritems(): if isinstance(Element(), p.dataType()): to_ret += p._key + ": " for e in p.getValues(): to_ret += e.printState() + "\n" else: to_ret += p.printState() + "\n" return to_ret
def reset(self, add_root=True, scene_name="skiros:blank_scene"): """ @brief Initialize the scene """ IndividualsDataset.reset(self) self._id_gen.clear() if add_root: if self.has_individual(scene_name): root = self.get_individual(scene_name) else: root = Element("skiros:Scene", scene_name) self.add_element(root, self.__class__.__name__)
def inferUnvalidParams(self, skill): #print '{}: {} '.format(skill._label, self.printParams(skill._params)) unvalid_params = skill.checkPreCond(self._verbose) if unvalid_params: log.info("[{}] Reset unvalid params {}".format(skill._label, unvalid_params)) for k in unvalid_params: skill._params.setDefault(k) p = skill._params.getParam(k) if p.dataTypeIs(Element()) and p.getValue().getIdNumber() >= 0: skill._params.specify(k, self._wm.get_element(p.getValue()._id)) return self._autoParametrizeBB(skill) return True
def test_save_and_load_scene(skiros_bot): "Test saving a scene and loading it back" #setup a scene and save it scene_name = "test_scene.turtle" element = Element(":testType") element_inst = skiros_bot.add_object(element) skiros_bot.save_scene(scene_name) #reset the scene to clear the changes skiros_bot.reset_scene() assert not skiros_bot.gui_has_element(element_inst) #load the changes back in skiros_bot.load_scene(scene_name) assert skiros_bot.gui_has_element(element_inst)
def _updateRoutine(self, time): """ @brief Sync the modified parameters elements with wm @time The time to evaluate if a parameter was changed """ for k, p in self.params.iteritems(): if p.dataTypeIs(Element()) and p.hasChanges(time): vs = p.values for i, e in enumerate(vs): if not e.isAbstract(): self._wmi.update_element(e) else: vs[i] = self._wmi.add_element(e)
def generateDefConditions(self): """ @brief Some default preconditions are added automatically """ #self.addPreCondition(self.getRelationCond("HasSkill", "hasSkill", "Robot", "Skill", True)) # for key, param in self._params.getParamMapFiltered(params.ParamTypes.Hardware).iteritems(): # self.addPreCondition(self.getPropCond("DeviceIdle", "deviceState", key, "Idle", True)) for key, param in self._params.getParamMapFiltered( params.ParamTypes.Optional).items(): if isinstance(Element(), param.dataType()): c1 = self.getGenerateCond("Has" + key, key, True) dont_add = False for c2 in self._post_conditions: if c1.isEqual(c2) or c1.hasConflict(c2): dont_add = True if not dont_add: self._post_conditions = [c1] + self._post_conditions return True
def addParam(self, key, value, param_type, options=[], description=""): """ @brief Adds a parameter key: a unique string identifier value: the default value or type param_type: the type of parameter (see ParamTypes) options: see ParamOptions description: an optional verbose description """ self._params.addParam(key, value, param_type, description) if isinstance(value, type(Element())): for o in options: if o == ParamOptions.Consume: self._post_conditions += [ self.getGenerateCond("Consume" + key, key, False) ] elif o == ParamOptions.Unspecify: self._post_conditions += [ self.getIsSpecifiedCond("Unset" + key, key, False) ] elif o == ParamOptions.Lock: self._pre_conditions += [ self.getPropCond(key + 'Idle', "skiros:StateProperty", key, "=", "Idle", True) ] self._hold_conditions += [ self.getPropCond(key + 'Busy', "skiros:StateProperty", key, "=", "Idle", False) ] self._post_conditions += [ self.getPropCond(key + 'Idle', "skiros:StateProperty", key, "=", "Idle", True) ] elif o == ParamOptions.RespectType: self._pre_conditions.append( self.getOnTypeCond(key + 'OfType', key, self.params[key].default.type))
def createDescription(self): #=======Params========= self.addParam("WorldModelObject", Element("skiros:TransformationPose"), ParamTypes.Required)
def createDescription(self): self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) self.addParam("Linear", float, ParamTypes.Required, "Linear velocity") self.addParam("Angular", float, ParamTypes.Required, "Angular velocity in degrees")
def createDescription(self): #=======Params========= self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required)
def initProblem(self): objects = {} elements = {} self._elements = {} # Find objects for objType in self._pddl_interface._types._types["thing"]: temp = self._wmi.resolve_elements(wmi.Element(objType)) elements[objType] = temp if len(temp) > 0: objects[objType] = [] for e in temp: objects[objType].append(e.id) self._elements[e.id] = e self._elements[e.id.lower()] = e for e in self._abstract_objects: ctype = self._wmi.get_super_class(e.type) if ctype not in objects: objects[ctype] = [] elements[ctype] = [] e._id = e.label if not e.label in objects[ctype]: # Avoids duplicates objects[ctype].append(e._label) elements[ctype].append(e) self._elements[e.id] = e self._elements[e.id.lower()] = e self._pddl_interface.setObjects(objects) # Evaluate inital state for supertype, types in self._pddl_interface._types._types.items(): elements[supertype] = [] for t in types: elements[supertype] += elements[t] params = skirosp.ParamHandler() params.addParam("x", Element(), skirosp.ParamTypes.Required) params.addParam("y", Element(), skirosp.ParamTypes.Required) for p in self._pddl_interface._predicates: if self._wmi.get_reasoner(p.name) is not None: # The predicate is handled by a reasoner xtype = p.params[0]["valueType"] ytype = p.params[1]["valueType"] for xe in elements[xtype]: for ye in elements[ytype]: relations = self._wmi.get_reasoner( p.name).computeRelations(xe, ye) if self._verbose: log.info( "[Checking {}-{}-{}]".format( xe.id, p.name, ye.id), " Got: {}".format(relations)) if p.name in relations: self._pddl_interface.addInitState( pddl.GroundPredicate(p.name, [xe.id, ye.id])) else: # The predicate is handled normally if len(p.params) == 1: if p.value != None: c = cond.ConditionProperty("", p.name, "x", p.operator, p.value, True) else: c = cond.ConditionHasProperty("", p.name, "x", True) xtype = p.params[0]["valueType"] for xe in elements[xtype]: params.specify("x", xe) if c.evaluate(params, self._wmi): self._pddl_interface.addInitState( pddl.GroundPredicate(p.name, [xe._id], p.operator, p.value)) else: xtype = p.params[0]["valueType"] ytype = p.params[1]["valueType"] subx = [xtype] if self._pddl_interface.getSubTypes( xtype) is None else self._pddl_interface.getSubTypes( xtype) suby = [ytype] if self._pddl_interface.getSubTypes( ytype) is None else self._pddl_interface.getSubTypes( ytype) if p.abstracts: query_str_template = """ SELECT ?x ?y WHERE {{ {{ ?xtypes rdfs:subClassOf* {xtype}. }} UNION {{ {xtype} rdfs:subClassOf* ?xtypes. }} {{ ?ytypes rdfs:subClassOf* {ytype}. }} UNION {{ {ytype} rdfs:subClassOf* ?ytypes. }} ?xtypes rdfs:subClassOf ?restriction . ?restriction owl:onProperty {relation}. ?restriction ?quantity ?ytypes. ?x rdf:type/rdfs:subClassOf* ?xtypes. ?y rdf:type/rdfs:subClassOf* ?ytypes. }}""" else: query_str_template = """ SELECT ?x ?y WHERE {{ {{ ?x {relation} ?y. ?x rdf:type/rdfs:subClassOf* {xtype}. ?y rdf:type/rdfs:subClassOf* {ytype}.}} UNION {{?t {relation} ?z. ?t rdf:type/rdfs:subClassOf* {xtype}. ?z rdf:type/rdfs:subClassOf* {ytype}. ?t skiros:hasTemplate ?x. ?z skiros:hasTemplate ?y. }} UNION {{?t {relation} ?y. ?t rdf:type/rdfs:subClassOf* {xtype}. ?y rdf:type/rdfs:subClassOf* {ytype}. ?t skiros:hasTemplate ?x.}} UNION {{?x {relation} ?z. ?x rdf:type/rdfs:subClassOf* {xtype}. ?z rdf:type/rdfs:subClassOf* {ytype}. ?z skiros:hasTemplate ?y.}} }}""" for x in subx: for y in suby: query_str = query_str_template.format( relation=p.name, xtype=x, ytype=y) answer = self._wmi.query_ontology(query_str) for line in answer: tokens = line.strip().split(" ") self._pddl_interface.addInitState( pddl.GroundPredicate(p.name, tokens)) for p in self._pddl_interface._functions: c = cond.ConditionProperty("", p.name, "x", p.operator, p.value, True) xtype = p.params[0]["valueType"] for xe in elements[xtype]: params.specify("x", xe) if c.evaluate(params, self._wmi): self._pddl_interface.addInitState( pddl.GroundPredicate(p.name, [xe._id], p.operator, p.value)) if self._verbose: log.info("[Problem]", self._pddl_interface.printProblem(False))
def createDescription(self): self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) self.addParam("Target", Element("cora:Robot"), ParamTypes.Required)
def createDescription(self): self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) self.addParam("Linear", 0.0, ParamTypes.Required) self.addParam("Angular", 0.0, ParamTypes.Required) self.addParam("Duration", 1.0, ParamTypes.Optional)
def createDescription(self): #=======Params========= self.addParam("Robot", Element("cora:Robot"), ParamTypes.Required) self.addParam("On/Off", True, ParamTypes.Required) self.addPreCondition(ConditionProperty('r1 in scan', 'owl_name:r1_act_pos', 'Robot', '=', 'scan', True))