def test_copy_symbol(self): attrs = {"name": ["param"], "circ": [1], "test": [3.7], "test2": [5.3], "test3": [6.5], "value": ["(3, 6)"], "rotation": ["(1,2,3)"], "_type": ["Can"]} attr_types = {"name": str, "test": float, "test3": str, "test2": int, "circ": circle.BlueCircle, "value": matrix.Vector2d, "rotation": matrix.Vector3d, "_type": str} p = parameter.Symbol(attrs, attr_types) p2 = p.copy(new_horizon=7) self.assertEqual(p2.name, "param") self.assertEqual(p2.test, 3.7) self.assertTrue(np.allclose(p2.value, [[3], [6]])) self.assertTrue(np.allclose(p2.rotation, [[1], [2], [3]])) p2 = p.copy(new_horizon=2) self.assertTrue(np.allclose(p2.value, [[3], [6]])) self.assertTrue(np.allclose(p2.rotation, [[1], [2], [3]])) attrs["value"] = ["undefined"] attrs["rotation"] = ["undefined"] p = parameter.Symbol(attrs, attr_types) p2 = p.copy(new_horizon=7) self.assertEqual(p2.name, "param") self.assertEqual(p2.test, 3.7) arr = np.empty((2, 1)) arr[:] = np.NaN self.assertTrue(np.allclose(p2.value, arr, equal_nan=True)) arr = np.empty((3, 1)) arr[:] = np.NaN self.assertTrue(np.allclose(p2.rotation, arr, equal_nan=True))
def test_symbol(self): attrs = {"circ": [1], "test": [3.7], "test2": [5.3], "test3": [6.5], "pose": [(3, 5)], "_type": ["CanSym"], "name": ["sym"]} attr_types = {"test": float, "test3": str, "test2": int, "circ": circle.BlueCircle, "pose": matrix.Vector2d, "_type": str, "name": str} with self.assertRaises(AssertionError) as cm: parameter.Symbol(attrs, attr_types) attrs["value"] = [(4, 6)] with self.assertRaises(DomainConfigException) as cm: parameter.Symbol(attrs, attr_types) self.assertEqual(cm.exception.message, "Attribute 'value' for Symbol 'sym' not defined in domain file.") attr_types["value"] = matrix.Vector2d param = parameter.Symbol(attrs, attr_types) self.assertEqual(param.name, "sym") self.assertEqual(param.get_type(), "CanSym") self.assertTrue(param.is_symbol()) self.assertTrue(param.is_defined()) self.assertEqual(param.test, 3.7) self.assertEqual(param.test2, 5) self.assertEqual(param.test3, "6.5") self.assertEqual(param.circ.radius, 1) param.pose = "undefined" self.assertTrue(param.is_defined()) param.value = "undefined" self.assertFalse(param.is_defined()) # test get_attr_type self.assertEqual(param.get_attr_type("test"), float) self.assertEqual(param.get_attr_type("test2"), int) self.assertEqual(param.get_attr_type("test3"), str) self.assertEqual(param.get_attr_type("circ"), circle.BlueCircle) self.assertEqual(param.get_attr_type("pose"), matrix.Vector2d) self.assertEqual(param.get_attr_type("_type"), str) self.assertEqual(param.get_attr_type("_attr_types"), dict) with self.assertRaises(KeyError): param.get_attr_type("does not exist")
def test_expr_pred(self): ## test initialization: x_dim computed correctly ## test radius = 1 attrs = {"name": ["can"], "geom": [radius], "pose": ["undefined"], "_type": ["Can"]} attr_types = {"name": str, "geom": circle.RedCircle, "pose": int, "_type": str} p1 = parameter.Object(attrs, attr_types) p1.pose = np.array([[1, 2, 3], [2, 3, 1], [3, 1, 2]]) attrs = {"name": ["sym"], "value": ["undefined"], "_type": ["Sym"]} attr_types = {"name": str, "value": int, "_type": str} p2 = parameter.Symbol(attrs, attr_types) p2.value = np.array([2, 2, 2]) ## ExprPred Construction attr_inds = OrderedDict([(p1, [("pose", np.array([0], dtype=np.int))]), (p2, [])]) e = expr.EqExpr(e1, np.array([2])) pred = common_predicates.ExprPredicate("expr_pred", e, attr_inds, [p1, p2], ["Can", "Sym"]) # with self.assertRaises(NotImplementedError): # pred.get_expr(None, None) ## get_param_vector self.assertEqual(pred.x_dim, 1) self.assertTrue(np.allclose(pred.get_param_vector(0), [1])) self.assertTrue(np.allclose(pred.get_param_vector(1), [2])) self.assertTrue(np.allclose(pred.get_param_vector(2), [3])) ## unpacking unpacked = pred.unpack([10]) self.assertTrue("can" in unpacked) self.assertTrue("sym" in unpacked) self.assertEqual(len(unpacked["can"]), 1) self.assertEqual(len(unpacked["sym"]), 0) self.assertEqual(("pose", [10]), unpacked["can"][0])
def setup_pr2_pose(name="pr2_pose"): attrs = { "name": [name], "value": [(0, 0, 0)], "_type": ["RobotPose"], "backHeight": [0.2], "lGripper": [0.5], "rGripper": [0.5] } attrs["lArmPose"] = [(0, 0, 0, 0, 0, 0, 0)] attrs["rArmPose"] = [(0, 0, 0, 0, 0, 0, 0)] attr_types = { "name": str, "value": matrix.Vector3d, "_type": str, "backHeight": matrix.Value, "lArmPose": matrix.Vector7d, "rArmPose": matrix.Vector7d, "lGripper": matrix.Value, "rGripper": matrix.Value } rPose = parameter.Symbol(attrs, attr_types) # Set the initial arm pose so that pose is not close to joint limit rPose.lArmPose = np.array([[ np.pi / 4, np.pi / 8, np.pi / 2, -np.pi / 2, np.pi / 8, -np.pi / 8, np.pi / 2 ]]).T rPose.rArmPose = np.array([[ -np.pi / 4, np.pi / 8, -np.pi / 2, -np.pi / 2, -np.pi / 8, -np.pi / 8, np.pi / 2 ]]).T return rPose
def test_expr_pred_leq(self): ## test (multiple tols) ## grad tols = [1e-6, 1e-4, 1e-2] radius = 1 attrs = {"name": ["can"], "geom": [radius], "pose": ["undefined"], "_type": ["Can"]} attr_types = {"name": str, "geom": circle.RedCircle, "pose": float, "_type": str} p1 = parameter.Object(attrs, attr_types) p1.pose = np.array([[1, 2, 3], [1 + tols[0], 2 + tols[0], 3], [1 + tols[1], 2 + tols[1], 3]]).T attrs = {"name": ["sym"], "value": ["undefined"], "_type": ["Sym"]} attr_types = {"name": str, "value": float, "_type": str} p2 = parameter.Symbol(attrs, attr_types) p2.value = np.array([[1, 2], [2, 3]]).T ## pred is p1.pose[:1] = p2.value attr_inds = OrderedDict([(p1, [("pose", np.array([0, 1], dtype=np.int))]), (p2, [("value", np.array([0, 1], dtype=np.int))])]) A = np.array([[1, 1, -1, -1]]) b = np.array([0]) aff_e = expr.AffExpr(A, b) e = expr.LEqExpr(aff_e, np.array([[0.]])) pred0 = common_predicates.ExprPredicate("leq_pred", e, attr_inds, [p1, p2], ["Can", "Sym"]) pred0.tol = tols[0] self.assertTrue(pred0.test(0)) # if pred0.expr.eval(pred0.get_param_vector(1), tol = pred0.tol): # import ipdb; ipdb.set_trace() self.assertFalse(pred0.test(1)) self.assertFalse(pred0.test(2)) pred1 = common_predicates.ExprPredicate("leq_pred", e, attr_inds, [p1, p2], ["Can", "Sym"]) pred1.tol = tols[1] self.assertTrue(pred1.test(0)) self.assertTrue(pred1.test(1)) self.assertFalse(pred1.test(2)) pred2 = common_predicates.ExprPredicate("leq_pred", e, attr_inds, [p1, p2], ["Can", "Sym"]) pred2.tol = tols[2] self.assertTrue(pred2.test(0)) self.assertTrue(pred2.test(1)) self.assertTrue(pred2.test(2)) ## its LEq, so increasing value for sym should make everything work p2.value += 5 self.assertTrue(pred0.test(0)) self.assertTrue(pred0.test(1)) self.assertTrue(pred0.test(2)) self.assertTrue(pred1.test(0)) self.assertTrue(pred1.test(1)) self.assertTrue(pred1.test(2)) self.assertTrue(pred2.test(0)) self.assertTrue(pred2.test(1)) self.assertTrue(pred2.test(2))
def test_param_validation(self): attrs = { "name": ["can"], "geom": [1], "pose": ["undefined"], "_type": ["Can"] } attr_types = { "name": str, "geom": RedCircle, "pose": int, "_type": str } p1 = parameter.Object(attrs, attr_types) attrs = { "name": ["target"], "geom": [1], "pose": ["undefined"], "_type": ["Target"] } attr_types = { "name": str, "geom": BlueCircle, "pose": int, "_type": str } p2 = parameter.Object(attrs, attr_types) attrs = {"name": ["sym"], "value": ["undefined"], "_type": ["Sym"]} attr_types = {"name": str, "value": int, "_type": str} p3 = parameter.Symbol(attrs, attr_types) # this one should work pred = predicate.Predicate("errorpred", [p1, p2, p3], ["Can", "Target", "Sym"]) self.assertEqual(pred.get_type(), "Predicate") with self.assertRaises(ParamValidationException) as cm: predicate.Predicate("errorpred", [p1, p2, p3], ["Target", "Can", "Sym"]) self.assertEqual( cm.exception.message, "Parameter type validation failed for predicate 'errorpred: (Predicate can target sym)'." ) with self.assertRaises(ParamValidationException) as cm: predicate.Predicate("errorpred", [p1, p2, p3], ["Can", "Target"]) self.assertEqual( cm.exception.message, "Parameter type validation failed for predicate 'errorpred: (Predicate can target sym)'." ) with self.assertRaises(ParamValidationException) as cm: predicate.Predicate("errorpred", [p1, p2, p3], ["Can", "Target", "Target", "Sym"]) self.assertEqual( cm.exception.message, "Parameter type validation failed for predicate 'errorpred: (Predicate can target sym)'." )
def test_in_gripper(self): # InGripper, Robot, Can, Grasp radius = 1 attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Robot"], "name": ["pr2"] } attr_types = { "geom": circle.GreenCircle, "pose": Vector2d, "_type": str, "name": str } robot = parameter.Object(attrs, attr_types) attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Can"], "name": ["can1"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } can = parameter.Object(attrs, attr_types) attrs = {"value": [(0, 0)], "_type": ["Grasp"], "name": ["grasp"]} attr_types = {"value": Vector2d, "_type": str, "name": str} grasp = parameter.Symbol(attrs, attr_types) pred = namo_predicates.InGripper("InGripper", [robot, can, grasp], ["Robot", "Can", "Grasp"]) grasp.value = np.array([[1], [2]]) #First test should fail because all objects's positions are in (0,0) self.assertFalse(pred.test(time=0)) #robot.pose - can.pose = grasp.value robot.pose = np.zeros((2, 4)) can.pose = np.array([[-1, 1, 3, 5], [-2, 2, 4, 6]]) self.assertTrue(pred.test(time=0)) self.assertFalse(pred.test(time=1)) self.assertFalse(pred.test(time=2)) self.assertFalse(pred.test(time=3)) robot.pose = np.array([[0, 2, 4, 6], [0, 4, 6, 8]]) self.assertTrue(pred.test(time=0)) self.assertTrue(pred.test(time=1)) self.assertTrue(pred.test(time=2)) self.assertTrue(pred.test(time=3))
def test_expr_pred_eq(self): ## test (multiple tols) ## grad tols = [1e-8, 1e-3, 1e-2] radius = 1 attrs = { "name": ["can"], "geom": [radius], "pose": ["undefined"], "_type": ["Can"] } attr_types = { "name": str, "geom": circle.RedCircle, "pose": float, "_type": str } p1 = parameter.Object(attrs, attr_types) p1.pose = np.array([[1, 2, 3], [1 + tols[1], 2 + tols[1], 3], [1 + tols[2], 2 + tols[2], 3]]).T attrs = {"name": ["sym"], "value": ["undefined"], "_type": ["Sym"]} attr_types = {"name": str, "value": float, "_type": str} p2 = parameter.Symbol(attrs, attr_types) # The [2,3] in p2's pose shouldn't be used. p2.value = np.array([[1, 2], [2, 3]], dtype=np.float64).T ## pred is p1.pose[:1] = p2.value attr_inds = { p1: [("pose", np.array([0, 1], dtype=np.int))], p2: [("value", np.array([0, 1], dtype=np.int))] } A = np.array([[1, 1, -1, -1]]) b = np.array([0]) aff_e = expr.AffExpr(A, b) e = expr.EqExpr(aff_e, np.array([[0.]])) pred0 = common_predicates.ExprPredicate("eq_expr_pred", e, attr_inds, [p1, p2], ["Can", "Sym"]) self.assertTrue(pred0.test(0)) self.assertFalse(pred0.test(1)) self.assertFalse(pred0.test(2)) pred1 = common_predicates.ExprPredicate("eq_expr_pred", e, attr_inds, [p1, p2], ["Can", "Sym"]) self.assertTrue(pred1.test(0)) self.assertFalse(pred1.test(1)) self.assertFalse(pred1.test(2)) pred2 = common_predicates.ExprPredicate("eq_expr_pred", e, attr_inds, [p1, p2], ["Can", "Sym"]) self.assertTrue(pred2.test(0)) self.assertFalse(pred2.test(1)) self.assertFalse(pred2.test(2))
def test_obstructs(self): #Obstructs, Robot, RobotPose, Can; radius = 1 attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Robot"], "name": ["robot"] } attr_types = { "geom": circle.GreenCircle, "pose": Vector2d, "_type": str, "name": str } robot = parameter.Object(attrs, attr_types) attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]} attr_types = {"value": Vector2d, "_type": str, "name": str} robotPose = parameter.Symbol(attrs, attr_types) attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Can"], "name": ["can1"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } can = parameter.Object(attrs, attr_types) env = Environment() pred = namo_predicates.Obstructs( "obstructs", [robot, robotPose, robotPose, can], ["Robot", "RobotPose", "RobotPose", "Can"], env) pred.expr.expr.grad(np.array([1.9, 0, 0, 0]), True, 1e-2) # val, jac = pred.distance_from_obj(np.array([1.9,0,0,0])) # self.assertTrue(np.allclose(np.array(val), .11, atol=1e-2)) # jac2 = np.array([[-0.95968306, -0., 0.95968306, 0.]]) # self.assertTrue(np.allclose(jac, jac2, atol=1e-2)) robot.pose = np.zeros((2, 4)) can.pose = np.array( [[2 * (radius + pred.dsafe), 0, 0, 2 * radius - pred.dsafe], [0, 2 * (radius + pred.dsafe), 0, 0]]) self.assertFalse(pred.test(time=0)) self.assertFalse(pred.test(time=1)) self.assertTrue(pred.test(time=2)) self.assertTrue(pred.test(time=3))
def test_at(self): # At, Can, Target can = ParamSetup.setup_blue_can() target = ParamSetup.setup_target() pred = robot_predicates.At("testpred", [can, target], ["Can", "Target"]) self.assertEqual(pred.get_type(), "At") # target is a symbol and doesn't have a value yet self.assertFalse(pred.test(time=0)) can.pose = np.array([[3, 3, 5, 6], [6, 6, 7, 8], [6, 6, 4, 2]]) can.rotation = np.zeros((3, 4)) target.value = np.array([[3, 4, 5, 7], [6, 5, 8, 7], [6, 3, 4, 2]]) self.assertTrue(pred.is_concrete()) # Test timesteps with self.assertRaises(PredicateException) as cm: pred.test(time=4) self.assertEqual( cm.exception.message, "Out of range time for predicate 'testpred: (At blue_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 blue_can target)'." ) # self.assertTrue(pred.test(time=0)) self.assertTrue(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 = robot_predicates.At("testpred", [can, sym], ["Can", "Target"]) self.assertEqual( cm.exception.message, "Parameter type validation failed for predicate 'testpred: (At blue_can sym)'." ) # Test rotation can.rotation = np.array([[1, 2, 3, 4], [2, 3, 4, 5], [3, 4, 5, 6]]) target.rotation = np.array([[2], [3], [4]]) self.assertFalse(pred.test(time=0)) self.assertTrue(pred.test(time=1)) self.assertFalse(pred.test(time=2)) self.assertFalse(pred.test(time=3))
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 setup_ee_pose(name="ee_pose"): attrs = { "name": [name], "value": ["undefined"], "rotation": [(0, 0, 0)], "_type": ["EEPose"] } attr_types = { "name": str, "value": matrix.Vector3d, "rotation": matrix.Vector3d, "_type": str } ee_pose = parameter.Symbol(attrs, attr_types) return ee_pose
def setup_target(name="target"): # This is the target parameter attrs = { "name": [name], "value": ["undefined"], "rotation": [(0, 0, 0)], "_type": ["Target"] } attr_types = { "name": str, "value": matrix.Vector3d, "rotation": matrix.Vector3d, "_type": str } target = parameter.Symbol(attrs, attr_types) return target
def test_grad(self): radius = 1 attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Robot"], "name": ["robot"] } attr_types = { "geom": circle.GreenCircle, "pose": Vector2d, "_type": str, "name": str } robot = parameter.Object(attrs, attr_types) attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]} attr_types = {"value": Vector2d, "_type": str, "name": str} robotPose = parameter.Symbol(attrs, attr_types) attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Can"], "name": ["can1"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } can = parameter.Object(attrs, attr_types) env = Environment() pred = namo_predicates.Obstructs( "obstructs", [robot, robotPose, robotPose, can], ["Robot", "RobotPose", "RobotPose", "Can"], env) pred.expr.expr.grad(np.array([1.9, 0, 0, 0]), True, 1e-2)
def setup_baxter_pose(name="baxter_pose"): attrs = { "name": [name], "value": [(0)], "_type": ["RobotPose"], "geom": [], "lGripper": [0.02], "rGripper": [0.02] } attrs["lArmPose"] = [(0, 0, 0, 0, 0, 0, 0)] attrs["rArmPose"] = [(0, 0, 0, 0, 0, 0, 0)] attr_types = { "name": str, "value": matrix.Value, "_type": str, "geom": robots.Baxter, "lArmPose": matrix.Vector7d, "rArmPose": matrix.Vector7d, "lGripper": matrix.Value, "rGripper": matrix.Value } robot = parameter.Symbol(attrs, attr_types) return robot
def test_grasp_valid(self): # GraspValid RobotPose Target Grasp radius = 1 attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]} attr_types = {"value": Vector2d, "_type": str, "name": str} robotPose = parameter.Symbol(attrs, attr_types) attrs = { "geom": [radius], "value": [(0, 0)], "_type": ["Target"], "name": ["can1"] } attr_types = { "geom": circle.BlueCircle, "value": Vector2d, "_type": str, "name": str } target = parameter.Symbol(attrs, attr_types) attrs = {"value": [(0, 0)], "_type": ["Grasp"], "name": ["grasp"]} attr_types = {"value": Vector2d, "_type": str, "name": str} grasp = parameter.Symbol(attrs, attr_types) pred = namo_predicates.GraspValid("GraspValid", [robotPose, target, grasp], ["RobotPose", "Target", "Grasp"]) #setting displacement to be <1, 2> grasp.value = np.array([[1], [2]]) #First test should fail because all objects's positions are in (0,0) self.assertFalse(pred.test(time=0)) #robotPose.value - target.pose = grasp.value rp_values = np.zeros((2, 4)) values = np.array([[-1, 1, 3, 5], [-2, 2, 4, 6]]) robotPose.value = rp_values[:, 0].reshape(2, 1) target.value = values[:, 0].reshape(2, 1) self.assertTrue(pred.test(0)) target.value = values[:, 1].reshape(2, 1) self.assertFalse(pred.test(0)) target.value = values[:, 2].reshape(2, 1) self.assertFalse(pred.test(0)) target.value = values[:, 3].reshape(2, 1) self.assertFalse(pred.test(0)) rp_values = np.array([[4, 2, 1, 6], [6, 4, 0, 8]]) robotPose.value = rp_values[:, 0].reshape(2, 1) target.value = values[:, 0].reshape(2, 1) self.assertFalse(pred.test(0)) robotPose.value = rp_values[:, 1].reshape(2, 1) target.value = values[:, 1].reshape(2, 1) self.assertTrue(pred.test(0)) robotPose.value = rp_values[:, 2].reshape(2, 1) target.value = values[:, 2].reshape(2, 1) self.assertFalse(pred.test(0)) robotPose.value = rp_values[:, 3].reshape(2, 1) target.value = values[:, 3].reshape(2, 1) self.assertTrue(pred.test(0))
def test_obstructs_holding(self): # ObstructsHolding, Robot, RobotPose, Can, Can; radius = 1 attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Robot"], "name": ["pr2"] } attr_types = { "geom": circle.GreenCircle, "pose": Vector2d, "_type": str, "name": str } robot = parameter.Object(attrs, attr_types) attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]} attr_types = {"value": Vector2d, "_type": str, "name": str} robotPose = parameter.Symbol(attrs, attr_types) attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Can"], "name": ["can1"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } can1 = parameter.Object(attrs, attr_types) attrs = { "geom": [radius], "pose": [(0, 2)], "_type": ["Can"], "name": ["can2"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } can2 = parameter.Object(attrs, attr_types) env = Environment() pred = namo_predicates.ObstructsHolding( "ObstructsHolding", [robot, robotPose, robotPose, can1, can2], ["Robot", "RobotPose", "RobotPose", "Can", "Can"], env) #First test should fail because all objects's positions are in (0,0) self.assertTrue(pred.test(time=0)) # This predicate has two expressions # pred.expr.expr.grad(np.array([1.9,0,0,0,0,0]), True, 1e-2) robot.pose = np.zeros((2, 4)) can1.pose = np.array( [[2 * (radius + pred.dsafe) + 0.1, 0, .1, 2 * radius - pred.dsafe], [0, 2 * (radius + pred.dsafe) + 0.1, 0, 0]]) can2.pose = np.zeros((2, 4)) self.assertFalse(pred.test(time=0)) self.assertFalse(pred.test(time=1)) self.assertTrue(pred.test(time=2)) self.assertTrue(pred.test(time=3)) pred2 = namo_predicates.ObstructsHolding( "obstruct holding2", [robot, robotPose, robotPose, can2, can2], ["Robot", "RobotPose", "RobotPose", "Can", "Can"], env) # since the object holding, object obstructing and robot are at the same position, can2 obstructs robot self.assertTrue(pred2.test(0)) can2.pose = np.array([[ 2 * radius, 2 * radius + pred2.dsafe, 2 * radius - pred2.dsafe, 3 * radius ], [0, 0, 0, 0]]) # At timestep 0, touching is allowed, so not obstruct self.assertFalse(pred2.test(time=0)) # At timestep 1, with a distance of dsafe is also allowed, also not obstruct self.assertFalse(pred2.test(time=1)) # At timestep 2, there is intersect distance of dsafe, it obstructs self.assertTrue(pred2.test(time=2)) # At timestep 4, objects are far away from robot, thus not obstructs self.assertFalse(pred2.test(time=3)) # Test whether negation is consistent self.assertTrue(pred2.test(time=0, negated=True)) self.assertTrue(pred2.test(time=1, negated=True)) self.assertFalse(pred2.test(time=2, negated=True)) self.assertTrue(pred2.test(time=3, negated=True))
def test_in_contact(self): # InContact, Robot, RobotPose, Target radius = 1 attrs = { "geom": [radius], "pose": [(0, 0)], "_type": ["Robot"], "name": ["pr2"] } attr_types = { "geom": circle.GreenCircle, "pose": Vector2d, "_type": str, "name": str } robot = parameter.Object(attrs, attr_types) attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]} attr_types = {"value": Vector2d, "_type": str, "name": str} robotPose = parameter.Symbol(attrs, attr_types) attrs = { "geom": [radius], "value": [(0, 0)], "_type": ["Target"], "name": ["target"] } attr_types = { "geom": circle.BlueCircle, "value": Vector2d, "_type": str, "name": str } target = parameter.Symbol(attrs, attr_types) env = Environment() pred = namo_predicates.InContact("InContact", [robot, robotPose, target], ["Robot", "RobotPose", "Target"], env=env) #First test should fail because all objects's positions are in (0,0) self.assertFalse(pred.test(time=0)) # Test Gradient for it pred.expr.expr.grad(np.array([1.9, 0, 0, 0]), True, 1e-2) val, jac = pred.distance_from_obj(np.array([1.9, 0, 0, 0])) # self.assertTrue(np.allclose(np.array(val), .11, atol=1e-2)) jac2 = np.array([[-0.95968306, -0., 0.95968306, 0.]]) # self.assertTrue(np.allclose(jac, jac2, atol=1e-2)) robotPose.value = np.zeros((2, 4)) target.value = np.array( [[2 * radius, radius, 2 * radius, 2 * radius - pred.dsafe, 0], [0, 0, 0, 0, 0]]) self.assertTrue(pred.test(time=0)) self.assertTrue(pred.test(time=1)) self.assertTrue(pred.test(time=2)) self.assertTrue(pred.test(time=3)) self.assertTrue(pred.test(time=4)) # since it symbol are assumed to be unchanged, test should always check distance with first traj vector robotPose.value = np.array([[ -pred.dsafe, 3 * radius + pred.dsafe, 0, -pred.dsafe, 2 * radius + pred.dsafe ], [0, 0, 0, 0, 0]]) self.assertFalse(pred.test(time=0)) self.assertFalse(pred.test(time=1)) self.assertFalse(pred.test(time=2)) self.assertFalse(pred.test(time=3)) self.assertFalse(pred.test(time=4))
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 test_robot_at(self): # RobotAt Robot RobotPose radius = 1 attrs = { "name": ["robot"], "geom": [radius], "pose": ["undefined"], "_type": ["Robot"] } attr_types = { "name": str, "geom": circle.RedCircle, "pose": Vector2d, "_type": str } p1 = parameter.Object(attrs, attr_types) attrs = { "name": ["r_pose"], "value": ["undefined"], "_type": ["RobotPose"] } attr_types = {"name": str, "value": Vector2d, "_type": str} p2 = parameter.Symbol(attrs, attr_types) pred = namo_predicates.RobotAt("testpred", [p1, p2], ["Robot", "RobotPose"]) self.assertEqual(pred.get_type(), "RobotAt") self.assertFalse(pred.test(time=400)) p1.pose = np.array([[3, 4, 5, 6], [6, 5, 7, 8]]) p2.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: (RobotAt robot r_pose)'." ) with self.assertRaises(PredicateException) as cm: pred.test(time=-1) self.assertEqual( cm.exception.message, "Out of range time for predicate 'testpred: (RobotAt robot r_pose)'." ) 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} p3 = parameter.Symbol(attrs, attr_types) with self.assertRaises(ParamValidationException) as cm: pred = namo_predicates.RobotAt("testpred", [p1, p3], ["Robot", "RobotPose"]) self.assertEqual( cm.exception.message, "Parameter type validation failed for predicate 'testpred: (RobotAt robot sym)'." ) pred = namo_predicates.RobotAt("testpred", [p1, p2], ["Robot", "RobotPose"]) self.assertTrue(pred.test(time=0)) self.assertFalse(pred.test(time=1))
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)