def setup(self):
attrs = {"name": ["robot"], "geom": [1], "pose": [(0,0)], "_type": ["Robot"]}
attr_types = {"name": str, "geom": circle.RedCircle,"pose": Vector2d, "_type": str}
self.robot = parameter.Object(attrs, attr_types)
attrs = {"name": ["can1"], "geom": [1], "pose": [(3,4)], "_type": ["Can"]}
attr_types = {"name": str, "geom": circle.RedCircle, "pose": Vector2d, "_type": str}
self.can1 = parameter.Object(attrs, attr_types)
attrs = {"name": ["can2"], "geom": [1], "pose": [(3,4)], "_type": ["Can"]}
attr_types = {"name": str, "geom": circle.RedCircle, "pose": Vector2d, "_type": str}
self.can2 = parameter.Object(attrs, attr_types)
attrs = {"name": ["target"], "geom": [1], "value": [(3,4)], "_type": ["Target"]}
attr_types = {"name": str, "geom": circle.BlueCircle, "value": Vector2d, "_type": str}
self.target = parameter.Symbol(attrs, attr_types)
attrs = {"name": ["rpose"], "value": ["undefined"], "_type": ["RobotPose"]}
attr_types = {"name": str, "value": Vector2d, "_type": str}
self.rpose = parameter.Symbol(attrs, attr_types)
self.pred0 = namo_predicates.At("At_0", [self.can1, self.target], ["Can", "Target"])
self.pred1 = namo_predicates.At("At_1", [self.can2, self.target], ["Can", "Target"])
self.pred2 = namo_predicates.RobotAt("RobotAt_0", [self.robot, self.rpose], ["Robot", "RobotPose"])
params = [self.can1, self.target]
pred_dict = [{'pred': self.pred0, 'negated': False, 'hl_info': 'pre', 'active_timesteps': (0, 5)}]
act0 = action.Action(2, 'test_action0', (0,5), params, pred_dict)
params = [self.can2, self.target]
pred_dict = [{'pred': self.pred1, 'negated': False, 'hl_info': 'pre', 'active_timesteps': (3, 7)}]
act1 = action.Action(2, 'test_action1', (3,7), params, pred_dict)
params = [self.robot, self.rpose]
pred_dict = [{'pred': self.pred2, 'negated': False, 'hl_info': 'pre', 'active_timesteps': (4, 9)}]
act2 = action.Action(2, 'test_action2', (4,9), params, pred_dict)
plan_params = {"robot": self.robot, "can1": self.can1, "can2": self.can2, "target": self.target, "rpose": self.rpose}
plan_actions = [act0, act1, act2]
self.plan = plan.Plan(plan_params, plan_actions, 10, 1) #1 is a dummy_env
serializer = plan_hdf5_serialization.PlanSerializer()
serializer.write_plan_to_hdf5("test/test_plan.hdf5", self.plan)
def setup(self):
attrs = {"name": ["robot"], "geom": [1], "pose": [(0,0)], "_type": ["Robot"]}
attr_types = {"name": str, "geom": circle.RedCircle,"pose": Vector2d, "_type": str}
self.robot = parameter.Object(attrs, attr_types)
attrs = {"name": ["can1"], "geom": [1], "pose": [(3,4)], "_type": ["Can"]}
attr_types = {"name": str, "geom": circle.RedCircle, "pose": Vector2d, "_type": str}
self.can1 = parameter.Object(attrs, attr_types)
attrs = {"name": ["can2"], "geom": [1], "pose": [(3,4)], "_type": ["Can"]}
attr_types = {"name": str, "geom": circle.RedCircle, "pose": Vector2d, "_type": str}
self.can2 = parameter.Object(attrs, attr_types)
attrs = {"name": ["target"], "geom": [1], "value": [(3,4)], "_type": ["Target"]}
attr_types = {"name": str, "geom": circle.BlueCircle, "value": Vector2d, "_type": str}
self.target = parameter.Symbol(attrs, attr_types)
attrs = {"name": ["rpose"], "value": ["undefined"], "_type": ["RobotPose"]}
attr_types = {"name": str, "value": Vector2d, "_type": str}
self.rpose = parameter.Symbol(attrs, attr_types)
self.pred0 = namo_predicates.At("At_0", [self.can1, self.target], ["Can", "Target"])
self.pred1 = namo_predicates.At("At_1", [self.can2, self.target], ["Can", "Target"])
self.pred2 = namo_predicates.RobotAt("RobotAt_0", [self.robot, self.rpose], ["Robot", "RobotPose"])
def test_expr_at(self):
radius = 1
attrs = {
"name": ["can"],
"geom": [radius],
"pose": ["undefined"],
"_type": ["Can"]
}
attr_types = {
"name": str,
"geom": circle.RedCircle,
"pose": Vector2d,
"_type": str
}
can = parameter.Object(attrs, attr_types)
attrs = {
"name": ["target"],
"geom": [radius],
"value": ["undefined"],
"_type": ["Target"]
}
attr_types = {
"name": str,
"geom": circle.BlueCircle,
"value": Vector2d,
"_type": str
}
target = parameter.Symbol(attrs, attr_types)
pred = namo_predicates.At("testpred", [can, target], ["Can", "Target"])
self.assertEqual(pred.get_type(), "At")
self.assertFalse(pred.test(time=400))
can.pose = np.array([[3, 4, 5, 6], [6, 5, 7, 8]])
# target doesn't have a value yet
self.assertFalse(pred.test(time=400))
target.value = np.array([[3, 4, 5, 7], [6, 5, 8, 7]])
self.assertTrue(pred.is_concrete())
with self.assertRaises(PredicateException) as cm:
pred.test(time=4)
self.assertEqual(
cm.exception.message,
"Out of range time for predicate 'testpred: (At can target)'.")
with self.assertRaises(PredicateException) as cm:
pred.test(time=-1)
self.assertEqual(
cm.exception.message,
"Out of range time for predicate 'testpred: (At can target)'.")
self.assertTrue(pred.test(time=0))
self.assertFalse(pred.test(time=1))
self.assertFalse(pred.test(time=2))
self.assertFalse(pred.test(time=3))
attrs = {"name": ["sym"], "value": ["undefined"], "_type": ["Sym"]}
attr_types = {"name": str, "value": str, "_type": str}
sym = parameter.Symbol(attrs, attr_types)
with self.assertRaises(ParamValidationException) as cm:
pred = namo_predicates.At("testpred", [can, sym],
["Can", "Target"])
self.assertEqual(
cm.exception.message,
"Parameter type validation failed for predicate 'testpred: (At can sym)'."
)
attrs = {
"name": ["target"],
"geom": [radius],
"value": ["undefined"],
"_type": ["Target"]
}
attr_types = {
"name": str,
"geom": circle.BlueCircle,
"value": Vector2d,
"_type": str
}
sym = parameter.Symbol(attrs, attr_types)
sym.value = np.array([[3, 2], [6, 4]])
pred = namo_predicates.At("testpred", [can, sym], ["Can", "Target"])
self.assertTrue(pred.test(time=0))
self.assertFalse(pred.test(time=1))
# testing get_expr
pred_dict = {
"negated": False,
"hl_info": "pre",
"active_timesteps": (0, 0),
"pred": pred
}
self.assertTrue(
isinstance(pred.get_expr(pred_dict["negated"]), expr.EqExpr))
pred_dict['hl_info'] = "hl_state"
self.assertTrue(
isinstance(pred.get_expr(pred_dict["negated"]), expr.EqExpr))
pred_dict['negated'] = True
self.assertTrue(pred.get_expr(pred_dict["negated"]) is None)
pred_dict['hl_info'] = "pre"
self.assertTrue(pred.get_expr(pred_dict["negated"]) is None)
def setUp(self):
attrs = {
"name": ["robot"],
"geom": [1],
"pose": [(0, 0)],
"_type": ["Robot"]
}
attr_types = {
"name": str,
"geom": circle.RedCircle,
"pose": Vector2d,
"_type": str
}
self.robot = parameter.Object(attrs, attr_types)
attrs = {
"name": ["can"],
"geom": [1],
"pose": ["undefined"],
"_type": ["Can"]
}
attr_types = {
"name": str,
"geom": circle.RedCircle,
"pose": Vector2d,
"_type": str
}
self.can = parameter.Object(attrs, attr_types)
attrs = {
"name": ["target"],
"geom": [1],
"value": ["undefined"],
"_type": ["Target"]
}
attr_types = {
"name": str,
"geom": circle.BlueCircle,
"value": Vector2d,
"_type": str
}
self.target = parameter.Symbol(attrs, attr_types)
attrs = {"name": ["gp"], "value": [(3, 6.)], "_type": ["Sym"]}
attr_types = {"name": str, "value": Vector2d, "_type": str}
self.gp = parameter.Symbol(attrs, attr_types)
self.at = namo_predicates.At("at", [self.can, self.target],
["Can", "Target"])
env = Environment()
self.in_contact = namo_predicates.InContact(
"incontact", [self.robot, self.gp, self.target],
["Robot", "Sym", "Target"],
env=env)
self.init_state = state.State("state", {
self.can.name: self.can,
self.target.name: self.target,
self.gp.name: self.gp
}, [self.at, self.in_contact],
timestep=0)