def make_environment(pidx):
np.random.seed(pidx)
random.seed(pidx)
problem_env = PaPOneArmMoverEnv(pidx)
goal = ['rectangular_packing_box1_region'
] + [obj.GetName() for obj in problem_env.objects[:1]]
problem_env.set_goal(goal)
return problem_env
def get_problem_env(config, goal_region, goal_objs):
np.random.seed(config.pidx)
random.seed(config.pidx)
if config.domain == 'two_arm_mover':
problem_env = PaPMoverEnv(config.pidx)
[utils.set_color(o, [0.8, 0, 0]) for o in goal_objs]
problem_env.set_goal(goal_objs, goal_region)
elif config.domain == 'one_arm_mover':
problem_env = PaPOneArmMoverEnv(config.pidx)
[utils.set_color(obj, [0.0, 0.0, 0.7]) for obj in problem_env.objects]
[utils.set_color(o, [1.0, 1.0, 0]) for o in goal_objs]
problem_env.set_goal(goal_objs, goal_region)
else:
raise NotImplementedError
return problem_env
def get_problem_env(config):
n_objs_pack = config.n_objs_pack
if config.domain == 'two_arm_mover':
problem_env = PaPMoverEnv(config.pidx)
goal = ['home_region'] + [
obj.GetName() for obj in problem_env.objects[:n_objs_pack]
]
problem_env.set_goal(goal)
elif config.domain == 'one_arm_mover':
problem_env = PaPOneArmMoverEnv(config.pidx)
goal = ['rectangular_packing_box1_region'] + [
obj.GetName() for obj in problem_env.objects[:n_objs_pack]
]
problem_env.set_goal(goal)
else:
raise NotImplementedError
return problem_env
def get_problem_env(config, goal_region, goal_objs):
n_objs_pack = config.n_objs_pack
if config.domain == 'two_arm_mover':
problem_env = PaPMoverEnv(config.pidx)
# goal = ['home_region'] + [obj.GetName() for obj in problem_env.objects[:n_objs_pack]]
# for obj in problem_env.objects[:n_objs_pack]:
# utils.set_color(obj, [0, 1, 0])
[utils.set_color(o, [0, 0, 0.8]) for o in goal_objs]
# goal = ['home_region'] + ['rectangular_packing_box1', 'rectangular_packing_box2', 'rectangular_packing_box3',
# 'rectangular_packing_box4']
problem_env.set_goal(goal_objs, goal_region)
elif config.domain == 'one_arm_mover':
problem_env = PaPOneArmMoverEnv(config.pidx)
goal = ['rectangular_packing_box1_region'] + [obj.GetName() for obj in problem_env.objects[:n_objs_pack]]
problem_env.set_goal(goal)
else:
raise NotImplementedError
return problem_env
def main():
problem_idx = 0
env_name = 'two_arm_mover'
if env_name == 'one_arm_mover':
problem_env = PaPOneArmMoverEnv(problem_idx)
goal = ['rectangular_packing_box1_region'
] + [obj.GetName() for obj in problem_env.objects[:3]]
state_fname = 'one_arm_state_gpredicate.pkl'
else:
problem_env = PaPMoverEnv(problem_idx)
goal_objs = [
'square_packing_box1', 'square_packing_box2',
'rectangular_packing_box3', 'rectangular_packing_box4'
]
goal_region = 'home_region'
goal = [goal_region] + goal_objs
state_fname = 'two_arm_state_gpredicate.pkl'
problem_env.set_goal(goal)
if os.path.isfile(state_fname):
state = pickle.load(open(state_fname, 'r'))
else:
statecls = helper.get_state_class(env_name)
state = statecls(problem_env, problem_env.goal)
utils.viewer()
if env_name == 'one_arm_mover':
obj_name = 'c_obst9'
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={
'object': problem_env.env.GetKinBody(obj_name)
},
continuous_parameters=state.pick_params[obj_name][0])
pick_op.execute()
problem_env.env.Remove(problem_env.env.GetKinBody('computer_table'))
utils.set_color(obj_name, [0.9, 0.8, 0.0])
utils.set_color('c_obst0', [0, 0, 0.8])
utils.set_obj_xytheta(
np.array([[4.47789478, -0.01477591, 4.76236795]]), 'c_obst0')
T_viewer = np.array(
[[-0.69618481, -0.41674492, 0.58450867, 3.62774134],
[-0.71319601, 0.30884202, -0.62925993, 0.39102399],
[0.08172004, -0.85495045, -0.51223194, 1.70261502],
[0., 0., 0., 1.]])
viewer = problem_env.env.GetViewer()
viewer.SetCamera(T_viewer)
import pdb
pdb.set_trace()
utils.release_obj()
else:
T_viewer = np.array(
[[0.99964468, -0.00577897, 0.02602139, 1.66357124],
[-0.01521307, -0.92529857, 0.37893419, -7.65383244],
[0.02188771, -0.37919541, -0.92505771, 6.7393589],
[0., 0., 0., 1.]])
viewer = problem_env.env.GetViewer()
viewer.SetCamera(T_viewer)
import pdb
pdb.set_trace()
# prefree and occludespre
target = 'rectangular_packing_box4'
utils.set_obj_xytheta(np.array([[0.1098148, -6.33305931, 0.22135689]]),
target)
utils.get_body_xytheta(target)
utils.visualize_path(
state.cached_pick_paths['rectangular_packing_box4'])
import pdb
pdb.set_trace()
# manipfree and occludesmanip
pick_obj = 'square_packing_box2'
pick_used = state.pick_used[pick_obj]
utils.two_arm_pick_object(pick_obj, pick_used.continuous_parameters)
utils.visualize_path(state.cached_place_paths[(u'square_packing_box2',
'home_region')])
import pdb
pdb.set_trace()
def create_environment(self):
problem_env = PaPOneArmMoverEnv(self.problem_idx)
goal = ['rectangular_packing_box1_region'] + ['c_obst1']
problem_env.set_goal(goal)
return problem_env, problem_env.env