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
