def is_collision_and_region_constraints_satisfied(self, target_robot_region1, target_robot_region2, target_obj_region): target_region_contains = target_robot_region1.contains(self.robot.ComputeAABB()) or \ target_robot_region2.contains(self.robot.ComputeAABB()) obj = self.robot.GetGrabbed()[0] if not target_region_contains: return False if not target_obj_region.contains(obj.ComputeAABB()): return False is_object_pose_infeasible = self.env.CheckCollision(obj) if is_object_pose_infeasible: return False is_base_pose_infeasible = self.env.CheckCollision(self.robot) if is_base_pose_infeasible: return False else: dof_vals = self.robot.GetDOFValues() utils.release_obj() utils.fold_arms() self.problem_env.set_robot_to_default_dof_values() is_base_pose_infeasible = self.env.CheckCollision(self.robot) self.robot.SetDOFValues(dof_vals) utils.grab_obj(obj) if is_base_pose_infeasible: return False return True
def restore(self, state_saver): curr_obj = state_saver.curr_obj which_operator = state_saver.which_operator if not which_operator == 'two_arm_pick': grab_obj(self.robot, curr_obj) else: if len(self.robot.GetGrabbed()) > 0: release_obj(self.robot, self.robot.GetGrabbed()[0]) state_saver.Restore() self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
def place_object_and_robot_at_new_pose(self, obj, obj_pose, obj_region): T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), self.robot.GetTransform()) if len(self.robot.GetGrabbed()) > 0: release_obj() set_obj_xytheta(obj_pose, obj) new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o) self.robot.SetTransform(new_T_robot) new_base_pose = get_body_xytheta(self.robot) set_robot_config(new_base_pose, self.robot) fold_arms() set_point(obj, np.hstack([obj_pose[0:2], obj_region.z + 0.001])) return new_base_pose
def compute_robot_base_pose_given_object_pose(obj, robot, obj_pose, T_r_wrt_o): original_robot_T = robot.GetTransform() release_obj(robot, obj) set_obj_xytheta(obj_pose, obj) new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o) robot.SetTransform(new_T_robot) robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) robot_xytheta = robot.GetActiveDOFValues() grab_obj(robot, obj) robot.SetTransform(original_robot_T) return robot_xytheta
def get_objects_in_collision_with_given_op_inst(self, op_inst): saver = CustomStateSaver(self.problem_env.env) if len(self.problem_env.robot.GetGrabbed()) > 0: held = self.problem_env.robot.GetGrabbed()[0] release_obj() else: held = None fold_arms() new_cols = self.problem_env.get_objs_in_collision(op_inst.low_level_motion, 'entire_region') saver.Restore() if held is not None: grab_obj(held) return new_cols
def is_swept_volume_cleared(self, obj): saver = CustomStateSaver(self.problem_env.env) if len(self.problem_env.robot.GetGrabbed()) > 0: held = self.problem_env.robot.GetGrabbed()[0] release_obj() else: held = None collision_occurred = self.pick_swept_volume.is_collision_in_all_volumes(obj) \ or self.place_swept_volume.is_collision_in_all_volumes(obj) saver.Restore() if held is not None: grab_obj(held) if collision_occurred: return False else: return True
def check_collision_in_pap(self, pick_op, place_op, obj_object, swept_volume): old_tf = obj_object.GetTransform() collision = False pick_op.execute() if self.problem_env.env.CheckCollision(self.problem_env.robot): utils.release_obj() obj_object.SetTransform(old_tf) return True place_op.execute() if self.problem_env.env.CheckCollision(self.problem_env.robot): obj_object.SetTransform(old_tf) return True if self.problem_env.env.CheckCollision(obj_object): obj_object.SetTransform(old_tf) return True is_object_pose_infeasible = not swept_volume.is_swept_volume_cleared(obj_object) if is_object_pose_infeasible: obj_object.SetTransform(old_tf) return True obj_object.SetTransform(old_tf) return collision
def get_placement(self, obj, target_obj_region, T_r_wrt_o): original_trans = self.robot.GetTransform() original_config = self.robot.GetDOFValues() self.robot.SetTransform(original_trans) self.robot.SetDOFValues(original_config) release_obj(self.robot, obj) with self.robot: # print target_obj_region obj_pose = randomly_place_in_region( self.env, obj, target_obj_region) # randomly place obj obj_pose = obj_pose.squeeze() # compute the resulting robot transform new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o) self.robot.SetTransform(new_T_robot) self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) robot_xytheta = self.robot.GetActiveDOFValues() set_robot_config(robot_xytheta, self.robot) grab_obj(self.robot, obj) return obj_pose, robot_xytheta
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'
def check_feasibility(self, operator_skeleton, place_parameters, swept_volume_to_avoid=None): # Note: # this function checks if the target region contains the robot when we place object at place_parameters # and whether the robot will be in collision obj = self.robot.GetGrabbed()[0] obj_region = operator_skeleton.discrete_parameters['region'] if type(obj_region) == str: obj_region = self.problem_env.regions[obj_region] obj_pose = place_parameters T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), self.robot.GetTransform()) original_trans = self.robot.GetTransform() original_obj_trans = obj.GetTransform() target_robot_region1 = self.problem_env.regions['home_region'] target_robot_region2 = self.problem_env.regions['loading_region'] target_obj_region = obj_region # for fetching, you want to move it around robot_xytheta = self.compute_robot_base_pose_given_object_pose( obj, self.robot, obj_pose, T_r_wrt_o) set_robot_config(robot_xytheta, self.robot) # why do I disable objects in region? Most likely this is for minimum constraint computation #self.problem_env.disable_objects_in_region('entire_region') target_region_contains = target_robot_region1.contains(self.robot.ComputeAABB()) or \ target_robot_region2.contains(self.robot.ComputeAABB()) is_base_pose_infeasible = self.env.CheckCollision(self.robot) or \ (not target_region_contains) is_object_pose_infeasible = self.env.CheckCollision(obj) or \ (not target_obj_region.contains(obj.ComputeAABB())) #self.problem_env.enable_objects_in_region('entire_region') if is_base_pose_infeasible or is_object_pose_infeasible: action = { 'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None, 'action_parameters': place_parameters } self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) return action, 'NoSolution' else: release_obj(self.robot, obj) if swept_volume_to_avoid is not None: # release the object if not swept_volume_to_avoid.is_swept_volume_cleared(obj): self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(self.robot, obj) action = { 'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None, 'action_parameters': place_parameters } return action, 'NoSolution' """ for config in swept_volume_to_avoid: set_robot_config(config, self.robot) if self.env.CheckCollision(self.robot, obj): self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(self.robot, obj) action = {'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None, 'action_parameters': place_parameters} return action, 'NoSolution' """ self.robot.SetTransform(original_trans) obj.SetTransform(original_obj_trans) grab_obj(self.robot, obj) action = { 'operator_name': 'two_arm_place', 'base_pose': robot_xytheta, 'object_pose': obj_pose, 'action_parameters': place_parameters } return action, 'HasSolution'
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 sample_cont_params(self): # todo refactor this function robot_pose = utils.get_body_xytheta(self.robot) robot_config = self.robot.GetDOFValues() assert len(self.robot.GetGrabbed()) == 0 if self.cached_picks is not None: (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['region'] pick_params = copy.deepcopy(pick_params) place_params = copy.deepcopy(place_params) #assert pick_params['operator_name'] == 'one_arm_pick' #assert place_params['operator_name'] == 'one_arm_pick' 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) #self.pick_op.execute() set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:]) 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' def assert_region(region): try: assert self.problem_env.get_region_containing(self.target_obj).name == region.name return True except Exception as e: print(e) set_color(self.target_obj, [1,1,1]) import pdb pdb.set_trace() return False if not place_region.contains(self.target_obj.ComputeAABB()): self.target_obj.SetTransform(old_tf) return None, None, 'InfeasibleIK' #assert_region(place_region) 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['region'].contains(self.target_obj.ComputeAABB()): self.target_obj.SetTransform(old_tf) return None, None, 'InfeasibleIK' #assert_region(place_region) #place_action = { # 'operator_name': 'one_arm_place', # 'q_goal': np.hstack([grasp_config, new_base_pose]), # 'base_pose': new_base_pose, # 'object_pose': place_parameters, # 'action_parameters': obj_pose, # 'g_config': grasp_config, # 'grasp_params': grasp_params, #} #pick_action = { # 'operator_name': operator_skeleton.type, # 'q_goal': np.hstack([grasp_config, pick_base_pose]), # 'grasp_params': grasp_params, # 'g_config': grasp_config, # 'action_parameters': pick_parameters, #} self.target_obj.SetTransform(old_tf) #assert pick_params['operator_name'] == 'one_arm_pick' #assert place_params['operator_name'] == 'one_arm_place' return pick_params, place_params, 'HasSolution' # sample pick pick_cont_params = None n_ik_attempts = 0 n_base_attempts = 0 status = "NoSolution" 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 == 500: 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 == 500: break 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 main(): config = parse_arguments() solution_file_name = get_solution_file_name(config) is_problem_solved_before = os.path.isfile(solution_file_name) if config.gather_planning_exp: assert config.h_option == 'hcount_old_number_in_goal' #config.timelimit = goal_objs, goal_region = get_goal_obj_and_region(config) print "Goal:", goal_objs, goal_region problem_env = get_problem_env(config, goal_region, goal_objs) set_problem_env_config(problem_env, config) if config.v: problem_env.env.SetViewer('qtcoin') if is_problem_solved_before and not config.f: print "***************Already solved********************" with open(solution_file_name, 'rb') as f: data = pickle.load(f) success = data['success'] tottime = data['tottime'] num_nodes = data['num_nodes'] plan_length = len(data['plan']) if success else 0 print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d N_feasible: %d' % ( tottime, success, plan_length, num_nodes, data['n_feasibility_checks']['ik']) else: is_use_gnn = 'qlearned' in config.h_option if is_use_gnn: pap_model = get_pap_gnn_model(problem_env, config) else: pap_model = None t = time.time() np.random.seed(config.planner_seed) random.seed(config.planner_seed) learned_sampler_model = get_learned_sampler_models(config) nodes_to_goal, plan, num_nodes, nodes = search(problem_env, config, pap_model, goal_objs, goal_region, learned_sampler_model) tottime = time.time() - t n_feasibility_checks = get_total_n_feasibility_checks(nodes) success = plan is not None plan_length = len(plan) if success else 0 if success and config.domain == 'one_arm_mover': make_pklable(plan) if config.gather_planning_exp: [make_node_pklable(nd) for nd in nodes] else: nodes = None h_for_sampler_training = [] if success: h_for_sampler_training = [] for n in nodes_to_goal: h_for_sampler_training.append(n.h_for_sampler_training) data = { 'n_objs_pack': config.n_objs_pack, 'tottime': tottime, 'success': success, 'plan_length': plan_length, 'num_nodes': num_nodes, 'plan': plan, 'nodes': nodes, 'hvalues': h_for_sampler_training, 'n_feasibility_checks': n_feasibility_checks } with open(solution_file_name, 'wb') as f: pickle.dump(data, f) print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d' % (tottime, success, plan_length, num_nodes) print(data['n_feasibility_checks']) problem_env.reset_to_init_state_stripstream() color = get_color_of(problem_env.objects[0]) for obj in problem_env.objects: utils.set_color(obj, [0.0, 0.7, 0.0]) raw_input('continue?') for op in data['plan']: o = op.discrete_parameters['object'] obj = problem_env.env.GetKinBody(o) utils.set_color(obj, [0.8, 0, 0]) raw_input('continue?') utils.set_robot_config([1000,1000,0]) pick_skeleton = Operator('two_arm_pick', {'object': o}) generator = UniformGenerator(pick_skeleton, problem_env, None) for i in range(20): param = generator.sample_next_point(pick_skeleton, n_iter=10, n_parameters_to_try_motion_planning=1, dont_check_motion_existence=True) if param['is_feasible']: utils.draw_robot_at_conf(param['q_goal'], .5, 'pick' + str(i), problem_env.robot, problem_env.env) raw_input('continue?') utils.remove_drawn_configs('pick') utils.visualize_path(op.continuous_parameters['pick']['motion']) # visualize_path calls raw_input and remove_drawn_configs op.execute_pick() raw_input('continue?') utils.set_color(obj, color) utils.release_obj() op.execute() raw_input('continue?')