def __init__(self, problem_idx):
Mover.__init__(self, problem_idx)
self.operator_names = ['one_arm_pick', 'one_arm_place']
set_robot_config([4.19855789, 2.3236321, 5.2933337], self.robot)
set_obj_xytheta([3.35744004, 2.19644156, 3.52741118], self.objects[1])
self.boxes = self.objects
self.objects = self.problem_config['shelf_objects']
self.objects = [k for v in self.objects.values() for k in v]
self.objects[0], self.objects[1] = self.objects[1], self.objects[0]
self.target_box = self.env.GetKinBody('rectangular_packing_box1')
utils.randomly_place_region(self.target_box, self.regions['home_region'])
self.regions['rectangular_packing_box1_region'] = self.compute_box_region(self.target_box)
self.shelf_regions = self.problem_config['shelf_regions']
self.target_box_region = self.regions['rectangular_packing_box1_region']
self.regions.update(self.shelf_regions)
self.entity_names = [obj.GetName() for obj in self.objects] + ['rectangular_packing_box1_region',
'center_shelf_region']
self.name = 'one_arm_mover'
self.init_saver = utils.CustomStateSaver(self.env)
self.object_names = self.entity_names
# fix incorrectly named regions
self.regions = {
region.name: region
for region in self.regions.values()
}
def compute_clf_rates_from_diff_pinstances(problem_env, key_configs, net,
action_type):
clf_rates = []
for problem_seed in range(400):
np.random.seed(problem_seed)
random.seed(problem_seed)
[
utils.randomly_place_region(obj,
problem_env.regions['loading_region'])
for obj in problem_env.objects
]
utils.randomly_place_region(problem_env.robot,
problem_env.regions['loading_region'])
if action_type == 'place':
sampler = UniformSampler(problem_env.regions['home_region'])
checker = TwoArmPaPFeasibilityCheckerWithoutSavingFeasiblePick(
problem_env)
status = "NoSolution"
while status != "HasSolution":
pick_param = sampler.sample()
target_obj = problem_env.objects[np.random.randint(8)]
abstract_action = Operator(
operator_type='two_arm_pick_two_arm_place',
discrete_parameters={
'object': target_obj,
'place_region': 'home_region'
})
op_parameters, status = checker.check_feasibility(
abstract_action, pick_param)
utils.two_arm_pick_object(target_obj, op_parameters['pick'])
region_name = 'loading_region'
else:
region_name = 'loading_region'
clf_rate = get_clf_accuracy(problem_env, key_configs, net, region_name)
if action_type == 'place':
utils.two_arm_place_object(op_parameters['pick'])
clf_rates.append(clf_rate)
# print "pidx %d Clf rate %.2f " % (problem_seed, clf_rate)
print np.array(clf_rates).sum(axis=0), problem_seed
def set_goal(self, goal_objects, goal_region):
self.goal_objects = goal_objects
[utils.set_color(o, [1, 0, 0]) for o in self.goal_objects]
if 40000 <= self.problem_idx < 50000:
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance and obj.GetName() not in goal_objects][0:3]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
# surround the robot?
# Force the goal object to be around the robot
#utils.randomly_place_region(self.robot, robot_region)
radius = 1
center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
xmin = center[0] - radius
xmax = center[0]
ymin = center[1]
ymax = center[1] + radius
goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# doing the below because we ran n_objs_pack=1 and n_objs_pack=4 on different commits
if len(goal_objects) == 4:
for obj in goal_objects[0:1]:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)
else:
for obj in goal_objects:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)
elif 50000 <= self.problem_idx < 60000:
# hard problems for both RSC and Greedy
# hard for RSC: make sure the goal object needs to be moved twice
# dillema: if I put the goal object near the entrance, then I can just move that to the goal region
# I must surround the robot with the goal object such that the shortest path always go
# through the goal object
# another option is to block the object in the entrance region with the goal object
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region',
# xmin,xmax ymin, ymax
((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance
if obj.GetName() not in goal_objects][0:3]
# object_around_entrance = np.array(object_around_entrance)[
# np.random.choice(range(len(object_around_entrance)), 3, replace=False)]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
# xmin,xmax ymin, ymax
robot_region = AARegion('robot_region', ((3.0, 4.29), (-8.0, -6.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
utils.randomly_place_region(self.robot, robot_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects
if obj not in object_around_entrance + objs_to_move_near_entrance]
# surround the robot?
# Force the goal object to be around the robot
utils.randomly_place_region(self.robot, robot_region)
radius = 1
center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
xmin = center[0] - radius
xmax = center[0] + radius
ymin = center[1] - radius
ymax = center[1] + radius
goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
for obj in goal_objects:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region)
elif self.problem_idx >= 60000:
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance][0:3]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
self.initial_robot_base_pose = get_body_xytheta(self.robot)
self.object_init_poses = {o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects}
self.init_saver = CustomStateSaver(self.env)
self.goal_region = goal_region
self.goal_entities = self.goal_objects + [self.goal_region]
def put_obj_out_of_region(body, region):
while region.contains(body.ComputeAABB()):
utils.randomly_place_region(body, region)