def test_args(self):
attrs = {
"name": ["robot"],
"geom": [1],
"pose": [(0, 0)],
"_type": ["Robot"]
}
attr_types = {
"name": str,
"geom": circle.RedCircle,
"pose": Vector2d,
"_type": str
}
robot = parameter.Object(attrs, attr_types)
attrs = {
"name": ["can"],
"geom": [1],
"pose": [(3, 4)],
"_type": ["Can"]
}
attr_types = {
"name": str,
"geom": circle.RedCircle,
"pose": Vector2d,
"_type": str
}
can = parameter.Object(attrs, attr_types)
param_map = {"robot": robot, "can": can}
act_list = ["up", "down"]
s = plan.Plan(param_map, act_list, 3, 4)
self.assertEqual(s.params, param_map)
self.assertEqual(s.actions, act_list)
self.assertEqual(s.horizon, 3)
self.assertEqual(s.env, 4)
def test_get_active_preds(self):
self.setup()
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]
test_plan = plan.Plan(plan_params, plan_actions, 10,
1) #1 is a dummy_env
self.assertEqual(test_plan.get_active_preds(2), [self.pred0])
self.assertEqual(test_plan.get_active_preds(4),
[self.pred0, self.pred1, self.pred2])
self.assertEqual(test_plan.get_active_preds(6),
[self.pred1, self.pred2])
self.assertEqual(test_plan.get_active_preds(8), [self.pred2])
self.assertEqual(test_plan.get_active_preds(10), [])
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 test_get_failed_preds(self):
#this function tests get_failed_pred() and satisfied()
self.setup()
params = [self.can1, self.target]
pred_dict = [{'pred': self.pred0, 'negated': False, 'hl_info': 'pre', 'active_timesteps': (0, 1)}]
act0 = action.Action(2, 'test_action0', (0,10), params, pred_dict)
params = [self.can2, self.target]
pred_dict = [{'pred': self.pred1, 'negated': False, 'hl_info': 'pre', 'active_timesteps': (1, 3)}]
act1 = action.Action(2, 'test_action1', (0,10), params, pred_dict)
params = [self.robot, self.rpose]
pred_dict = [{'pred': self.pred2, 'negated': False, 'hl_info': 'pre', 'active_timesteps': (2, 3)}]
act2 = action.Action(2, 'test_action2', (0,10), 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]
test_plan = plan.Plan(plan_params, plan_actions, 10, 1) #1 is a dummy_env
self.robot.pose = np.array([[3, 4, 7, 7],
[2, 5, 8, 9]])
self.can1.pose = np.array([[5, 5, 7, 9],
[5, 5, 6, 8]])
self.can2.pose = np.array([[9, 5, 5, 5],
[4, 5, 5, 4]])
self.rpose.value = np.array([[7],
[9]])
self.target.value = np.array([[5],
[5]])
self.assertFalse(test_plan.satisfied())
self.assertEqual(test_plan.get_failed_pred(), (False, self.pred2, 2))
self.assertEqual(test_plan.get_failed_preds(), [(False, self.pred1, 3), (False, self.pred2, 2)])
self.robot.pose = np.array([[3, 4, 7, 7],
[2, 5, 9, 9]])
self.assertEqual(test_plan.get_failed_pred(), (False, self.pred1, 3))
self.assertEqual(test_plan.get_failed_preds(), [(False, self.pred1, 3)])
self.assertFalse(test_plan.satisfied())
self.can2.pose = np.array([[9, 5, 5, 5],
[4, 5, 5, 5]])
self.assertEqual(test_plan.get_failed_pred(), (False, None, 11))
self.assertEqual(test_plan.get_failed_preds(), [])
self.assertTrue(test_plan.satisfied())