Beispiel #1
0
    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()
Beispiel #2
0
    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