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))
