def test_pick_and_move(self): self.env = self.init_openrave_test_env() self.robot = self.env.GetRobots()[0] # self.ro = 0.02 # self.ro = 0.2 # self.ro = 2 self.ro = 20 rp = HLParam("rp", 3, 1, ro=self.ro) end = HLParam("end", 3, 1, is_var=False, value=np.array((2,0,0))) pick_obj = self.env.GetKinBody('obj') gp = HLParam("gp", 3, 1, ro=self.ro) obj_loc = HLParam("obj_loc", 3, 1, is_var=False, value=mat_to_base_pose(pick_obj.GetTransform())) env = self.env.CloneSelf(1) # clones objects in the environment robot = env.GetRobots()[0] obj = env.GetKinBody('obj') pick = Pick(self, env, robot, rp, obj, obj_loc, gp) # must clone env, robot and obj before solve env = self.env.CloneSelf(1) # clones objects in the environment robot = env.GetRobots()[0] obj = env.GetKinBody('obj') self.add_hl_action(pick) pick.solve_opt_prob() rp.dual_update() gp.dual_update() move = Move(self, env, robot, rp, end, obj, gp) self.add_hl_action(move) params = [rp, gp] llplan = LLPlan(params, self.hl_actions) llplan.solve()
def test_pick(self): self.env = self.init_openrave_test_env() self.robot = self.env.GetRobots()[0] # self.ro = 0.02 # self.ro = 0.2 # self.ro = 2 self.ro = 20 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))) rp = HLParam("rp", 3, 1, ro=self.ro) # end = HLParam("end", 3, 1, is_var=False, value=np.array((2,0,0))) pick_obj = self.env.GetKinBody('obj') gp = HLParam("gp", 3, 1, ro=self.ro) obj_loc = HLParam("obj_loc", 3, 1, is_var=False, value=mat_to_base_pose(pick_obj.GetTransform())) env = self.env.CloneSelf(1) # clones objects in the environment robot = env.GetRobots()[0] obj = env.GetKinBody('obj') pick = Pick(self, env, robot, rp, obj, obj_loc, gp) self.add_hl_action(pick) pick.solve_opt_prob() import ipdb; ipdb.set_trace() # BREAKPOINT