def __init__(self, problem_idx, problem_type='normal'): ProblemEnvironment.__init__(self, problem_idx) problem_defn = MoverEnvironmentDefinition(self.env) self.problem_config = problem_defn.get_problem_config() self.robot = self.env.GetRobots()[0] self.objects = self.problem_config['packing_boxes'] self.set_problem_type(problem_type) self.object_init_poses = {o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects} self.initial_robot_base_pose = get_body_xytheta(self.robot) self.regions = {'entire_region': self.problem_config['entire_region'], 'home_region': self.problem_config['home_region'], 'loading_region': self.problem_config['loading_region']} self.region_names = ['entire_region', 'home_region', 'loading_region'] self.object_names = [obj.GetName() for obj in self.objects] self.placement_regions = {'home_region': self.problem_config['home_region'], 'loading_region': self.problem_config['loading_region']} #self.entity_names = self.object_names + ['home_region', 'loading_region','entire_region'] self.entity_names = self.object_names + ['home_region', 'loading_region', 'entire_region'] self.entity_idx = {name: idx for idx, name in enumerate(self.entity_names)} self.is_init_pick_node = True self.name = 'two_arm_mover' self.init_saver = CustomStateSaver(self.env) self.problem_config['env'] = self.env self.operator_names = ['two_arm_pick', 'two_arm_place'] self.reward_function = None self.applicable_op_constraint = None self.two_arm_pick_continuous_constraint = None self.two_arm_place_continuous_constraint = None self.objects_to_check_collision = None self.goal = None
def update_collisions_at_prm_vertices(self, parent_collides): # global prm_vertices # global prm_edges # if prm_vertices is None or prm_edges is None: # prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb')) is_robot_holding = len(self.problem_env.robot.GetGrabbed()) > 0 def in_collision(q, obj): set_robot_config(q, self.problem_env.robot) if is_robot_holding: # note: # openrave bug: when an object is held, it won't check the held_obj and given object collision unless # collision on robot is first checked. So, we have to check it twice col = self.problem_env.env.CheckCollision( self.problem_env.robot) col = self.problem_env.env.CheckCollision( self.problem_env.robot, obj) else: col = self.problem_env.env.CheckCollision( self.problem_env.robot, obj) return col obj_name_to_pose = { obj.GetName(): tuple(get_body_xytheta(obj)[0].round(6)) for obj in self.problem_env.objects } collisions_at_all_obj_pose_pairs = {} old_q = get_body_xytheta(self.problem_env.robot) for obj in self.problem_env.objects: obj_name_pose_tuple = (obj.GetName(), obj_name_to_pose[obj.GetName()]) collisions_with_obj_did_not_change = parent_collides is not None and \ obj_name_pose_tuple in parent_collides if collisions_with_obj_did_not_change: collisions_at_all_obj_pose_pairs[ obj_name_pose_tuple] = parent_collides[obj_name_pose_tuple] else: prm_vertices_in_collision_with_obj = { i for i, q in enumerate(self.prm_vertices) if in_collision(q, obj) } collisions_at_all_obj_pose_pairs[ obj_name_pose_tuple] = prm_vertices_in_collision_with_obj set_robot_config(old_q, self.problem_env.robot) # what's the diff between collides and curr collides? # collides include entire set of (obj, obj_pose) pairs that we have seen so far # current collides are the collisions at the current object pose collisions_at_current_obj_pose_pairs = { obj.GetName(): collisions_at_all_obj_pose_pairs[(obj.GetName(), obj_name_to_pose[obj.GetName()])] for obj in self.problem_env.objects } return collisions_at_all_obj_pose_pairs, collisions_at_current_obj_pose_pairs
def update_collisions_at_prm_vertices(self, parent_state): global prm_vertices global prm_edges if prm_vertices is None or prm_edges is None: prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb')) holding = len(self.problem_env.robot.GetGrabbed()) > 0 if holding: held = self.problem_env.robot.GetGrabbed()[0] def in_collision(q, obj): #old_q = get_body_xytheta(self.problem_env.robot) set_robot_config(q, self.problem_env.robot) col = self.problem_env.env.CheckCollision(self.problem_env.robot, obj) #set_robot_config(old_q, self.problem_env.robot) return col obj_name_to_pose = { obj.GetName(): tuple(get_body_xytheta(obj)[0].round(6)) for obj in self.problem_env.objects } # if robot is holding an object, all collision information changes # but we should still retain previous collision information # because collisions at the next state will be similar to collisions at the previous state # todo once we placed the object, instead of setting the collides to be empty, # we could use the collision information from state-before-pick, because # the robot shape goes back to whatever it was. collides = copy.copy(parent_state.collides) if holding else {} old_q = get_body_xytheta(self.problem_env.robot) for obj in self.problem_env.objects: obj_name_pose_tuple = (obj.GetName(), obj_name_to_pose[obj.GetName()]) collisions_with_obj_did_not_change = parent_state is not None and \ obj_name_pose_tuple in parent_state.collides and \ not holding if collisions_with_obj_did_not_change: collides[obj_name_pose_tuple] = parent_state.collides[ obj_name_pose_tuple] else: prm_vertices_in_collision_with_obj = { i for i, q in enumerate(prm_vertices) if in_collision(q, obj) } collides[ obj_name_pose_tuple] = prm_vertices_in_collision_with_obj set_robot_config(old_q, self.problem_env.robot) current_collides = { obj.GetName(): collides[(obj.GetName(), obj_name_to_pose[obj.GetName()])] for obj in self.problem_env.objects } return collides, current_collides
def __init__(self, abstract_state, abstract_action): ConcreteNodeState.__init__(self, abstract_state, abstract_action) self.key_configs = self.abstract_state.prm_vertices self.pick_collision_vector = self.get_konf_obstacles(abstract_state.current_collides) self.place_collision_vector = None self.abs_robot_pose = utils.clean_pose_data(utils.get_body_xytheta(self.problem_env.robot)) self.abs_obj_pose = utils.clean_pose_data(utils.get_body_xytheta(self.problem_env.env.GetKinBody(self.obj))) self.abs_goal_obj_poses = [np.array([0, 0, 0])]
def __init__(self, problem_env, goal_entities, parent_state=None, parent_action=None): self.state_saver = CustomStateSaver(problem_env.env) self.problem_env = problem_env self.parent = parent_state # used to update the node features # raw variables self.robot_pose = get_body_xytheta(problem_env.robot) self.object_poses = { obj.GetName(): get_body_xytheta(obj) for obj in problem_env.objects }
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 sample_from_continuous_space(self): assert len(self.robot.GetGrabbed()) == 0 # sample pick pick_cont_params = None n_ik_attempts = 0 n_base_attempts = 0 status = "NoSolution" robot_pose = utils.get_body_xytheta(self.robot) robot_config = self.robot.GetDOFValues() # sample pick parameters pick_cont_params, status = self.sample_pick_cont_parameters() if status != 'HasSolution': utils.set_robot_config(robot_pose) return None, None, status self.pick_op.continuous_parameters = pick_cont_params self.pick_op.execute() # sample place place_cont_params, status = self.sample_place_cont_parameters(pick_cont_params) # reverting back to the state before sampling utils.one_arm_place_object(pick_cont_params) self.robot.SetDOFValues(robot_config) utils.set_robot_config(robot_pose) if status != 'HasSolution': return None, None, "InfeasibleIK" else: self.place_op.continuous_parameters = place_cont_params return pick_cont_params, place_cont_params, status
def sample_from_continuous_space(self): # sample pick pick_cont_params = None n_ik_attempts = 0 n_base_attempts = 0 status = "NoSolution" robot_pose = utils.get_body_xytheta(self.robot) robot_config = self.robot.GetDOFValues() # sample pick parameters while pick_cont_params is None: pick_cont_params, status = self.sample_pick_cont_parameters() if status == 'InfeasibleBase': n_base_attempts += 1 elif status == 'InfeasibleIK': n_ik_attempts += 1 elif status == 'HasSolution': n_ik_attempts += 1 break if n_ik_attempts == 1 or n_base_attempts == 4: break if status != 'HasSolution': utils.set_robot_config(robot_pose) return None, None, status self.pick_op.continuous_parameters = pick_cont_params self.pick_op.execute() # sample place n_ik_attempts = 0 n_base_attempts = 0 status = "NoSolution" place_cont_params = None while place_cont_params is None: place_cont_params, status = self.sample_place_cont_parameters(pick_cont_params) if status == 'InfeasibleBase': n_base_attempts += 1 elif status == 'InfeasibleIK': n_ik_attempts += 1 elif status == 'HasSolution': n_ik_attempts += 1 break if n_ik_attempts == 1 or n_base_attempts == 1: break # reverting back to the state before sampling utils.one_arm_place_object(pick_cont_params) self.robot.SetDOFValues(robot_config) utils.set_robot_config(robot_pose) if status != 'HasSolution': return None, None, status else: self.place_op.continuous_parameters = place_cont_params return pick_cont_params, place_cont_params, status
def __init__(self, problem_env, goal_entities, parent_state=None, parent_action=None, paps_used_in_data=None): self.state_saver = CustomStateSaver(problem_env.env) self.problem_env = problem_env self.problem_env.set_robot_to_default_dof_values() self.parent_state = parent_state # used to update the node features self.goal_entities = goal_entities self.object_names = [str(obj.GetName()) for obj in problem_env.objects] # raw variables self.robot_pose = get_body_xytheta(problem_env.robot) self.object_poses = { obj.GetName(): get_body_xytheta(obj) for obj in problem_env.objects } self.use_prm = problem_env.name.find('two_arm') != -1 if paps_used_in_data is not None: self.pick_used = paps_used_in_data[0] self.place_used = paps_used_in_data[1] self.mc_pick_path = {} self.mc_place_path = {} self.reachable_entities = [] self.pick_in_way = None self.place_in_way = None self.in_region = None self.is_holding_goal_entity = None self.ternary_edges = None self.binary_edges = None self.nodes = None self.prm_vertices, self.prm_edges = pickle.load(open( './prm.pkl', 'rb'))
def get_smpler_state(pidx, obj, problem_env): fname = cached_env_path + 'policy_eval_pidx_%d.pkl' % pidx if os.path.isfile(fname): state = pickle.load(open(fname, 'r')) else: state = compute_state(obj, 'loading_region', problem_env) pickle.dump(state, open(fname, 'wb')) state.obj = obj state.goal_flags = state.get_goal_flags() state.abs_obj_pose = utils.get_body_xytheta(obj) return state
def get_place_param_with_feasible_motion_plan(self, chosen_pick_param, candidate_pap_parameters, cached_holding_collisions): original_config = utils.get_body_xytheta( self.problem_env.robot).squeeze() utils.two_arm_pick_object( self.operator_skeleton.discrete_parameters['object'], chosen_pick_param) place_op_params = [op['place'] for op in candidate_pap_parameters] chosen_place_param = self.get_op_param_with_feasible_motion_plan( place_op_params, cached_holding_collisions) utils.two_arm_place_object(chosen_pick_param) utils.set_robot_config(original_config) return chosen_place_param
def __init__(self, abstract_state, abstract_action, key_configs, parent_state=None): ConcreteNodeState.__init__(self, abstract_state, abstract_action) self.parent_state = parent_state self.key_configs = key_configs init_config = utils.get_rightarm_torso_config(abstract_state.problem_env.robot) self.pick_collision_vector = self.get_konf_obstacles_from_key_configs() #self.konf_collisions_with_obj_poses = self.get_object_pose_collisions() #self.pick_collision_vector = self.get_konf_obstacles(self.konf_collisions_with_obj_poses) utils.set_rightarm_torso(init_config, abstract_state.problem_env.robot) self.place_collision_vector = None # following three variables are used in get_processed_poses_from_state self.abs_robot_pose = utils.get_rightarm_torso_config(self.problem_env.robot) self.abs_obj_pose = utils.clean_pose_data(utils.get_body_xytheta(self.problem_env.env.GetKinBody(self.obj))) self.abs_goal_obj_poses = [np.array([0, 0, 0])]
def __init__(self, problem_env, obj, region, goal_entities, key_configs=None, collision_vector=None): self.obj = obj self.region = region self.goal_entities = goal_entities self.key_configs = self.get_key_configs(key_configs) self.collision_vector = self.get_collison_vector(collision_vector) if type(obj) == str or type(obj) == unicode: obj = problem_env.env.GetKinBody(obj) self.abs_robot_pose = utils.clean_pose_data( utils.get_body_xytheta(problem_env.robot)) self.abs_obj_pose = utils.clean_pose_data(utils.get_body_xytheta(obj)) self.abs_goal_obj_pose = utils.clean_pose_data( utils.get_body_xytheta('square_packing_box1')) self.goal_flags = self.get_goal_flags() self.rel_konfs = None
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()): path = [get_body_xytheta(self.problem_env.robot).squeeze()] #self.reachable_regions_while_holding.append((obj, region_name)) else: if self.holding_collides is not None: path, status = motion_planner.get_motion_plan(region, cached_collisions=self.holding_collides) else: # I think the trouble here is that we do not hold the object when checking collisions # So, the best way to solve this problem is to not keep reachable_regions_while_holding # and just use the cached path. But I am wondering how we got a colliding-path in # the first place. It must be from place_in_way? No, we execute this function first, # and then using the cached paths, compute the place_in_way. # Also, there is something wrong with the collision checking too. # I think this has to do with the fact that the collisions are computed using # the robot only, not with the object in hand. # So, here is what I propose: # Plan motions here, but do not add to reachable regions while holding. # This way, it will plan motions as if it is not holding the object, # but check collisions inside place_in_way path, status = motion_planner.get_motion_plan(region, cached_collisions=self.collisions_at_all_obj_pose_pairs) if status == 'HasSolution': pass else: obj_not_moved = obj != moved_obj parent_state_has_cached_path_for_obj = parent_state is not None \ and obj in parent_state.cached_place_paths # todo: What is this logic here...? # it is about re-using the parent place path; # but this assumes that the object has not moved? if parent_state_has_cached_path_for_obj and obj_not_moved: path = parent_state.cached_place_paths[(obj, region_name)] else: path, _ = motion_planner.get_motion_plan(region, cached_collisions={}) saver.Restore() # assert path is not None self.cached_place_paths[(obj, region_name)] = path
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None parent_state_has_cached_path_for_obj \ = parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj cached_path_is_shortest_path = parent_state is not None and \ not (obj, region_name) in parent_state.reachable_regions_while_holding saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()): path = [get_body_xytheta(self.problem_env.robot).squeeze()] self.reachable_regions_while_holding.append((obj, region_name)) else: if region.name == 'home_region': # a location right at the entrance of home goal = [np.array([0.73064842, -2.85306871, 4.87927762])] else: goal = region if self.holding_collides is not None: path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions=self.holding_collides, return_start_set_and_path_idxs=True) else: # note: self.collides is computed without holding the object. path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions=self.collides, return_start_set_and_path_idxs=True) if status == 'HasSolution': self.reachable_regions_while_holding.append((obj, region_name)) else: if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path: path = parent_state.cached_place_paths[(obj, region_name)] start_and_prm_idxs = parent_state.cached_place_start_and_prm_idxs[(obj, region_name)] else: path, _, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions={}, return_start_set_and_path_idxs=True) saver.Restore() # assert path is not None self.cached_place_paths[(obj, region_name)] = path self.cached_place_start_and_prm_idxs[(obj, region_name)] = start_and_prm_idxs
def get_augmented_state_vec_and_poses(obj, state_vec, smpler_state): n_key_configs = 615 utils.set_color(obj, [1, 0, 0]) is_goal_obj = utils.convert_binary_vec_to_one_hot( np.array([obj in smpler_state.goal_entities])) is_goal_obj = np.tile(is_goal_obj, (n_key_configs, 1)).reshape( (1, n_key_configs, 2, 1)) is_goal_region = utils.convert_binary_vec_to_one_hot( np.array([smpler_state.region in smpler_state.goal_entities])) is_goal_region = np.tile(is_goal_region, (n_key_configs, 1)).reshape( (1, n_key_configs, 2, 1)) state_vec = np.concatenate([state_vec, is_goal_obj, is_goal_region], axis=2) poses = np.hstack([ utils.encode_pose_with_sin_and_cos_angle( utils.get_body_xytheta(obj).squeeze()), 0, 0, 0, 0 ]).reshape((1, 8)) return state_vec, poses
def check_place_feasible(self, pick_parameters, place_parameters, operator_skeleton): pick_op = Operator('two_arm_pick', operator_skeleton.discrete_parameters) pick_op.continuous_parameters = pick_parameters # todo remove the CustomStateSaver #saver = utils.CustomStateSaver(self.problem_env.env) original_config = utils.get_body_xytheta( self.problem_env.robot).squeeze() pick_op.execute() place_op = Operator('two_arm_place', operator_skeleton.discrete_parameters) place_cont_params, place_status = TwoArmPlaceFeasibilityChecker.check_feasibility( self, place_op, place_parameters) utils.two_arm_place_object(pick_op.continuous_parameters) utils.set_robot_config(original_config) #saver.Restore() return place_cont_params, place_status
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None if not self.use_shortest_path: continue if pick_path is None: continue if parent_state is not None and obj in parent_state.cached_pick_paths and obj != moved_obj: self.cached_place_paths[( obj, region_name)] = parent_state.cached_place_paths[( obj, region_name)] else: saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains( self.problem_env.env.GetKinBody( obj).ComputeAABB()): self.cached_place_paths[(obj, region_name)] = [ get_body_xytheta(self.problem_env.robot).squeeze() ] else: self.cached_place_paths[( obj, region_name)] = motion_planner.get_motion_plan( region, cached_collisions={})[0] if self.cached_place_paths[(obj, region_name)] is None: import pdb pdb.set_trace() saver.Restore() try: assert self.cached_place_paths[(obj, region_name)] is not None except: import pdb pdb.set_trace()
def get_node_features(self, entity, goal_entities): isobj = entity not in self.problem_env.regions obj = self.problem_env.env.GetKinBody(entity) if isobj else None pose = get_body_xytheta(obj)[0] if isobj else None is_entity_reachable = self.is_reachable(entity) # todo turn these into one-hot encoding # todo I need is_holding predicate for not putting high value on regions return [ 0, # l 0, # w 0, # h pose[0] if isobj else 0, # x pose[1] if isobj else 0, # y pose[2] if isobj else 0, # theta entity not in self.problem_env.regions, # IsObj entity in self.problem_env.regions, # IsRoom entity in self.goal_entities, # IsGoal is_entity_reachable, self.is_holding_goal_entity(), ]
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None parent_state_has_cached_path_for_obj \ = parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj cached_path_is_shortest_path = parent_state is not None and \ not (obj, region_name) in parent_state.reachable_regions_while_holding saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains( self.problem_env.env.GetKinBody(obj).ComputeAABB()): path = [get_body_xytheta(self.problem_env.robot).squeeze()] self.reachable_regions_while_holding.append( (obj, region_name)) else: if self.holding_collides is not None: path, status = motion_planner.get_motion_plan( region, cached_collisions=self.holding_collides) else: path, status = motion_planner.get_motion_plan( region, cached_collisions=self.collides) if status == 'HasSolution': self.reachable_regions_while_holding.append( (obj, region_name)) else: if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path: path = parent_state.cached_place_paths[( obj, region_name)] else: path, _ = motion_planner.get_motion_plan( region, cached_collisions={}) saver.Restore() # assert path is not None self.cached_place_paths[(obj, region_name)] = path
def get_object_pose_collisions(self): def in_collision_with_obj(q, obj): utils.set_rightarm_torso(q, self.problem_env.robot) col = self.problem_env.env.CheckCollision(self.problem_env.robot, obj) return col obj_name_to_pose = { obj.GetName(): tuple(utils.get_body_xytheta(obj)[0].round(6)) for obj in self.problem_env.objects } parent_collides = None if self.parent_state is None else self.parent_state.konf_collisions_with_obj_poses collisions_at_all_obj_pose_pairs = {} for obj in self.problem_env.objects: obj_name_pose_tuple = (obj.GetName(), obj_name_to_pose[obj.GetName()]) collisions_with_obj_did_not_change = parent_collides is not None and obj_name_pose_tuple in parent_collides if collisions_with_obj_did_not_change: konfs_in_collision_with_obj = parent_collides[obj_name_pose_tuple] else: konfs_in_collision_with_obj = {i for i, q in enumerate(self.key_configs) if in_collision_with_obj(q, obj)} collisions_at_all_obj_pose_pairs[obj_name_pose_tuple] = konfs_in_collision_with_obj return collisions_at_all_obj_pose_pairs
def get_pap_param_with_feasible_motion_plan(self, operator_skeleton, feasible_op_parameters, cached_collisions, cached_holding_collisions): # getting pick motion - I can still use the cached collisions from state computation pick_op_params = [op['pick'] for op in feasible_op_parameters] chosen_pick_param = self.get_op_param_with_feasible_motion_plan( pick_op_params, cached_collisions) if not chosen_pick_param['is_feasible']: return {'is_feasible': False} target_obj = operator_skeleton.discrete_parameters['object'] if target_obj in self.feasible_pick_params: self.feasible_pick_params[target_obj].append(chosen_pick_param) else: self.feasible_pick_params[target_obj] = [chosen_pick_param] # self.feasible_pick_params[target_obj].append(pick_op_params) # getting place motion original_config = utils.get_body_xytheta( self.problem_env.robot).squeeze() utils.two_arm_pick_object( operator_skeleton.discrete_parameters['object'], chosen_pick_param) place_op_params = [op['place'] for op in feasible_op_parameters] chosen_place_param = self.get_op_param_with_feasible_motion_plan( place_op_params, cached_holding_collisions) utils.two_arm_place_object(chosen_pick_param) utils.set_robot_config(original_config) if not chosen_place_param['is_feasible']: return {'is_feasible': False} chosen_pap_param = { 'pick': chosen_pick_param, 'place': chosen_place_param, 'is_feasible': True } return chosen_pap_param
def __init__(self): ProblemEnvironment.__init__(self) problem = MoverProblem(self.env) self.problem_config = problem.get_problem_config() self.robot = self.env.GetRobots()[0] self.objects = self.problem_config['packing_boxes'] self.object_init_poses = { o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects } self.init_base_conf = np.array([0, 1.05, 0]) self.regions = { 'entire_region': self.problem_config['entire_region'], 'home_region': self.problem_config['home_region'], 'loading_region': self.problem_config['loading_region'] } self.placement_regions = { 'home_region': self.problem_config['home_region'], 'loading_region': self.problem_config['loading_region'] } self.entity_names = [obj.GetName() for obj in self.objects] + list(self.regions) self.entity_idx = { name: idx for idx, name in enumerate(self.entity_names) } self.is_init_pick_node = True self.name = 'mover' self.init_saver = CustomStateSaver(self.env) self.problem_config['env'] = self.env self.operator_names = ['two_arm_pick', 'two_arm_place'] self.reward_function = None self.applicable_op_constraint = None self.two_arm_pick_continuous_constraint = None self.two_arm_place_continuous_constraint = None self.objects_to_check_collision = None
def sample_placements(self, pose_ids, collisions, pick_samples, n_smpls): stttt = time.time() obj_kinbody = self.abstract_state.problem_env.env.GetKinBody(self.obj) stime = time.time() obj_xyth = utils.get_body_xytheta(obj_kinbody) print 'objxytheta time', time.time() - stime stime = time.time() pick_abs_poses = [] for s in pick_samples: _, poses = utils.get_pick_base_pose_and_grasp_from_pick_parameters( obj_kinbody, s, obj_xyth) pick_abs_poses.append(poses) print "Pick abs pose time", time.time() - stime stime = time.time() encoded_pick_abs_poses = np.array([ utils.encode_pose_with_sin_and_cos_angle(s) for s in pick_abs_poses ]) print "Pick pose encoding time", time.time() - stime pose_ids[:, -6:-2] = encoded_pick_abs_poses if 'home' in self.region: chosen_sampler = self.samplers['place_goal_region'] else: chosen_sampler = self.samplers['place_obj_region'] stime = time.time() place_samples = chosen_sampler.generate(self.state_vec, pose_ids) print "prediction time", time.time() - stime stime = time.time() place_samples = np.array([ utils.decode_pose_with_sin_and_cos_angle(s) for s in place_samples ]) print "place decoding time", time.time() - stime # print time.time() - stttt return place_samples
def get_node_features(self, entity, goal_entities): isobj = entity not in self.problem_env.regions obj = self.problem_env.env.GetKinBody(entity) if isobj else None pose = get_body_xytheta(obj)[0] if isobj else None if isobj: is_entity_reachable = entity in self.nocollision_pick_op else: is_entity_reachable = True return [ 0, # l 0, # w 0, # h pose[0] if isobj else 0, # x pose[1] if isobj else 0, # y pose[2] if isobj else 0, # theta entity not in self.problem_env.regions, # IsObj entity in self.problem_env.regions, # IsRoom entity in self.goal_entities, # IsGoal is_entity_reachable, self.is_holding_goal_entity(), ]
def get_node_features(self, entity, goal_entities): isobj = entity not in self.problem_env.regions obj = self.problem_env.env.GetKinBody(entity) if isobj else None pose = get_body_xytheta(obj)[0] if isobj else None if isobj: if self.use_shortest_path: motion_planner = BaseMotionPlanner(self.problem_env, 'prm') pick_for_obj = self.pick_used[obj.GetName()] plan, status = motion_planner.get_motion_plan( pick_for_obj.continuous_parameters['potential_q_goals'], cached_collisions=self.collides) pick_for_obj.low_level_motion = plan if status == 'HasSolution': pick_for_obj.continuous_parameters['q_goal'] = plan[-1] self.reachable_entities.append(entity) is_entity_reachable = True else: is_entity_reachable = False else: is_entity_reachable = obj.GetName() in self.reachable_entities else: is_entity_reachable = True return [ 0, # l 0, # w 0, # h pose[0] if isobj else 0, # x pose[1] if isobj else 0, # y pose[2] if isobj else 0, # theta entity not in self.problem_env.regions, # IsObj entity in self.problem_env.regions, # IsRoom entity in self.goal_entities, # IsGoal is_entity_reachable, self.is_holding_goal_entity(), ]
def __init__(self, problem_env, goal_entities, parent_state=None, parent_action=None, paps_used_in_data=None, use_shortest_path=False): self.state_saver = CustomStateSaver(problem_env.env) self.problem_env = problem_env self.parent_state = parent_state self.goal_entities = goal_entities # raw variables self.robot_pose = get_body_xytheta(problem_env.robot) self.object_poses = { obj.GetName(): get_body_xytheta(obj) for obj in problem_env.objects } # cached info self.use_prm = problem_env.name.find('two_arm') != -1 if self.use_prm: self.collides, self.current_collides = self.update_collisions_at_prm_vertices( parent_state) else: self.collides = None self.current_collides = None # adopt from parent predicate evaluations self.parent_ternary_predicates = {} self.parent_binary_predicates = {} if parent_state is not None and paps_used_in_data is None: assert parent_action is not None moved_obj_type = type( parent_action.discrete_parameters['two_arm_place_object']) if moved_obj_type == str or moved_obj_type == unicode: moved_obj = parent_action.discrete_parameters[ 'two_arm_place_object'] else: moved_obj = parent_action.discrete_parameters[ 'two_arm_place_object'].GetName() self.parent_ternary_predicates = { (a, b, r): v for (a, b, r), v in parent_state.ternary_edges.items() if a != moved_obj and b != moved_obj } self.parent_binary_predicates = { (a, b): v for (a, b), v in parent_state.binary_edges.items() if a != moved_obj and b != moved_obj } self.use_shortest_path = False self.cached_place_paths = {} if paps_used_in_data is None: self.pick_used = {} self.place_used = {} else: self.pick_used = paps_used_in_data[0] self.place_used = paps_used_in_data[1] self.mc_pick_path = {} self.mc_place_path = {} self.reachable_entities = [] # predicates self.pick_in_way = PickInWay(self.problem_env, collides=self.current_collides, pick_poses=self.pick_used, use_shortest_path=self.use_shortest_path) self.place_in_way = PlaceInWay( self.problem_env, collides=self.current_collides, pick_poses=self.pick_used, use_shortest_path=self.use_shortest_path) self.in_region = InRegion(self.problem_env) self.is_holding_goal_entity = IsHoldingGoalEntity( self.problem_env, goal_entities) self.ternary_edges = self.get_ternary_edges() self.binary_edges = self.get_binary_edges() self.nodes = self.get_nodes()
def get_motion_plan(self, goal, region_name='entire_region', n_iterations=None, cached_collisions=None, source='', return_start_set_and_path_idxs=False): self.problem_env.robot.SetActiveDOFs( [], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) if region_name == 'bridge_region': region_name = 'entire_region' region_x = self.problem_env.problem_config[region_name + '_xy'][0] region_y = self.problem_env.problem_config[region_name + '_xy'][1] region_x_extents = self.problem_env.problem_config[region_name + '_extents'][0] region_y_extents = self.problem_env.problem_config[region_name + '_extents'][1] d_fn = base_distance_fn(self.problem_env.robot, x_extents=region_x_extents, y_extents=region_y_extents) s_fn = base_sample_fn(self.problem_env.robot, x_extents=region_x_extents, y_extents=region_y_extents, x=region_x, y=region_y) e_fn = base_extend_fn(self.problem_env.robot) if cached_collisions is not None and self.algorithm == 'prm': c_fn = set() for tmp in cached_collisions.values(): c_fn = c_fn.union(tmp) else: c_fn = collision_fn(self.problem_env.env, self.problem_env.robot) q_init = utils.get_body_xytheta(self.problem_env.robot).squeeze() if n_iterations is None: n_iterations = [20, 50, 100, 500, 1000] path = None status = 'NoSolution' if self.algorithm == 'rrt': print "Calling an RRT" planning_algorithm = rrt_connect #assert cached_collisions is None if not isinstance(goal, list): goal = [goal] for n_iter in n_iterations: #print n_iter for g in goal: path = planning_algorithm(q_init, g, d_fn, s_fn, e_fn, c_fn, iterations=n_iter) if path is not None: return path, 'HasSolution' else: print "Calling PRM connect" planning_algorithm = prm_connect if return_start_set_and_path_idxs: path, start_and_prm_idxs = planning_algorithm( q_init, goal, c_fn, source, return_start_set_and_path_idxs) else: path = planning_algorithm(q_init, goal, c_fn, source, return_start_set_and_path_idxs) if path is not None: status = "HasSolution" if return_start_set_and_path_idxs: return path, status, start_and_prm_idxs else: return path, status
def get_pap_param_with_feasible_motion_plan(self, operator_skeleton, feasible_op_parameters, cached_collisions, cached_holding_collisions): # getting pick motion - I can still use the cached collisions from state computation n_feasible = len(feasible_op_parameters) n_mp_tried = 0 obj_poses = { o.GetName(): utils.get_body_xytheta(o) for o in self.problem_env.objects } prepick_q0 = utils.get_body_xytheta(self.problem_env.robot) all_mp_data = [] for op in feasible_op_parameters: print "n_mp_tried / n_feasible_params = %d / %d" % (n_mp_tried, n_feasible) chosen_pick_param = self.get_op_param_with_feasible_motion_plan( [op['pick']], cached_collisions) n_mp_tried += 1 mp_data = { 'q0': prepick_q0, 'qg': op['pick']['q_goal'], 'object_poses': obj_poses, 'held_obj': None } if not chosen_pick_param['is_feasible']: print "Pick motion does not exist" mp_data['label'] = False all_mp_data.append(mp_data) continue else: mp_data['label'] = True mp_data['motion'] = chosen_pick_param['motion'] all_mp_data.append(mp_data) original_config = utils.get_body_xytheta( self.problem_env.robot).squeeze() utils.two_arm_pick_object( operator_skeleton.discrete_parameters['object'], chosen_pick_param) mp_data = { 'q0': op['pick']['q_goal'], 'qg': op['place']['q_goal'], 'object_poses': obj_poses, 'held_obj': operator_skeleton.discrete_parameters['object'], 'region': operator_skeleton.discrete_parameters['place_region'] } chosen_place_param = self.get_op_param_with_feasible_motion_plan( [op['place']], cached_holding_collisions) # calls MP utils.two_arm_place_object(chosen_pick_param) utils.set_robot_config(original_config) if chosen_place_param['is_feasible']: mp_data['label'] = True mp_data['motion'] = chosen_place_param['motion'] all_mp_data.append(mp_data) print 'Motion plan exists' break else: mp_data['label'] = False all_mp_data.append(mp_data) print "Place motion does not exist" pickle.dump( all_mp_data, open( './planning_experience/motion_planning_experience/' + str(uuid.uuid4()) + '.pkl', 'wb')) if not chosen_pick_param['is_feasible']: print "Motion plan does not exist" return {'is_feasible': False} if not chosen_place_param['is_feasible']: print "Motion plan does not exist" return {'is_feasible': False} chosen_pap_param = { 'pick': chosen_pick_param, 'place': chosen_place_param, 'is_feasible': True } return chosen_pap_param
def sample_from_discretized_set(self): (pick_tf, pick_params), (place_tf, place_params) = random.choice(zip(*self.cached_picks)) pick_region = self.problem_env.get_region_containing(self.target_obj) place_region = self.place_op.discrete_parameters['place_region'] pick_params = copy.deepcopy(pick_params) place_params = copy.deepcopy(place_params) old_tf = self.target_obj.GetTransform() old_pose = get_body_xytheta(self.target_obj).squeeze() self.pick_op.continuous_parameters = place_params self.target_obj.SetTransform(place_tf) set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:]) # This is the only sampling here place_pose = self.place_generator.sample_from_uniform() place_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj, place_pose, place_region) if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision( self.target_obj): self.target_obj.SetTransform(old_tf) return None, None, 'InfeasibleIK' if not place_region.contains(self.target_obj.ComputeAABB()): self.target_obj.SetTransform(old_tf) return None, None, 'InfeasibleIK' place_params['operator_name'] = 'one_arm_place' place_params['object_pose'] = place_pose place_params['action_parameters'] = place_pose place_params['base_pose'] = place_base_pose place_params['q_goal'][-3:] = place_base_pose self.place_op.continuous_parameters = place_params self.pick_op.continuous_parameters = pick_params # is reference and will be changed lader self.target_obj.SetTransform(pick_tf) # assert_region(pick_region) # self.pick_op.execute() set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:]) pick_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj, old_pose, pick_region) pick_params['q_goal'][-3:] = pick_base_pose # assert_region(pick_region) # self.target_obj.SetTransform(pick_tf) # self.pick_op.execute() # tf = np.dot(np.dot(old_tf, np.linalg.pinv(pick_tf)), self.problem_env.robot.GetTransform()) # self.place_op.execute() # self.target_obj.SetTransform(old_tf) # self.problem_env.robot.SetTransform(tf) # pick_base_pose = get_body_xytheta(self.problem_env.robot).squeeze() # pick_params['q_goal'][-3:] = pick_base_pose bad = False self.target_obj.SetTransform(old_tf) self.pick_op.execute() # assert_region(pick_region) if self.problem_env.env.CheckCollision(self.problem_env.robot): release_obj() self.target_obj.SetTransform(old_tf) return None, None, 'InfeasibleIK' self.place_op.execute() if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision( self.target_obj): self.target_obj.SetTransform(old_tf) return None, None, 'InfeasibleIK' if not self.place_op.discrete_parameters['place_region'].contains(self.target_obj.ComputeAABB()): self.target_obj.SetTransform(old_tf) return None, None, 'InfeasibleIK' self.target_obj.SetTransform(old_tf) return pick_params, place_params, 'HasSolution'