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)