def get_pap_used_in_plan(self, plan):
obj_to_pick = {op.discrete_parameters['object']: [] for op in plan}
obj_to_place = {(op.discrete_parameters['object'],
op.discrete_parameters['place_region']): []
for op in plan}
for op in plan:
# making obj_to_pick and obj_to_place used in the plan; this can be done once, not every time
pick_cont_param = op.continuous_parameters['pick']
pick_disc_param = {'object': op.discrete_parameters['object']}
pick_op = Operator('two_arm_pick', pick_disc_param,
pick_cont_param)
pick_op.low_level_motion = pick_cont_param['motion']
obj_to_pick[op.discrete_parameters['object']].append(pick_op)
place_cont_param = op.continuous_parameters['place']
place_disc_param = {
'object': op.discrete_parameters['object'],
'place_region': op.discrete_parameters['place_region']
}
place_op = Operator('two_arm_pick', place_disc_param,
place_cont_param)
place_op.low_level_motion = place_cont_param['motion']
obj_to_place[(
op.discrete_parameters['object'],
op.discrete_parameters['place_region'])].append(place_op)
return [obj_to_pick, obj_to_place]
def plan_minimum_constraint_path_to_region(self, region):
obj_holding = self.robot.GetGrabbed()[0]
target_region = self.problem_env.regions[region]
place_op = Operator(operator_type='two_arm_place', discrete_parameters={'object': obj_holding,
'region': target_region})
obj_region_key = (obj_holding.GetName(), region)
if obj_region_key in self.mc_paths:
motion = self.mc_paths[(obj_holding.GetName(), region)]
place_op.low_level_motion = motion
place_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, 'HasSolution', place_op
if obj_region_key in self.place_used:
print "Using the place data" # todo but this depends on which state...
motion = self.place_used[obj_region_key].low_level_motion
status = 'HasSolution'
else:
potential_motion_plan_goals = self.generate_potential_place_configs(place_op, n_pick_configs=10)
if potential_motion_plan_goals is None:
return None, "NoSolution", None
self.mc_calls += 1
motion, status = self.get_minimum_constraint_path_to(potential_motion_plan_goals, obj_holding)
if motion is None:
return None, "NoSolution", None
place_op.low_level_motion = motion
place_op.continuous_parameters = {'q_goal': motion[-1]}
if obj_region_key not in self.place_used:
self.place_used[obj_region_key] = place_op
return motion, status, place_op
def __call__(self, a, b, r, cached_path=None):
assert r in self.problem_env.regions
# While transferring "a" to region "r", is "b" in the way to region
if (a, b, r) in self.evaluations.keys():
return self.evaluations[(a, b, r)]
else:
# what happens a is already in r? We still plan a path, because we want to know if we can move object a
# inside region r
is_a_obj = a not in self.problem_env.region_names
is_b_obj = b not in self.problem_env.region_names
is_r_region = r in self.problem_env.region_names # this is already defended
if not is_a_obj or not is_b_obj:
return False
if a == b:
return False
if not is_r_region:
return False
min_constraint_path_already_computed = (a, r) in self.mc_to_entity.keys()
if min_constraint_path_already_computed:
objs_in_collision = self.mc_to_entity[(a, r)]
else:
saver = CustomStateSaver(self.problem_env.env)
if cached_path is not None:
self.pick_used[a].execute()
path = cached_path
status = 'HasSolution'
place_op = Operator(operator_type='two_arm_place', discrete_parameters={
'object': self.problem_env.env.GetKinBody(a),
'region': self.problem_env.regions[r],
})
place_op.low_level_motion = path
place_op.continuous_parameters = {'q_goal': path[-1]}
else:
self.pick_object(a)
path, status, place_op = self.plan_minimum_constraint_path_to_region(r)
if status != 'HasSolution':
objs_in_collision = None
else:
objs_in_collision = self.problem_env.get_objs_in_collision(path, 'entire_region')
objs_in_collision = [o.GetName() for o in objs_in_collision]
self.mc_to_entity[(a, r)] = objs_in_collision
self.mc_path_to_entity[(a, r)] = path
if len(objs_in_collision) == 0:
self.reachable_obj_region_pairs.append((a, r))
saver.Restore()
evaluation = objs_in_collision is not None and b in objs_in_collision
self.evaluations[(a, b, r)] = evaluation
return evaluation
def add_pap_swept_volume(self, pap_instance):
is_one_arm = pap_instance.type.find('one_arm') != -1
if not is_one_arm:
raise NotImplementedError
target_obj = pap_instance.discrete_parameters['object']
target_region = pap_instance.discrete_parameters['region']
dummy_pick = Operator(operator_type='one_arm_pick',
discrete_parameters={'object': target_obj},
continuous_parameters=pap_instance.continuous_parameters['pick'])
dummy_pick.low_level_motion = [pap_instance.low_level_motion['pick']]
dummy_place = Operator(operator_type='one_arm_place',
discrete_parameters={'object': target_obj, 'region': target_region},
continuous_parameters=pap_instance.continuous_parameters['place'])
dummy_place.low_level_motion = [pap_instance.low_level_motion['place']]
self.add_pick_swept_volume(dummy_pick)
self.add_place_swept_volume(dummy_place, dummy_pick)
def plan_pick_motion_for(self, object_to_move, pick_op_instance):
pick_op = Operator(operator_type='two_arm_pick',
discrete_parameters={'object': object_to_move})
motion_planner = MinimumConstraintPlanner(self.problem_env,
object_to_move, 'prm')
motion, status = motion_planner.get_motion_plan(
pick_op_instance.continuous_parameters['q_goal'])
if motion is None:
return None, "NoSolution", None
motion.append(pick_op_instance.continuous_parameters['q_goal'])
pick_op.low_level_motion = motion
pick_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, status, pick_op
def get_pick_from_initial_config(self, obj):
utils.set_robot_config(self.problem_env.initial_robot_base_pose)
pick_op = Operator(operator_type='two_arm_pick',
discrete_parameters={'object': obj})
potential_motion_plan_goals = self.generate_motion_plan_goals(
pick_op, n_configs=5)
if potential_motion_plan_goals is None:
return None, "NoSolution", None
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj)
if motion is None:
return None, "NoSolution", None
pick_op.low_level_motion = motion
pick_op.continuous_parameters = {
'q_goal': motion[-1],
'motion': motion
}
return motion, status, pick_op
def get_pick_from_initial_config(self, obj):
utils.set_robot_config(self.problem_env.initial_robot_base_pose)
pick_op = Operator(operator_type='two_arm_pick',
discrete_parameters={'object': obj})
we_already_have_pick_config = obj.GetName(
) in self.sampled_pick_configs_for_objects.keys()
if we_already_have_pick_config:
return self.sampled_pick_configs_for_objects[obj.GetName()]
else:
potential_motion_plan_goals = self.generate_motion_plan_goals(
pick_op, n_configs=5)
if potential_motion_plan_goals is None:
return None, "NoSolution", None
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj)
if motion is None:
return None, "NoSolution", None
pick_op.low_level_motion = motion
pick_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, status, pick_op
def plan_place(self, target_region, swept_volumes):
obj_holding = self.robot.GetGrabbed()[0]
place_op = Operator(operator_type='two_arm_place',
discrete_parameters={
'object': obj_holding,
'region': target_region
})
stime = time.time()
potential_motion_plan_goals = self.generate_motion_plan_goals(
place_op, n_configs=5, swept_volumes=swept_volumes)
print time.time() - stime
if potential_motion_plan_goals is None:
return None, "NoSolution", None
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj_holding)
if motion is None:
return None, "NoSolution", None
place_op.low_level_motion = motion
place_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, status, place_op
