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_pick_and_move():
env = pick_test_env()
# start = HLParam("start", (3, 1), is_var=False, value=np.array([[-2], [0], [0]]))
# end = HLParam("end", (3, 1), is_var=False, value=np.array([[2], [0], [0]]))
hl_plan = TestDomain(env)
pick_env = env.CloneSelf(1) # clones objects in the environment
move_env = env.CloneSelf(1) # clones objects in the environment
robot = Robot(env.GetRobots()[0].GetName())
rp = HLParam("rp", (3, 1))
end = HLParam("end", (3, 1), is_var=False, value=np.array([[2],[0],[0]]))
gp = GP("gp", (3, 1), is_resampled=True)
obj = Obj("obj")
obj_loc = HLParam("obj_loc", (3, 1), is_var=False, value=obj.get_pose(env))
gp.resample()
pick = Pick(0, hl_plan, pick_env, robot, rp, obj, obj_loc, gp)
move = Move(0, hl_plan, move_env, robot, rp, end, obj, gp)
hlas = [pick, move]
ll_prob = LLProb(hlas)
ll_prob.solve()
ipdb.set_trace()
def test_pick_move_and_place():
env = pick_test_env()
robot = Robot(env.GetRobots()[0].GetName())
hl_plan = TestDomain(env)
place_env = env.CloneSelf(1) # clones objects in the environment
pick_env = env.CloneSelf(1) # clones objects in the environment
move_env = env.CloneSelf(1) # clones objects in the environment
rp1 = HLParam("rp1", (3, 1))
rp2 = HLParam("rp2", (3, 1))
gp = GP("gp", (3, 1), is_resampled=True)
obj = Obj("obj")
obj_loc = HLParam("obj_loc", (3, 1), is_var=False, value=obj.get_pose(env))
target_loc = HLParam("target_loc", (3, 1), is_var=False, value=np.array([[2],[0.5],[0]]))
gp.resample()
pick = Pick(0, hl_plan, pick_env, robot, rp1, obj, obj_loc, gp)
move = Move(0, hl_plan, move_env, robot, rp1, rp2, obj, gp)
import ipdb; ipdb.set_trace()
place = Place(0, hl_plan, place_env, robot, rp2, obj, target_loc, gp)
hlas = [pick, move, place]
ll_prob = LLProb(hlas)
ll_prob.solve()
def test_pick():
env = pick_test_env()
hl_plan = TestDomain(env)
pick_env = env.CloneSelf(1) # clones objects in the environment
robot = Robot(env.GetRobots()[0].GetName())
rp = HLParam("rp", (3, 1))
# collision checking returns -1 * normal when objects are touching
gp_val = np.array([[0], [0.56], [0]], dtype=np.float)
# gp_val = np.array([[0], [0.55], [0]], dtype=np.float)
# gp_val = np.array([[0], [0.54], [0]], dtype=np.float)
# gp_val = np.array([[0], [0.552], [0]], dtype=np.float)
gp = GP("gp", (3, 1), value=gp_val, is_resampled=True)
obj = Obj("obj")
obj_loc = HLParam("obj_loc", (3, 1), is_var=False, value=obj.get_pose(env))
# gp.resample()
pick = Pick(0, hl_plan, pick_env, robot, rp, obj, obj_loc, gp)
hlas = [pick]
ll_prob = LLProb(hlas)
ll_prob.solve()
assert np.allclose(pick.pos.value, np.array([[-2.0],[-0.61],[ 0.]]))
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)