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_place(): env = pr2_small_cans_test_env() # robot = PR2('pr2') # active_body_parts = ['rightarm', 'rgripper', 'torso', 'base'] # active_bodyparts = ['rightarm', 'rgripper', 'base'] active_bodyparts = ["rightarm", "rgripper"] robot = PR2("pr2", active_bodyparts) robot.tuck_arms(env) hl_plan = TestDomain(env) place_env = env.CloneSelf(1) # clones objects in the environment robot.init_for_env(env) robot.init_for_env(place_env) pose = robot.get_pose(env) robot_body = robot.get_env_body(env) hl_plan = HLPlan(env) obj = Obj("object7") K = robot.dofs # 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)) # pose = np.array([[-1.57265580e+00], [5.27875957e-01], [-1.40000000e+00], \ # [-1.58244363e+00], [-1.64143184e+00], [-9.76938766e-02], [-5.78403045e-01], [0.0],[0.],[0.],[0.]]) # pose = np.array([[-1.57265580e+00], [5.27875957e-01], [-1.40000000e+00], \ # [-1.58244363e+00], [-1.64143184e+00], [-9.76938766e-02], [-5.78403045e-01]]) pose = np.array( [ [-1.57265580e00], [5.27875957e-01], [-1.40000000e00], [-1.58244363e00], [-1.64143184e00], [-9.76938766e-02], [-5.78403045e-01], [0.4], ] ) start = HLParam("start", (K, 1), value=pose, is_var=False) end = HLParam("end", (K, 1), value=pose) loc = HLParam("loc", (3, 1), is_var=False, value=obj.get_pose(env)[3:6].reshape((3, 1))) # import ipdb; ipdb.set_trace() place = PR2Place(0, hl_plan, place_env, robot, start, end, obj, loc) hlas = [place] ll_prob = LLProb(hlas) # ll_prob.solve() import ipdb ipdb.set_trace() ll_prob.solve_at_priority(-1) ll_prob.solve_at_priority(1)
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 optimize(pr, recently_sampled, all_sampled_params, first_prio): llprob = LLProb(pr.action_list) llprob.solve_at_priority_regular(priority=first_prio, recently_sampled=recently_sampled) # llprob.solve_at_priority(priority=-1, recently_sampled=recently_sampled) # llprob.solve_at_priority(priority=0, recently_sampled=recently_sampled) llprob.solve_at_priority(priority=2) if llprob.recently_converged_vio_fluent is not None: # early converged based on a violated fluent violated_fluents = [llprob.recently_converged_vio_fluent] else: fluents = [f for a in pr.action_list for f in a.preconditions + a.postconditions] violated_fluents = pr.find_violated_fluents(fluents, 2) if not violated_fluents: print "Success" print llprob.traj_cost pr.execute(pause=False) while True: pr.execute(pause=False) return None else: print "Failed" print llprob.traj_cost recently_sampled = pr.randomized_resample(all_sampled_params, violated_fluents, mode="fluent", count=0) return recently_sampled
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()