def test(self, time, negated=False):
if not self.is_concrete():
return False
if time < 0:
raise PredicateException("Out of range time for predicate '%s'." %
self)
try:
return self.expr.eval(self.get_param_vector(time),
tol=self.tol,
negated=negated)
except IndexError:
## this happens with an invalid time
raise PredicateException("Out of range time for predicate '%s'." %
self)
def test(self, time, negated=False):
# This test is overwritten so that collisions can be calculated correctly
if not self.is_concrete():
return False
if time < 0:
raise PredicateException("Out of range time for predicate '%s'." %
self)
try:
return self.neg_expr.eval(self.get_param_vector(time),
tol=self.tol,
negated=(not negated))
except IndexError:
## this happens with an invalid time
raise PredicateException("Out of range time for predicate '%s'." %
self)
def set_active_dof_inds(self, robot_body, reset=False):
robot = robot_body.env_body
if reset == True and self.dof_cache != None:
robot.SetActiveDOFs(self.dof_cache)
self.dof_cache = None
elif reset == False and self.dof_cache == None:
self.dof_cache = robot.GetActiveDOFIndices()
robot.SetActiveDOFs(list(range(2, 18)), DOFAffine.RotationAxis,
[0, 0, 1])
else:
raise PredicateException("Incorrect Active DOF Setting")
def get_param_vector(self, t):
end_ind = get_param_vector_helper(self, self.x, 0, t, self.attr_inds)
if self.dynamic:
try:
get_param_vector_helper(self, self.x, end_ind, t + 1,
self.attr_inds)
except IndexError:
raise PredicateException(
"Insufficient pose trajectory to check dynamic predicate '%s' at the timestep."
% self)
return self.x.reshape((self.x_dim, 1))
def get_param_vector(self, t):
end_ind = 0
start, end = self.active_range
for rel_t in range(start, end + 1):
try:
end_ind = get_param_vector_helper(self, self.x, end_ind,
t + rel_t, self.attr_inds)
except IndexError as err:
if end - start >= 1:
raise PredicateException(
"Insufficient pose trajectory to check dynamic predicate '%s' at the timestep."
% self)
else:
raise err
return self.x.reshape((self.x_dim, 1))
def _create_pred_schemas(domain_config):
try:
pred_path = importlib.import_module(domain_config["Predicates Import Path"])
except KeyError:
pred_path = namo_predicates
pred_schemas = {}
# for p_defn in domain_config["Derived Predicates"].split(";"):
# p_type, exp_types = map(str.strip, p_defn.split(",", 1))
# if not hasattr(common_predicates, p_type):
# raise PredicateException("Predicate type '%s' not defined!"%p_type)
# pred_schemas[p_type] = PredicateSchema(p_type, getattr(common_predicates, p_type), [s.strip() for s in exp_types.split(",")])
for p_defn in domain_config["Derived Predicates"].split(";"):
p_type, exp_types = map(str.strip, p_defn.split(",", 1))
if not hasattr(pred_path, p_type):
raise PredicateException("Predicate type '%s' not defined!" % p_type)
pred_schemas[p_type] = PredicateSchema(p_type, getattr(pred_path, p_type),
[s.strip() for s in exp_types.split(",")])
return pred_schemas