def get_pick_poses(self, object, moved_obj, parent_state): if parent_state is not None and moved_obj != object.GetName(): return parent_state.pick_used[object.GetName()] operator_skeleton = Operator('two_arm_pick', {'object': object}) generator = UniformPaPGenerator(None, operator_skeleton, self.problem_env, None, n_candidate_params_to_smpl=3, total_number_of_feasibility_checks=500, dont_check_motion_existence=True) # we should disable objects, because we are getting shortest path that ignors all collisions anyways self.problem_env.disable_objects_in_region('entire_region') op_cont_params, _ = generator.sample_candidate_params_with_increasing_iteration_limit() motion_plan_goals = [op['q_goal'] for op in op_cont_params if op['q_goal'] is not None] """ for n_iter_to_try in n_iters: op_cont_params, _ = generator.sample_feasible_op_parameters(operator_skeleton, n_iter=n_iter_to_try, n_parameters_to_try_motion_planning=5) # I see. So here, return no op['q_goal'] when it is not feasible. motion_plan_goals = [op['q_goal'] for op in op_cont_params if op['q_goal'] is not None] if len(motion_plan_goals) > 2: break """ self.problem_env.enable_objects_in_region('entire_region') assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable operator_skeleton.continuous_parameters['q_goal'] = motion_plan_goals # to make it consistent with Dpl if len(motion_plan_goals) == 0: import pdb;pdb.set_trace() return operator_skeleton
def get_pick_poses(self, object, moved_obj, parent_state): if parent_state is not None and moved_obj != object.GetName(): return parent_state.pick_used[object.GetName()] operator_skeleton = Operator('two_arm_pick', {'object': object}) generator = UniformGenerator(operator_skeleton, self.problem_env, None) # we should disable objects, because we are getting shortest path that ignors all collisions anyways self.problem_env.disable_objects_in_region('entire_region') motion_plan_goals = [] n_iters = range(10, 500, 10) for n_iter_to_try in n_iters: op_cont_params, _ = generator.sample_feasible_op_parameters( operator_skeleton, n_iter=n_iter_to_try, n_parameters_to_try_motion_planning=5) motion_plan_goals = [ op['q_goal'] for op in op_cont_params if op['q_goal'] is not None ] if len(motion_plan_goals) > 2: break self.problem_env.enable_objects_in_region('entire_region') # assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable operator_skeleton.continuous_parameters[ 'q_goal'] = motion_plan_goals # to make it consistent with Dpl return operator_skeleton
def __init__(self, operator_skeleton, problem_env, swept_volume_constraint=None, cached_picks=None): self.problem_env = problem_env self.cached_picks = cached_picks target_region = None if 'place_region' in operator_skeleton.discrete_parameters: target_region = operator_skeleton.discrete_parameters['place_region'] if type(target_region) == str: target_region = self.problem_env.regions[target_region] target_obj = operator_skeleton.discrete_parameters['object'] self.robot = problem_env.robot self.target_region = target_region self.target_obj = target_obj self.swept_volume_constraint = swept_volume_constraint self.pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': target_obj}) self.pick_generator = UniformGenerator(self.pick_op, problem_env) # just use generator, I think self.place_op = Operator(operator_type='one_arm_place', discrete_parameters={'object': target_obj, 'place_region': target_region}, continuous_parameters={}) self.place_generator = UniformGenerator(self.place_op, problem_env) self.pick_feasibility_checker = OneArmPickFeasibilityChecker(problem_env) self.place_feasibility_checker = OneArmPlaceFeasibilityChecker(problem_env) self.operator_skeleton = operator_skeleton self.n_ik_checks = 0 self.n_mp_checks = 0
def sample_op_instance(self, curr_obj, region, swept_volume, n_iter): op = Operator(operator_type='one_arm_pick_one_arm_place', discrete_parameters={ 'object': curr_obj, 'region': region }) # use the following: # papg = OneArmPaPUniformGenerator(op_skel, self.problem_env, # cached_picks=(self.iksolutions[current_region], self.iksolutions[r])) pick_cont_param, place_cont_param = self.get_pap_pick_place_params( curr_obj.GetName(), region.name, swept_volume) #generator = OneArmPaPUniformGenerator(op, self.problem_env, swept_volume) #pick_cont_param, place_cont_param, status = generator.sample_next_point(max_ik_attempts=n_iter) if pick_cont_param is not None: status = 'HasSolution' else: status = 'NoSolution' op.continuous_parameters = { 'pick': pick_cont_param, 'place': place_cont_param } return op, status
def get_pick_poses(self, object, moved_obj, parent_state): if parent_state is not None and moved_obj != object.GetName(): return parent_state.pick_used[object.GetName()] operator_skeleton = Operator('two_arm_pick', {'object': object}) generator = UniformGenerator(operator_skeleton, self.problem_env, None) # This is just generating pick poses. It does not check motion feasibility self.problem_env.disable_objects_in_region('entire_region') object.Enable(True) self.problem_env.set_robot_to_default_dof_values() n_iters = range(10, 500, 10) feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, None) if len(feasible_cont_params) == 0 and moved_obj == object.GetName(): given_base_pose = utils.get_robot_xytheta() feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, given_base_pose) orig_xytheta = get_body_xytheta(self.problem_env.robot) self.problem_env.enable_objects_in_region('entire_region') min_n_collisions = np.inf chosen_pick_param = None for cont_param in feasible_cont_params: n_collisions = self.problem_env.get_n_collisions_with_objects_at_given_base_conf(cont_param['q_goal']) if min_n_collisions > n_collisions: chosen_pick_param = cont_param min_n_collisions = n_collisions utils.set_robot_config(orig_xytheta) # assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable operator_skeleton.continuous_parameters = chosen_pick_param operator_skeleton.continuous_parameters['q_goal'] = [chosen_pick_param['q_goal']] return operator_skeleton
def compute_and_cache_ik_solutions(self, ikcachename): before = CustomStateSaver(self.problem_env.env) utils.open_gripper() # for o in self.problem_env.env.GetBodies()[2:]: # o.Enable(False) self.problem_env.disable_objects() self.iksolutions = {} o = u'c_obst0' obj = self.problem_env.env.GetKinBody(o) if True: # for o, obj in self.objects.items(): # print(o) self.iksolutions[o] = {r: [] for r in self.regions} pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj}) pick_generator = UniformGenerator(pick_op, self.problem_env) pick_feasibility_checker = OneArmPickFeasibilityChecker(self.problem_env) for _ in range(10000): pick_params = pick_generator.sample_from_uniform() iks = [] for r, region in self.regions.items(): place_op = Operator(operator_type='one_arm_place', discrete_parameters={ 'object': obj, 'region': region}) place_generator = UniformGenerator(place_op, self.problem_env) status = 'NoSolution' for _ in range(10): obj_pose = place_generator.sample_from_uniform() set_obj_xytheta(obj_pose, obj) set_point(obj, np.hstack([obj_pose[0:2], region.z + 0.001])) params, status = pick_feasibility_checker.check_feasibility(pick_op, pick_params) if status == 'HasSolution': iks.append((obj.GetTransform(), params)) break if status == 'NoSolution': break if len(iks) == len(self.regions): for r, ik in zip(self.regions, iks): self.iksolutions[o][r].append(ik) if all(len(self.iksolutions[o][r]) >= 1000 for r in self.regions): break # print([len(self.iksolutions[o][r]) for r in self.regions]) self.iksolutions = self.iksolutions[o] # for o in self.problem_env.env.GetBodies()[2:]: # o.Enable(True) self.problem_env.enable_objects() before.Restore() pickle.dump(self.iksolutions, open(ikcachename, 'w'))
def sample_op_instance(self, curr_obj, n_iter): op = Operator(operator_type='one_arm_pick_one_arm_place', discrete_parameters={'object': curr_obj, 'region': self.goal_region}) target_object = op.discrete_parameters['object'] generator = OneArmPaPUniformGenerator(op, self.problem_env, None) print "Sampling paps for ", target_object pick_cont_param, place_cont_param, status = generator.sample_next_point(max_ik_attempts=n_iter) op.continuous_parameters = {'pick': pick_cont_param, 'place': place_cont_param} return op, status
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 find_pick(self, curr_obj): if self.problem_env.name.find("one_arm") != -1: pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': curr_obj}) else: pick_op = Operator(operator_type='two_arm_pick', discrete_parameters={'object': curr_obj}) params = self.sample_cont_params(pick_op, n_iter=500) if not params['is_feasible']: return None pick_op.continuous_parameters = params return pick_op
def get_applicable_ops(self, parent_op=None): applicable_ops = [] for op_name in self.operator_names: if op_name.find('place') != -1: if self.check_holding_object_precondition(): object_held = self.robot.GetGrabbed()[0] if self.applicable_op_constraint is None: for region in self.placement_regions.values(): if op_name == 'one_arm_place': assert parent_op is not None op = Operator( operator_type=op_name, discrete_parameters={ 'region': region, 'object': object_held }, continuous_parameters={ 'grasp_params': parent_op. continuous_parameters['grasp_params'] }) else: op = Operator(operator_type=op_name, discrete_parameters={ 'region': region, 'object': object_held }) applicable_ops.append(op) else: op = Operator( operator_type=op_name, discrete_parameters={ 'region': self.applicable_op_constraint['region'], 'object': object_held }) applicable_ops.append(op) else: if not self.check_holding_object_precondition(): if self.applicable_op_constraint is None: for obj in self.objects: op = Operator(operator_type=op_name, discrete_parameters={'object': obj}) applicable_ops.append(op) else: op = Operator( operator_type=op_name, discrete_parameters={ 'object': self.applicable_op_constraint['object'] }) applicable_ops.append(op) return applicable_ops
def find_pap(self, curr_obj): if self.problem_env.name.find("one_arm") != -1: op = Operator(operator_type='one_arm_pick_one_arm_place', discrete_parameters={ 'object': curr_obj, 'place_region': self.goal_region.name }) current_region = problem.get_region_containing( problem.env.GetKinBody(o)).name sampler = OneArmPaPUniformGenerator( action, problem, cached_picks=(iksolutions[current_region], self.iksolutions[self.goal_region.name])) pick_params, place_params, status = sampler.sample_next_point( self.config.n_iter_limit) self.n_ik += generator.n_ik_checks if status == 'HasSolution': params = {'pick': pick_params, 'place': place_params} else: params = None else: op = Operator(operator_type='two_arm_pick_two_arm_place', discrete_parameters={ 'object': curr_obj, 'place_region': self.goal_region.name }) state = None sampler = UniformSampler(op.type, self.goal_region) generator = TwoArmPaPGenerator( state, op, sampler, n_parameters_to_try_motion_planning=self.config.n_mp_limit, n_iter_limit=self.config.n_iter_limit, problem_env=self.problem_env, pick_action_mode='ir_parameters', place_action_mode='object_pose') params = generator.sample_next_point() self.n_mp += generator.n_mp_checks self.n_ik += generator.n_ik_checks # it must be because sampling a feasible pick can be done by trying as many as possible, # but placements cannot be made feasible by sampling more # also, it takes longer to check feasibility on place? # I just have to check the IK solution once if not params['is_feasible']: return None op.continuous_parameters = params return op
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_feasible_pick(problem_env, target_obj): pick_domain = utils.get_pick_domain() dim_parameters = pick_domain.shape[-1] domain_min = pick_domain[0] domain_max = pick_domain[1] smpls = np.random.uniform(domain_min, domain_max, (500, dim_parameters)).squeeze() feasibility_checker = two_arm_pick_feasibility_checker.TwoArmPickFeasibilityChecker(problem_env) op = Operator('two_arm_pick', {"object": target_obj}) for smpl in smpls: pick_param, status = feasibility_checker.check_feasibility(op, smpl, parameter_mode='ir_params') if status == 'HasSolution': op.continuous_parameters = pick_param return op
def get_pap_pick_place_params(self, obj, region, swept_volume): stime = time.time() r = region print(obj, r) current_region = self.problem_env.get_region_containing(obj).name # check existing solutions pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj}) place_op = Operator(operator_type='one_arm_place', discrete_parameters={ 'object': obj, 'region': self.problem_env.regions[r] }) obj_kinbody = self.problem_env.env.GetKinBody(obj) if len(self.pap_params[(obj, r)]) > 0: for pick_params, place_params in self.pap_params[(obj, r)]: pick_op.continuous_parameters = pick_params place_op.continuous_parameters = place_params if not self.check_collision_in_pap(pick_op, place_op, obj_kinbody, swept_volume): return pick_params, place_params op_skel = Operator(operator_type='one_arm_pick_one_arm_place', discrete_parameters={ 'object': self.problem_env.env.GetKinBody(obj), 'region': self.problem_env.regions[r] }) papg = OneArmPaPUniformGenerator( op_skel, self.problem_env, cached_picks=(self.iksolutions[current_region], self.iksolutions[r])) num_tries = 200 pick_params, place_params, status = papg.sample_next_point(num_tries) if 'HasSolution' in status: self.pap_params[(obj, r)].append((pick_params, place_params)) self.pick_params[obj].append(pick_params) print('success') pick_op.continuous_parameters = pick_params place_op.continuous_parameters = place_params collision = self.check_collision_in_pap(pick_op, place_op, obj_kinbody, swept_volume) if not collision: print('found nocollision', obj, r) return pick_params, place_params return None, None
def add_actions(self, action): new_action = Operator( operator_type=self.operator_skeleton.type, discrete_parameters=self.operator_skeleton.discrete_parameters, continuous_parameters=action) self.A.append(new_action) self.N[new_action] = 0
def get_uniform_sampler_place_feasibility_rate(pick_smpls, place_smpls, target_obj, problem_env): feasibility_checker = two_arm_pap_feasiblity_checker.TwoArmPaPFeasibilityChecker( problem_env) op = Operator('two_arm_place', { "object": target_obj, "place_region": 'loading_region' }) n_success = 0 orig_xytheta = utils.get_robot_xytheta(problem_env.robot) for pick_smpl, place_smpl in zip(pick_smpls, place_smpls): parameters = np.hstack([pick_smpl, place_smpl]) param, status = feasibility_checker.check_feasibility( op, parameters, swept_volume_to_avoid=None, parameter_mode='obj_pose') if status == 'HasSolution': motion, status = problem_env.motion_planner.get_motion_plan( [param['pick']['q_goal']], cached_collisions=None) if status == 'HasSolution': utils.two_arm_pick_object(target_obj, param['pick']) motion, status = problem_env.motion_planner.get_motion_plan( [param['place']['q_goal']], cached_collisions=None) utils.two_arm_place_object(param['pick']) utils.set_robot_config(orig_xytheta, problem_env.robot) n_success += status == 'HasSolution' total_samples = len(pick_smpls) return n_success / float(total_samples) * 100
def get_feasible_pick(problem_env, smpls): global target_obj_name feasibility_checker = two_arm_pick_feasibility_checker.TwoArmPickFeasibilityChecker( problem_env) op = Operator('two_arm_pick', {"object": target_obj_name}) for idx, param in enumerate(smpls): feasible_pick, status = feasibility_checker.check_feasibility( op, param, parameter_mode='ir_params') if status == 'HasSolution': print idx if idx == 98: continue # idx=110 op.continuous_parameters = feasible_pick break return op
def add_actions(self, continuous_parameters): new_action = Operator( operator_type=self.operator_skeleton.type, discrete_parameters=self.operator_skeleton.discrete_parameters, continuous_parameters=continuous_parameters, low_level_motion=None) self.A.append(new_action) self.N[new_action] = 0
def check_obj_reachable(self, obj): if len(self.problem_env.robot.GetGrabbed()) > 0: return False obj = self.problem_env.env.GetKinBody(obj) obj_name = str(obj.GetName()) if self.problem_env.name.find('one_arm') != -1: op = Operator('one_arm_pick', {'object': obj}) else: op = Operator('two_arm_pick', {'object': obj}) if obj_name in self.pick_used: motion_plan_goals = self.pick_used[obj_name].continuous_parameters['q_goal'] else: motion_plan_goals = self.generate_potential_pick_configs(op, n_pick_configs=10) if motion_plan_goals is not None: motion_planner = BaseMotionPlanner(self.problem_env, 'prm') motion, status = motion_planner.get_motion_plan(motion_plan_goals) is_feasible_param = status == 'HasSolution' else: is_feasible_param = False if is_feasible_param: op.make_pklable() op.continuous_parameters = {'q_goal': motion[-1]} self.motion_plans[obj_name] = motion if obj_name not in self.pick_used: self.pick_used[obj_name] = op self.evaluations[obj_name] = True return True else: self.evaluations[obj_name] = False return False
def find_place(self, curr_obj, pick): if self.problem_env.name.find("one_arm") != -1: place_op = Operator(operator_type='one_arm_place', discrete_parameters={'object': curr_obj, 'region': self.goal_region}, continuous_parameters=pick.continuous_parameters) else: place_op = Operator(operator_type='two_arm_place', discrete_parameters={'object': curr_obj, 'region': self.goal_region}) # it must be because sampling a feasible pick can be done by trying as many as possible, # but placements cannot be made feasible by sampling more # also, it takes longer to check feasibility on place? # I just have to check the IK solution once params = self.sample_cont_params(place_op, n_iter=500) if not params['is_feasible']: return None place_op.continuous_parameters = params return place_op
def get_pap_pick_place_params(self, obj, region, swept_volume): stime = time.time() r = region print(obj, r) current_region = self.problem_env.get_region_containing(obj).name # check existing solutions pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj}) place_op = Operator(operator_type='one_arm_place', discrete_parameters={'object': obj, 'place_region': self.problem_env.regions[r]}) obj_kinbody = self.problem_env.env.GetKinBody(obj) if len(self.pap_params[(obj, r)]) > 0: for pick_params, place_params in self.pap_params[(obj, r)]: pick_op.continuous_parameters = pick_params place_op.continuous_parameters = place_params if not self.check_collision_in_pap(pick_op, place_op, obj_kinbody, swept_volume): return pick_params, place_params op_skel = Operator(operator_type='one_arm_pick_one_arm_place', discrete_parameters={'object': self.problem_env.env.GetKinBody(obj), 'place_region': self.problem_env.regions[r]}) # papg = OneArmPaPUniformGenerator(op_skel, # self.problem_env, # cached_picks=None) sampler = {'pick': UniformSampler(target_region=None, atype='one_arm_pick'), 'place': UniformSampler(target_region=self.problem_env.regions[r], atype='one_arm_place')} papg = OneArmPaPGenerator(op_skel, n_iter_limit=self.config.n_iter_limit, problem_env=self.problem_env, pick_sampler=sampler['pick'], place_sampler=sampler['place']) pick_params, place_params, status = papg.sample_next_point() if 'HasSolution' in status: self.pap_params[(obj, r)].append((pick_params, place_params)) self.pick_params[obj].append(pick_params) print('success') pick_op.continuous_parameters = pick_params place_op.continuous_parameters = place_params collision = self.check_collision_in_pap(pick_op, place_op, obj_kinbody, swept_volume) if not collision: print('found nocollision', obj, r) return pick_params, place_params return None, None
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_applicable_op_skeleton(self, parent_action): if parent_action is None: op_name = 'two_arm_pick' else: # use_multipaps = parent_action.discrete_parameters['object'].GetName().find("big") != -1 use_multipaps = self.n_actions_per_node > 1 if parent_action.type == 'two_arm_pick': if use_multipaps: op_name = str(self.n_actions_per_node) + '_paps' else: op_name = 'two_arm_place' else: op_name = 'two_arm_pick' if op_name == 'two_arm_place': op = Operator(operator_type=op_name, discrete_parameters={ 'region': self.regions['object_region'], 'object': self.objects_currently_not_in_goal[0] }, continuous_parameters=None, low_level_motion=None) elif op_name == 'two_arm_pick': op = Operator(operator_type=op_name, discrete_parameters={ 'object': self.objects_currently_not_in_goal[0] }, continuous_parameters=None, low_level_motion=None) elif op_name.find('_paps') != -1: objects = self.objects_currently_not_in_goal[0:self. n_actions_per_node] op = Operator(operator_type=op_name, discrete_parameters={ 'objects': objects, 'region': self.regions['object_region'] }, continuous_parameters=None, low_level_motion=None) else: raise NotImplementedError return op
def get_applicable_op_skeleton(self, parent_action): op_name = self.which_operator() if parent_action is None: op_name = 'two_arm_pick' else: if parent_action.type == 'two_arm_pick': op_name = 'two_arm_place' else: op_name = 'two_arm_pick' if op_name == 'two_arm_place': op = Operator(operator_type=op_name, discrete_parameters={'region': self.regions['entire_region']}, continuous_parameters=None, low_level_motion=None) else: op = Operator(operator_type=op_name, discrete_parameters={'object': self.objects_currently_not_in_goal[0]}, continuous_parameters=None, low_level_motion=None) return 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
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 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 get_place_feasibility_rate(pick_smpls, place_smpls, target_obj, problem_env): # todo start from here pick_feasibility_checker = two_arm_pick_feasibility_checker.TwoArmPickFeasibilityChecker( problem_env) parameter_mode = 'ir_params' place_feasibility_checker = two_arm_place_feasibility_checker.TwoArmPlaceFeasibilityChecker( problem_env) n_success = 0 for pick_smpl, place_smpl in zip(pick_smpls, place_smpls): pick_op = Operator('two_arm_pick', {"object": target_obj}) pick_smpl, status = pick_feasibility_checker.check_feasibility( pick_op, pick_smpl, parameter_mode=parameter_mode) pick_op.continuous_parameters = pick_smpl if status == 'HasSolution': pick_op.execute() op = Operator('two_arm_place', { "object": target_obj, "place_region": 'loading_region' }, continuous_parameters=place_smpl) place_smpl, status = place_feasibility_checker.check_feasibility( op, place_smpl, parameter_mode='obj_pose') n_success += status == 'HasSolution' utils.two_arm_place_object(pick_op.continuous_parameters) total_samples = len(pick_smpls) return n_success / float(total_samples) * 100
def pick_object(self, obj_name): assert type(obj_name) == str or type(obj_name) == unicode obj = self.problem_env.env.GetKinBody(obj_name) # this assumes we have pick if obj_name in self.pick_used: # where does self.pick_used get updated? # we cannot use the pick path used in data because q_init is different motion_plan_goals = self.pick_used[obj_name].continuous_parameters[ 'q_goal'] # todo check if pick_used is still valid # during planning, we would never have pick_used. So we just have to make sure for the data processing # it is valid if it didn't move operator = self.pick_used[obj_name] else: operator = Operator('two_arm_pick', {'object': obj}) motion_plan_goals = self.generate_potential_pick_configs( operator, n_pick_configs=10) if motion_plan_goals is None: self.sampled_pick_configs_for_objects[obj_name] = None return None else: self.sampled_pick_configs_for_objects[obj_name] = motion_plan_goals motion_planner = MinimumConstraintPlanner(self.problem_env, obj, 'prm') motion, status = motion_planner.get_motion_plan( motion_plan_goals, cached_collisions=self.collides) # why does it ever enter here? try: assert motion is not None except: import pdb pdb.set_trace() if obj.GetName() not in self.pick_used: # import pdb;pdb.set_trace() operator.continuous_parameters = {'q_goal': motion_plan_goals} self.pick_used[obj.GetName()] = operator operator.execute()
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