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))
