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)
