def move_pick_and_place_in_container(self):
# self.env = World().generate_room_env()
self.env = World().generate_box_env()
self.robot = self.env.GetRobots()[0]
self.num_actions = 4
# self.ro = 0.02
# self.ro = 0.2
self.ro = 2
# self.ro = 10
# self.ro = 20
# self.ro = 300
robot_init = HLParam("rp_init", 3, 1, is_var=False, value=mat_to_base_pose(self.robot.GetTransform()))
rp1 = HLParam("rp1", 3, 1, ro=self.ro)
rp2 = HLParam("rp2", 3, 1, ro=self.ro)
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()))
# target_loc = HLParam("target_loc", 3, 1, is_var=False, value=np.array((2,1,0)))
target_loc = HLParam("target_loc", 3, 1, is_var=False, value=np.array((2,1,0)))
envs, robots, objs = self.create_local_envs(num_actions=self.num_actions)
pick = self.add_hl_action(Pick(self, envs[1], robots[1], rp1, objs[1], obj_loc, gp))
place = self.add_hl_action(Place(self, envs[2], robots[2], rp2, objs[2], target_loc, gp))
# for initialization
rp1.dual_update()
rp2.dual_update()
gp.dual_update()
pick.plot()
place.plot()
import ipdb; ipdb.set_trace() # BREAKPOINT
move1 = self.add_hl_action(Move(self, envs[0], robots[0], "move1", robot_init, rp1))
move2 = self.add_hl_action(Move(self, envs[3], robots[3], "move2", rp1, rp2, objs[3], gp))
params = [rp1, rp2, gp]
llplan = LLPlan(params, self.hl_actions)
llplan.solve()
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 place_in_container(self):
self.env = World().generate_room_env()
self.robot = self.env.GetRobots()[0]
# self.ro = 0.02
# self.ro = 0.2
self.ro = 2
# self.ro = 10
# self.ro = 20
# self.ro = 300
# rp1 = HLParam("rp1", 3, 1, ro=self.ro)
rp1 = HLParam("rp1", 3, 1, is_var=False, value=np.array((0,0,0)))
# rp2 = HLParam("rp2", 3, 1, ro=self.ro, value=np.array((2,-1,0)))
rp2 = HLParam("rp2", 3, 1, ro=self.ro, value=np.array((1,2,0)))
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(obj.GetTransform()))
# target_loc = HLParam("target_loc", 3, 1, is_var=False, value=np.array((2,1,0)))
# target_loc = HLParam("target_loc", 3, 1, is_var=False, value=np.array((2,0.5,0)))
envs, robots, objs = self.create_local_envs(num_actions=2)
place = self.add_hl_action(Place(self, envs[0], robots[0], rp2, objs[0], obj_loc, gp))
# for initialization
# rp1.dual_update()
rp2.dual_update()
gp.dual_update()
import ipdb; ipdb.set_trace() # BREAKPOINT
place.plot()
move = self.add_hl_action(Move(self, envs[1], robots[1], rp1, rp2, objs[1], gp))
params = [rp2, gp]
llplan = LLPlan(params, self.hl_actions)
llplan.solve()
def test_pick_move_and_place(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 = 10
# self.ro = 20
# self.ro = 300
rp1 = HLParam("rp1", 3, 1, ro=self.ro)
rp2 = HLParam("rp2", 3, 1, ro=self.ro)
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()))
# target_loc = HLParam("target_loc", 3, 1, is_var=False, value=np.array((2,1,0)))
target_loc = HLParam("target_loc", 3, 1, is_var=False, value=np.array((2,0.5,0)))
envs, robots, objs = self.create_local_envs(num_actions=3)
pick = self.add_hl_action(Pick(self, envs[0], robots[0], rp1, objs[0], obj_loc, gp))
place = self.add_hl_action(Place(self, envs[1], robots[1], rp2, objs[1], target_loc, gp))
# for initialization
rp1.dual_update()
rp2.dual_update()
gp.dual_update()
pick.plot()
place.plot()
# import ipdb; ipdb.set_trace() # BREAKPOINT
move = self.add_hl_action(Move(self, envs[2], robots[2], rp1, rp2, objs[2], gp))
params = [rp1, rp2, gp]
llplan = LLPlan(params, self.hl_actions)
llplan.solve()
def pick_place_boxes(self):
# self.env = World().generate_room_env()
num_boxes = 1
self.env, target_loc_values = World().generate_boxes_env(num=num_boxes)
target_locs = []
i=0
for loc in target_loc_values:
target_locs.append(ObjLoc("target"+"_loc"+str(i), 3, 1, is_var=False, value=mat_to_base_pose(loc)))
i += 1
self.robot = self.env.GetRobots()[0]
self.num_actions = 4*len(target_locs)
# self.ro = 0.02
# self.ro = 0.2
self.ro = 2
# self.ro = 10
# self.ro = 20
# self.ro = 300
hla_envs = self.clone_envs(self.num_actions)
rps = []
gps = []
objs = []
obj_locs =[]
for i in range(num_boxes):
gps.append(GP("gp"+str(i), 3, 1, ro=self.ro))
obj = self.env.GetKinBody('obj'+str(i))
objs.append(obj)
obj_locs.append(ObjLoc("obj"+str(i)+"_loc", 3, 1, is_var=False, value=mat_to_base_pose(obj.GetTransform())))
robot_positions = 1 + 2*num_boxes
for i in range(robot_positions):
if i == 0:
rps.append(RP("rp_init", 3, 1, is_var=False, value=mat_to_base_pose(self.robot.GetTransform())))
else:
rps.append(RP("rp"+str(i), 3, 1, ro=self.ro))
rp_index=1
for i in range(num_boxes):
env = hla_envs[1+4*i]
robot = env.GetRobots()[0]
obj = env.GetKinBody(objs[i].GetName())
pick = self.add_hl_action(Pick(self, env, robot, rps[rp_index], obj, obj_locs[i], gps[i], name="pick"+str(i)))
env = hla_envs[2+4*i]
robot = env.GetRobots()[0]
obj = env.GetKinBody(objs[i].GetName())
place = self.add_hl_action(Place(self, env, robot, rps[rp_index+1], obj, target_locs[i], gps[i], name="place"+str(i)))
rp_index += 2
for rp in rps:
rp.dual_update()
for gp in gps:
gp.dual_update()
for hl_action in self.hl_actions:
hl_action.plot()
# import ipdb; ipdb.set_trace() # BREAKPOINT
for i in range(num_boxes):
env = hla_envs[4*i]
robot = env.GetRobots()[0]
move1 = self.add_hl_action(Move(self, env, robot, "move"+str(2*i+1), rps[2*i], rps[2*i+1]))
env = hla_envs[4*i+3]
robot = env.GetRobots()[0]
obj = env.GetKinBody(objs[i].GetName())
move2 = self.add_hl_action(Move(self, env, robot, "move"+str(2*i+2), rps[2*i+1], rps[2*i+2], obj, gps[i]))
params = rps + gps
llplan = LLPlan(params, self.hl_actions)
llplan.solve()