def test_is_gp():
env = is_gp_test_env()
hl_plan = HLPlan(env)
robot = Robot(env.GetRobots()[0].GetName())
robot_pose = np.array([[0.1],[0.],[0.]])
robot.set_pose(env, robot_pose)
# obj has a radius of .35
obj = Obj('obj')
pose = np.array([[0.],[0.],[0.]])
obj.set_pose(env, pose)
pos = HLParam("pos", (3, 1), is_var=False, value=robot.get_pose(env))
obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
gp = HLParam("gp", (3, 1), is_var=False, value=pose)
action = TestIsGPAction(hl_plan, env, robot, obj, gp, pos, obj_pos)
fluent = action.precondition
fluent.pre()
raw_input('')
poses = [np.array([[0.7],[0.],[0.]]), \
np.array([[0.65],[0.],[0.]]), \
np.array([[0.55],[0.],[0.]]), \
np.array([[0.45],[0.],[0.]])]
values = [0., -0.05, 0.05, 0.15]
grads = [np.array([[0., 0., 0.]]),
np.array([[-1.0 ,0., 0.]]), \
np.array([[-1.0 ,0., 0.]]), \
np.array([[-1.0 ,0., 0.]])]
for i in range(len(poses)):
val, grad = fluent.distance_from_obj(poses[i], 0.05, (3,1))
print 'val: ', val
print 'grad: ', grad
import ipdb; ipdb.set_trace()
def test_not_obstructs_btn_two_cylinders():
env = not_obstructs_test_env()
hl_plan = HLPlan(env)
robot = Robot(env.GetRobots()[0].GetName())
robot_pose = np.array([[0.1],[0.],[0.]])
robot.set_pose(env, robot_pose)
# obj has a radius of .35
obj = Obj('obj')
pose = np.array([[0.],[0.],[0.]])
obj.set_pose(env, pose)
pos = HLParam("pos", (3, 1), is_var=False, value=robot.get_pose(env))
obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
action = TestAction(hl_plan, env, robot, pos, obj)
fluent = action.precondition
fluent.pre()
raw_input('')
poses = [np.array([[0.7],[0.],[0.]]), \
np.array([[0.65],[0.],[0.]]), \
np.array([[0.55],[0.],[0.]]), \
np.array([[0.45],[0.],[0.]])]
values = [0., -0.05, 0.05, 0.15]
grads = [np.array([[0., 0., 0.]]),
np.array([[-1.0 ,0., 0.]]), \
np.array([[-1.0 ,0., 0.]]), \
np.array([[-1.0 ,0., 0.]])]
for i in range(len(poses)):
val, grad = fluent.collisions(poses[i])
print 'val: ', val
print 'grad: ', grad
assert np.allclose(values[i], val, atol=2e-2)
assert np.allclose(grads[i], grad, atol=2e-2)
