def test_object(self): attrs = {"circ": [1], "test": [3.7], "test2": [5.3], "test3": [6.5], "pose": ["undefined"], "_type": ["Can"]} attr_types = {"test": float, "test3": str, "test2": int, "circ": circle.BlueCircle, "pose": matrix.Vector2d, "_type": str} with self.assertRaises(AssertionError) as cm: parameter.Object(attrs, attr_types) attrs["name"] = ["param"] with self.assertRaises(DomainConfigException) as cm: parameter.Object(attrs, attr_types) self.assertEqual(cm.exception.message, "Attribute 'name' for Object 'param' not defined in domain file.") attr_types["name"] = str param = parameter.Object(attrs, attr_types) self.assertEqual(param.name, "param") self.assertEqual(param.get_type(), "Can") self.assertFalse(param.is_symbol()) self.assertFalse(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 = matrix.Vector2d([2, 3]) self.assertTrue(param.is_defined()) self.assertEqual(param.pose.shape, (2, 1)) # 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_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_openraveviewer_draw_traj(self): attrs = { "geom": [1], "pose": [(3, 5)], "_type": ["Can"], "name": ["can0"] } attr_types = { "geom": circle.GreenCircle, "pose": matrix.Vector2d, "_type": str, "name": str } green_can = parameter.Object(attrs, attr_types) attrs = { "geom": [1], "pose": [(2, 1)], "_type": ["Can"], "name": ["can1"] } attr_types = { "geom": circle.BlueCircle, "pose": matrix.Vector2d, "_type": str, "name": str } blue_can = parameter.Object(attrs, attr_types) green_can.pose = np.array([[1, 2, 3, 4, 5], [3, 4, 5, 6, 7]]) blue_can.pose = np.array([[3, 4, 5, 6, 7], [1, 2, 3, 4, 5]]) """
def test_stationary_neq(self): # StationaryNEq, Can, Can attrs = { "geom": [1], "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": [1], "pose": [(0, 0)], "_type": ["Can"], "name": ["can2"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } can2 = parameter.Object(attrs, attr_types) pred1 = namo_predicates.StationaryNEq("test_stay_neq1", [can1, can1], ["Can", "Can"]) with self.assertRaises(PredicateException) as cm: pred1.test(time=0) self.assertEqual( cm.exception.message, "Insufficient pose trajectory to check dynamic predicate 'test_stay_neq1: (StationaryNEq can1 can1)' at the timestep." ) can1.pose = np.array([[1, 2, 3], [1, 2, 3]]) # Since two objects in this predicate are the same one, test should always pass self.assertTrue(pred1.test(time=0)) self.assertTrue(pred1.test(time=1)) pred2 = namo_predicates.StationaryNEq("test_stay_neq2", [can1, can2], ["Can", "Can"]) can2.pose = np.array([[1, 1, 1], [1, 1, 1]]) # Now that can1 is not can2, so it will check whether first can is stationary self.assertFalse(pred2.test(time=0)) self.assertFalse(pred2.test(time=1)) can1.pose = np.array([[2, 2, 3], [2, 2, 3]]) can2.pose = np.array([[1], [1]]) # no matter what kind of pose can2 has, it only checks whether first can is stationary self.assertTrue(pred2.test(time=0)) self.assertFalse(pred2.test(time=1))
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_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 setup_robot(self, name="pr2"): attrs = { "name": [name], "pose": [(0, 0, 0)], "_type": ["Robot"], "geom": [], "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, "pose": matrix.Vector3d, "_type": str, "geom": PR2, "backHeight": matrix.Value, "lArmPose": matrix.Vector7d, "rArmPose": matrix.Vector7d, "lGripper": matrix.Value, "rGripper": matrix.Value } robot = parameter.Object(attrs, attr_types) # Set the initial arm pose so that pose is not close to joint limit robot.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 robot.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 robot
def test_add_obstacle(self): attrs = { "geom": [], "pose": [(3, 5)], "_type": ["Obstacle"], "name": ["obstacle"] } attr_types = { "geom": Obstacle, "pose": Vector2d, "_type": str, "name": str } obstacle = parameter.Object(attrs, attr_types) env = Environment() """ to ensure the _add_obstacle is working, uncomment the line below to make sure the obstacle is being created """ # env.SetViewer('qtcoin') obstacle_body = OpenRAVEBody(env, obstacle.name, obstacle.geom) obstacle_body.set_pose([2, 0]) arr = np.eye(4) arr[0, 3] = 2 self.assertTrue(np.allclose(obstacle_body.env_body.GetTransform(), arr))
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_robot(self, name="pr2"): attrs = { "name": [name], "pose": [(0, 0, 0)], "_type": ["Robot"], "geom": [], "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, "pose": Vector3d, "_type": str, "geom": PR2, "backHeight": Value, "lArmPose": Vector7d, "rArmPose": Vector7d, "lGripper": Value, "rGripper": Value } robot = parameter.Object(attrs, attr_types) # Set the initial arm pose so that pose is not close to joint limit R_ARM_INIT = [-1.832, -0.332, -1.011, -1.437, -1.1, 0, -3.074] L_ARM_INIT = [0.06, 1.25, 1.79, -1.68, -1.73, -0.10, -0.09] robot.rArmPose = np.array(R_ARM_INIT).reshape((7, 1)) robot.lArmPose = np.array(L_ARM_INIT).reshape((7, 1)) return robot
def test_add_circle(self): attrs = { "geom": [1], "pose": [(3, 5)], "_type": ["Can"], "name": ["can0"] } attr_types = { "geom": circle.GreenCircle, "pose": Vector2d, "_type": str, "name": str } green_can = parameter.Object(attrs, attr_types) attrs = { "geom": [1], "pose": [(3, 5)], "_type": ["Can"], "name": ["can0"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } blue_can = parameter.Object(attrs, attr_types) env = Environment() """ to ensure the _add_circle and create_cylinder is working, uncomment the line below to make sure cylinders are being created """ # env.SetViewer('qtcoin') green_body = OpenRAVEBody(env, "can0", green_can.geom) blue_body = OpenRAVEBody(env, "can1", blue_can.geom) green_body.set_pose([2, 0]) arr = np.eye(4) arr[0, 3] = 2 self.assertTrue(np.allclose(green_body.env_body.GetTransform(), arr)) blue_body.set_pose(np.array([0, -1])) arr = np.eye(4) arr[1, 3] = -1 self.assertTrue(np.allclose(blue_body.env_body.GetTransform(), arr))
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_collides(self): env = Environment() attrs = { "geom": [1], "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 = { "geom": ["closet"], "pose": [(0, 0)], "_type": ["Obstacle"], "name": ["wall"] } attr_types = { "geom": wall.Wall, "pose": Vector2d, "_type": str, "name": str } border = parameter.Object(attrs, attr_types) pred = namo_predicates.Collides("collides", [can, border], ["Can", "Obstacle"], env) self.assertTrue(pred.test(0)) border.pose = np.zeros((2, 4)) can.pose = np.array( [[1 + pred.dsafe, 1 + 2 * pred.dsafe, 1, 1 - pred.dsafe], [0, 0, 0, 0]]) self.assertTrue(pred.test(0)) self.assertFalse(pred.test(1)) self.assertTrue(pred.test(2)) self.assertTrue(pred.test(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 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_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(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_copy_object(self): attrs = {"name": ["param"], "circ": [1], "test": [3.7], "test2": [5.3], "test3": [6.5], "pose": [[[3, 4, 5, 0], [6, 2, 1, 5], [1, 1, 1, 1]]], "rotation": [[[1, 2, 3],[4, 5, 6]]], "_type": ["Can"]} attr_types = {"name": str, "test": float, "test3": str, "test2": int, "circ": circle.BlueCircle, "pose": matrix.Vector3d, "rotation": matrix.Vector2d, "_type": str} p = parameter.Object(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.pose, [[3, 4, 5, 0, np.NaN, np.NaN, np.NaN], [6, 2, 1, 5, np.NaN, np.NaN, np.NaN], [1, 1, 1, 1, np.NaN, np.NaN, np.NaN]], equal_nan=True)) self.assertTrue(np.allclose(p2.rotation, [[1, 2, 3, np.NaN, np.NaN, np.NaN, np.NaN], [4, 5, 6, np.NaN, np.NaN, np.NaN, np.NaN]], equal_nan=True)) p2 = p.copy(new_horizon=2) self.assertTrue(np.array_equal(p2.pose, [[3, 4], [6, 2], [1, 1]])) self.assertTrue(np.array_equal(p2.rotation, [[1, 2], [4, 5]])) attrs["pose"] = "undefined" attrs["rotation"] = "undefined" p = parameter.Object(attrs, attr_types) p2 = p.copy(new_horizon=7) self.assertEqual(p2.name, "param") self.assertEqual(p2.test, 3.7) new_pose = np.empty((3, 7)) new_pose[:] = np.NaN self.assertTrue(np.allclose(p2.pose, new_pose, equal_nan=True)) new_pose = np.empty((2, 7)) new_pose[:] = np.NaN self.assertTrue(np.allclose(p2.rotation, new_pose, equal_nan=True))
def setup_red_can(name="red_can", geom=(0.02, 0.25)): attrs = { "name": [name], "geom": geom, "pose": ["undefined"], "rotation": [(0, 0, 0)], "_type": ["Can"] } attr_types = { "name": str, "geom": can.RedCan, "pose": matrix.Vector3d, "rotation": matrix.Vector3d, "_type": str } can_obj = parameter.Object(attrs, attr_types) return can_obj
def setup_obstacle(self, name="table"): attrs = { "name": [name], "geom": [[1.5, 0.94, 0.15, .2, 0.2, 0.6, False]], "pose": ["undefined"], "rotation": [(0, 0, 0)], "_type": ["Table"] } attr_types = { "name": str, "geom": Table, "pose": matrix.Vector3d, "rotation": matrix.Vector3d, "_type": str } table = parameter.Object(attrs, attr_types) return table
def setup_box(self, name="box"): attrs = { "name": [name], "geom": [[1, .5, .5]], "pose": ["undefined"], "rotation": [(0, 0, 0)], "_type": ["Table"] } attr_types = { "name": str, "geom": Box, "pose": matrix.Vector3d, "rotation": matrix.Vector3d, "_type": str } box = parameter.Object(attrs, attr_types) return box
def setup_can(self, name="can"): attrs = { "name": [name], "geom": (0.04, 0.25), "pose": ["undefined"], "rotation": [(0, 0, 0)], "_type": ["Can"] } attr_types = { "name": str, "geom": BlueCan, "pose": matrix.Vector3d, "rotation": matrix.Vector3d, "_type": str } can = parameter.Object(attrs, attr_types) return can
def test_exception(self): env = Environment() attrs = { "geom": [], "pose": [(3, 5)], "_type": ["Can"], "name": ["can0"] } attr_types = { "geom": DummyGeom, "pose": Vector2d, "_type": str, "name": str } green_can = parameter.Object(attrs, attr_types) with self.assertRaises(OpenRAVEException) as cm: green_body = OpenRAVEBody(env, "can0", green_can.geom) self.assertTrue("Geometry not supported" in cm.exception.message)
def test_is_mp(self): # IsMP Robot attrs = { "geom": [1], "pose": [(0, 0)], "_type": ["Robot"], "name": ["pr2"] } attr_types = { "geom": circle.GreenCircle, "pose": Vector2d, "_type": str, "name": str } robot = parameter.Object(attrs, attr_types) pred = namo_predicates.IsMP("IsMP", [robot], ["Robot"], dmove=1.) #predicate only have 1 timestep with self.assertRaises(PredicateException) as cm: pred.test(time=0) self.assertEqual( cm.exception.message, "Insufficient pose trajectory to check dynamic predicate 'IsMP: (IsMP pr2)' at the timestep." ) robot.pose = np.array([[1, 2, 8, 4], [0, 1, -4, 9]]) self.assertTrue([pred.test(time=0)]) self.assertFalse(pred.test(time=1)) self.assertFalse(pred.test(time=2)) robot.pose = np.array([[1, 2, 3, 4], [5, 6, 7, 8]]) self.assertTrue(pred.test(time=0)) self.assertTrue(pred.test(time=1)) self.assertTrue(pred.test(time=2)) with self.assertRaises(PredicateException) as cm: pred.test(time=3) self.assertEqual( cm.exception.message, "Insufficient pose trajectory to check dynamic predicate 'IsMP: (IsMP pr2)' at the timestep." )
def setup_baxter(name="baxter"): attrs = { "name": [name], "pose": [(0)], "_type": ["Robot"], "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, "pose": matrix.Value, "_type": str, "geom": robots.Baxter, "lArmPose": matrix.Vector7d, "rArmPose": matrix.Vector7d, "lGripper": matrix.Value, "rGripper": matrix.Value } robot = parameter.Object(attrs, attr_types) return robot
def test_stationary_w(self): attrs = { "geom": ["closet"], "pose": [(0, 0)], "_type": ["Obstacle"], "name": ["wall"] } attr_types = { "geom": wall.Wall, "pose": Vector2d, "_type": str, "name": str } border = parameter.Object(attrs, attr_types) pred = namo_predicates.StationaryW("test_StationaryW", [border], ["Obstacle"]) with self.assertRaises(PredicateException) as cm: pred.test(time=0) self.assertEqual( cm.exception.message, "Insufficient pose trajectory to check dynamic predicate 'test_StationaryW: (StationaryW wall)' at the timestep." ) border.pose = np.array([[1, 2], [4, 4]]) self.assertFalse(pred.test(time=0)) border.pose = np.array([[1, 1, 2], [2, 2, 2]]) self.assertTrue(pred.test(time=0)) self.assertFalse(pred.test(time=1)) with self.assertRaises(PredicateException) as cm: pred.test(time=2) self.assertEqual( cm.exception.message, "Insufficient pose trajectory to check dynamic predicate 'test_StationaryW: (StationaryW wall)' at the timestep." )
def test_stationary(self): # Stationary, Can attrs = { "geom": [1], "pose": [(0, 0)], "_type": ["Can"], "name": ["can"] } attr_types = { "geom": circle.BlueCircle, "pose": Vector2d, "_type": str, "name": str } can = parameter.Object(attrs, attr_types) pred = namo_predicates.Stationary("test_stay", [can], ["Can"]) with self.assertRaises(PredicateException) as cm: pred.test(time=0) self.assertEqual( cm.exception.message, "Insufficient pose trajectory to check dynamic predicate 'test_stay: (Stationary can)' at the timestep." ) can.pose = np.array([[1, 2], [4, 4]]) self.assertFalse(pred.test(time=0)) can.pose = np.array([[1, 1, 2], [2, 2, 2]]) self.assertTrue(pred.test(time=0)) self.assertFalse(pred.test(time=1)) with self.assertRaises(PredicateException) as cm: pred.test(time=2) self.assertEqual( cm.exception.message, "Insufficient pose trajectory to check dynamic predicate 'test_stay: (Stationary can)' at the timestep." )
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))