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():
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_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_move():
env = move_test_env()
hl_plan = TestDomain(env)
move_env = env.CloneSelf(1) # clones objects in the environment
robot = Robot(env.GetRobots()[0].GetName())
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]]))
move = Move(0, hl_plan, move_env, robot, start, end)
hlas = [move]
ll_prob = LLProb(hlas)
ll_prob.solve()
import ipdb; ipdb.set_trace()
def test_move_pr2_gradient():
env = cans_world_env()
obj17 = env.GetKinBody('object17')
tran = obj17.GetTransform()
tran[0,3] = .49268
tran[1,3] = -.51415
obj17.SetTransform(tran)
hl_plan = TestDomain(env)
move_env = env.CloneSelf(1) # clones objects in the environment
move_pr2 = move_env.GetKinBody('pr2')
move_pr2.SetActiveDOFs(move_pr2.GetManipulator('rightarm').GetArmIndices())
move_pr2.SetDOFValues([0.3],
[move_pr2.GetJoint("torso_lift_joint").GetDOFIndex()])
robot = PR2('pr2')
robot.tuck_arms(env)
robot._set_active_dofs(env, ['rightarm'])
pose = robot.get_pose(env)
hl_plan = HLPlan(env)
obj = Obj('object17')
K = 7
# obj_pose = np.array([[0.],[0.],[0.]])
# obj.set_pose(env, obj_pose)
# start = HLParam("pos", (self.K, 1), is_var=False, value=robot.get_pose(env))
start_pose = np.array([[-1.57265580e+00], [5.27875957e-01], [-1.40000000e+00], \
[-1.58244363e+00], [-1.64143184e+00], [-9.76938766e-02], [-5.78403045e-01]])
end_pose = np.array([[-1.31066711e+00], [1.08996543e+00], [-1.40000000e+00],\
[-2.10243169e+00], [2.93131497e+00], [-6.18669215e-01], [5.20253411e-01]])
start = HLParam("start", (K, 1), is_var=False, value=start_pose)
end = HLParam("end", (K, 1), is_var=False, value=end_pose)
obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
move = PR2Move(0, hl_plan, move_env, robot, start, end, obj)
hlas = [move]
ll_prob = LLProb(hlas)
ll_prob.solve()
import ipdb; ipdb.set_trace()
def test_move_pr2_with_obj_wrt_obj():
env = cans_world_env()
obj17 = env.GetKinBody('object17')
tran = obj17.GetTransform()
tran[0,3] = .49268
tran[1,3] = -.51415
obj17.SetTransform(tran)
hl_plan = TestDomain(env)
move_env = env.CloneSelf(1) # clones objects in the environment
move_pr2 = move_env.GetKinBody('pr2')
move_pr2.SetActiveDOFs(move_pr2.GetManipulator('rightarm').GetArmIndices())
move_pr2.SetDOFValues([0.3],
[move_pr2.GetJoint("torso_lift_joint").GetDOFIndex()])
robot = PR2('pr2')
robot.tuck_arms(env)
robot._set_active_dofs(env, ['rightarm'])
pose = robot.get_pose(env)
hl_plan = HLPlan(env)
obj = Obj('object17')
K = 7
# obj_pose = np.array([[0.],[0.],[0.]])
# obj.set_pose(env, obj_pose)
# start = HLParam("pos", (self.K, 1), is_var=False, value=robot.get_pose(env))
start_pose = np.array([[-1.4352011 ],\
[ 0.63522797],\
[-1.19804084],\
[-1.55807855],\
[-0.9962505 ],\
[-0.75279673],\
[-1.13988334]])
end_pose = np.array([[-1.28868338],\
[ 0.53921863],\
[-1.41293145],\
[-1.67185359],\
[-0.74117023],\
[-0.73603889],\
[-1.29004773]])
robot.set_pose(env, start_pose)
# traj_val = lininterp(start_pose, end_pose, 10)
traj_val = lininterp(start_pose, end_pose, 2)
# getting grasp pose
robot_body = robot.get_env_body(env)
rarm = robot_body.GetManipulator('rightarm')
# import ipdb; ipdb.set_trace()
W_T_EE = rarm.GetEndEffectorTransform()
EE_T_W = np.linalg.inv(W_T_EE)
W_T_O = obj17.GetTransform()
EE_T_O = np.dot(EE_T_W, W_T_O)
EE_T_O_pose = poseFromMatrix(EE_T_O)
# gp = HLParam("gp", (7, 1), is_var=False, value=EE_T_O_pose)
gp_val = np.array([0,0,.125]).reshape((3,1))
gp = HLParam("gp", (3, 1), is_var=False, value=gp_val)
# start_pose = np.array([[-1.07265580e+00], [5.17875957e-01], [-1.30000000e+00], \
# [-1.08244363e+00], [-1.04143184e+00], [-9.06938766e-02], [-5.38403045e-01]])
# obj trajectory to follow
# obj_start = poseFromMatrix(tran).reshape((7,1))
obj_start = obj_pose_from_transform(tran).reshape((6,1))
end_tran = tran.copy()
end_tran[1, 3] = -0.34
obj_end = obj_pose_from_transform(end_tran).reshape((6,1))
# print "obj_start:", obj_start
# print "obj_end:", obj_end
# obj_traj = lininterp(obj_start, obj_end, 10)
obj_traj = lininterp(obj_start, obj_end, 2)
start = HLParam("start", (K, 1), is_var=False, value=start_pose)
end = HLParam("end", (K, 1), is_var=False, value=end_pose)
# obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
move = PR2ObjMove(0, hl_plan, move_env, robot, start, end, traj_val, obj, gp)
hlas = [move]
ll_prob = LLProb(hlas)
ll_prob.solve()
import ipdb; ipdb.set_trace()
