def get_obstacle_re(ob, target_ori, obs_pos_in, Body_position, d_max): ''' :param ob: :param target_ori: :param obs_pos_in: :param Body_position: :param d_max: :return: ''' import copy from VFHplus_change_radius import influence obstacle_rearr = [] obs_pos = copy.deepcopy(obs_pos_in) vfh, km = influence(ob, target_ori, obs_pos, Body_position, d_max) if vfh == 1: return 0 while 1: ob = len(obs_pos) vfh, km = influence(ob, target_ori, obs_pos, Body_position, d_max) if vfh == 1: return obstacle_rearr elif vfh == 0: obstacle_rearr.append(obs_pos[km[0][1]]) target_ori = obs_pos[km[0][1]] obs_pos.remove(obs_pos[km[0][1]])
def get_obstacle_re(ob, target_ori, obs_pos_in, Body_position, d_max, eta): obstacle_rearr = [] obs_pos = copy.deepcopy(obs_pos_in) vfh, km = influence(ob, target_ori, obs_pos, Body_position, d_max, eta) influence() if vfh == 1: # print("no need to rearrange") return 0 while 1: ob = len(obs_pos) vfh, km = influence(ob, target_ori, obs_pos, Body_position, d_max) print("km", km, "to remove", obs_pos[km]) print("ob :", ob) print("target_ori :", target_ori) print("obs_pos :", obs_pos) print("Body_position :", Body_position, "\n") if vfh == 1: # obstacle_rearr.append(obs_pos[km[0][1]]) # obstacle_rearr.append(target_ori) # print("find way to rearrange \n list is:", obstacle_rearr) return obstacle_rearr elif vfh == 0: # print("we have to rearrange:", obs_pos[km[0][1]]) obstacle_rearr.append([obs_pos[km], km]) target_ori = obs_pos[km] obs_pos.remove(obs_pos[km])
def get_can_BT(in_can_info, in_obs_pos, in_tar_pos, obs_wall): import copy from VFHplus_change_radius import influence tmp_can_info = copy.deepcopy(in_can_info) bt_list = [] # print("\nCheck if candidate blocks the target") for ci in range(len(tmp_can_info)): vfh_obs_pos = copy.deepcopy(in_obs_pos) vfh_obs_pos.append(tmp_can_info[ci].pos) vfh_obs_pos.extend(obs_wall) vfh_tar_pos = copy.deepcopy(in_tar_pos) ob = len(vfh_obs_pos) rob_pos = [0.0, 0.0] d_max = 2.0 eta = 45 vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, rob_pos, d_max, eta) if vfh[3] == 0: #print ci, "BT = 1 (vfh)" tmp_can_info[ ci].BT = 1 # BT == 1 : The candidate blocks the target. bt_list.append(1) else: #print ci, "BT = 0 (vfh)" tmp_can_info[ ci].BT = 0 # BT == 0 : The candidate does not block the target. bt_list.append(0) return tmp_can_info, bt_list
def get_can_A(in_can_info, in_obs_pos, in_tar_pos, obs_wall): import copy from VFHplus_change_radius import influence tmp_can_info = copy.deepcopy(in_can_info) a_list = [] # print("\nCheck if the candidate is accessible.") for ci in range(len(tmp_can_info)): vfh_tar_pos = copy.deepcopy(tmp_can_info[ci].pos) vfh_obs_pos = copy.deepcopy(in_obs_pos) vfh_obs_pos.append(copy.deepcopy(in_tar_pos)) vfh_obs_pos.extend(obs_wall) ob = len(vfh_obs_pos) rob_pos = [0.0, 0.0] d_max = 2.0 eta = 45 vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, rob_pos, d_max, eta) if vfh[3] == 0: # print ci, "A = 0 (vfh)" tmp_can_info[ci].A = 0 # A == 0 : The candidate is not accessible. a_list.append(0) else: # # print ci, "A = 1 (vfh)" tmp_can_info[ci].A = 1 # A == 1 : The candidate is accessible. a_list.append(1) return tmp_can_info, a_list
def get_can_A(self, in_can_info, in_obs_pos, in_tar_pos): tmp_can_info = copy.deepcopy(in_can_info) # print("\nCheck if the candidate is accessible.") for ci in range(len(tmp_can_info)): vfh_tar_pos = copy.deepcopy(tmp_can_info[ci].pos) vfh_obs_pos = copy.deepcopy(in_obs_pos) vfh_obs_pos.append(copy.deepcopy(in_tar_pos)) vfh_obs_pos.extend(self.obs_wall) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) if vfh[3] == 0: # A == 1 : The candidate is accessible. tmp_can_info[ci].A = 0 # A == 0 : The candidate is not accessible. else: # tmp_can_info[ci].A = 1 return tmp_can_info
def get_can_BT(self, in_can_info, in_obs_pos, in_tar_pos): tmp_can_info = copy.deepcopy(in_can_info) # print("\nCheck if candidate blocks the target") for ci in range(len(tmp_can_info)): vfh_obs_pos = copy.deepcopy(in_obs_pos) vfh_obs_pos.append(tmp_can_info[ci].pos) vfh_obs_pos.extend(self.obs_wall) vfh_tar_pos = copy.deepcopy(in_tar_pos) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) if vfh[3] == 0: tmp_can_info[ci].BT = 1 # BT == 1 : The candidate blocks the target. # else: # BT == 0 : The candidate does not block the target. # tmp_can_info[ci].BT = 0 # print "can", ci, "bt=0" return tmp_can_info
def get_cf_b(self, in_cf, in_obs_pos): tmp_cf = copy.deepcopy(in_cf) tmp_obs_pos = copy.deepcopy(in_obs_pos) tmp_b = [] for cb in range(len(tmp_cf)): # cb: The candidate that will check the b value b = 0 for ci in range(len(tmp_cf)): # ci: Other candidates for checking the b value if cb != ci: # print "\ntar", tmp_cf_pos[ci] # print "obs", tmp_obs_pos vfh_tar_pos = copy.deepcopy(tmp_cf[ci].pos) vfh_obs_pos = copy.deepcopy(tmp_obs_pos) vfh_obs_pos.append(tmp_cf[cb].pos) vfh_obs_pos.extend(self.obs_wall) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) if vfh[3] == 0: b = b + 1 tmp_b.append(b) return tmp_b
def get_b(in_can_info, in_obs_pos, obs_wall): import copy from VFHplus_change_radius import influence tmp_cf = [] cf_index = [] for ci in range(len(in_can_info)): if in_can_info[ci].A == 1 and in_can_info[ci].BT == 0: tmp_cf.append(in_can_info[ci]) cf_index.append(ci) tmp_b = [] for cb in range(len(tmp_cf)): tmp_obs_pos = copy.deepcopy(in_obs_pos) b = 0 for ci in range( len(tmp_cf)): # ci: Other candidates for checking the b value if cb != ci: print "check", cf_index[ ci], "' b if rearrange obstacle at", cf_index[cb] # print "obs", tmp_obs_pos vfh_tar_pos = copy.deepcopy(in_can_info[ci].pos) vfh_obs_pos = copy.deepcopy(tmp_obs_pos) vfh_obs_pos.append(in_can_info[cb].pos) vfh_obs_pos.extend(obs_wall) ob = len(vfh_obs_pos) rob_pos = [0.0, 0.0] d_max = 2.0 eta = 45 vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, rob_pos, d_max, eta) if vfh[3] == 0: print "blocking ADD +1" b = b + 1 tmp_b.append(b) return cf_index, tmp_b
print "\ncheck if candidate occlude the target" can_pan = [] for i in can_cen_grid: xi, yi = ws_zero_pos[0] + i[0] * GRID_SIZE, ws_zero_pos[ 1] + i[1] * GRID_SIZE vfh_t_pos = copy.deepcopy(tar_pos) vfh_o_pos = copy.deepcopy(obs_re_pos) vfh_o_pos.append([xi, yi]) vfh_ob = len(vfh_o_pos) vfh_o_ori = [] for ori_i in range(vfh_ob): vfh_o_ori.append([0.0, 0.0, 0.0]) vfh_t_ori = [0.0, 0.0, 0.0] vfh_ret = influence(vfh_ob, vfh_t_pos, vfh_o_pos, rob_pos, d_max, eta=45) # print("vfh", vfh) if vfh_ret[3] == 0: print "\ncandidate", i, "occludes the target", vfh_t_pos print "candidate penalty" can_pan.append(i) for i in can_pan: can_cen_grid.remove(i) print "\ncheck if the candidate occlude the obstacle to remove" can_pan = [] for i in can_cen_grid: for ore_posi in ore_pos: # print "working", i, ore_posi
def get_can_info(self, in_can_info, in_obs_pos, in_obs_re_pos, in_ore_order, in_tar_pos): tmp_can_info = [] for i in range(len(in_ore_order)): tmp_can_info.append(copy.deepcopy(in_can_info)) tmp_obs_pos = copy.deepcopy(in_obs_pos) tmp_obs_re_pos = copy.deepcopy(in_obs_re_pos) tmp_ore_order = copy.deepcopy(in_ore_order) tmp_tar_pos = copy.deepcopy(in_tar_pos) # print("\nCheck if candidate blocks the target") for step_i in range(len(tmp_ore_order)): for i in range(len(tmp_can_info[step_i])): vfh_obs_pos = copy.deepcopy(tmp_obs_re_pos) vfh_obs_pos.append(tmp_can_info[step_i][i].pos) vfh_obs_pos.extend(self.obs_wall) vfh_tar_pos = copy.deepcopy(tmp_tar_pos) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) # vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.obs_r, self.tar_r) if vfh[3] == 0: tmp_can_info[step_i][ i].BT = 1 # BT == 1 : The candidate blocks the target. else: # BT == 0 : The candidate does not block the target. tmp_can_info[step_i][i].BT = 0 # print("\nCheck if the candidate is accessible.") for i in range(len(tmp_can_info[step_i])): vfh_tar_pos = copy.deepcopy(tmp_can_info[step_i][i].pos) vfh_obs_pos = copy.deepcopy(tmp_obs_pos) for si in range(step_i + 1): # print "\nstep", si, "\nbefore", vfh_obs_pos vfh_obs_pos.remove(tmp_obs_pos[tmp_ore_order[si]]) # print "after", vfh_obs_pos vfh_obs_pos.append(tmp_tar_pos) vfh_obs_pos.extend(self.obs_wall) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) if vfh[3] == 0: # A == 1 : The candidate is accessible. tmp_can_info[step_i][ i].A = 0 # A == 0 : The candidate is not accessible. else: # tmp_can_info[step_i][i].A = 1 # print("\nCheck the candidate ORC.") for i in range(len(tmp_can_info[step_i])): vfh_tar_pos = copy.deepcopy(tmp_can_info[step_i][i].pos) vfh_obs_pos = copy.deepcopy(tmp_obs_pos) vfh_obs_pos.append(tmp_tar_pos) vfh_obs_pos.extend(self.obs_wall) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) if vfh[3] == 0: # A == 1 : The candidate is accessible. tmp_can_info[step_i][ i].A = 0 # A == 0 : The candidate is not accessible. tm_tar_pos = copy.deepcopy(vfh_tar_pos) tm_tar_ori = [0.0, 0.0, 0.0] tm_obs_pos = copy.deepcopy(tmp_obs_pos) # tm_obs_pos.extend(self.obs_wall) ob = len(tm_obs_pos) tm_obs_ori = [] for obs_ori_i in range(ob): tm_obs_ori.append([0.0, 0.0, 0.0]) ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [ self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE ]) # ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max) ore_order.pop( ) # The last order is always the target so we need to pop the last element. tmp_can_info[step_i][i].ORC = ore_order else: # tmp_can_info[step_i][i].A = 1 return tmp_can_info
for i in range(len(can_grid)): can_info.append(CI()) can_info[i].pos = can_grid[i] # print "candidate position :", can_info[i].pos print("\nCheck if candidate blocks the target") for i in range(len(can_info)): xi, yi = can_info[i].pos vfh_obs_pos = copy.deepcopy(obs_re_pos) vfh_obs_pos.append( [ws_zero[0] + xi * GRID_SIZE, ws_zero[1] + yi * GRID_SIZE]) vfh_obs_pos.extend(obs_wall) vfh_tar_pos = copy.deepcopy(tar_pos) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, rob_pos, d_max, eta) if vfh[3] == 0: can_info[ i].BT = 1 # BT == 1 : The candidate blocks the target. else: # BT == 0 : The candidate does not block the target. can_info[i].BT = 0 # print("\nCheck if the candidate is accessible.") for i in range(len(can_info)): xi, yi = can_info[i].pos vfh_tar_pos = [ ws_zero[0] + xi * GRID_SIZE, ws_zero[1] + yi * GRID_SIZE ] vfh_obs_pos = copy.deepcopy(obs_pos) vfh_obs_pos.append(tar_pos)
def place(self, in_obs_pos, place_pose, current_joints): print "place at:", place_pose vfh_obs_pos = copy.deepcopy(in_obs_pos) vfh_obs_pos.remove(place_pose) vfh_obs_pos.extend(self.obs_wall) vfh_tar_pos = copy.deepcopy(place_pose) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) if vfh[3] == 1: print "angle:", vfh[-1] xi, yi = place_pose[0], place_pose[1] planner_name = 'RRTConnect' # planner_name = 'BiTRRT' n_attempt = 100 c_time = 3 n_repeat = 5 start_state = moveit_msgs.msg.RobotState() joint_state = sensor_msgs.msg.JointState() joint_state.header = std_msgs.msg.Header() joint_state.name = ['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'] joint_state.position = current_joints # joint_state.position = [3.1415927410125732, 4.537856101989746, 5.93411922454834, -0.6108652353286743, 1.7453292608261108, -0.5235987901687622] start_state.joint_state = joint_state # goal_pose: # goal_orientation: # goal_pose = [self.object_z, -yi, xi - 0.05] # goal_orientation = [-0.00145713772037, -0.998970756926, 0.0364956710831, 0.0268955302573] z = self.object_z # goal_pose = [z + 0.02, -yi, xi] # Set the grasp pose: substract 17cm from the z value of the object centroid goal_pitches = [] goal_pitch = np.deg2rad(vfh[-1]) # goal_pitch = vfh[-1] + math.pi/2 goal_pitches.append(goal_pitch) # approaching_angle: vfh[-1] from the input for i in range(5): goal_pitches.append(goal_pitch + (i + 1) * (math.pi / 36)) goal_pitches.append(goal_pitch - (i + 1) * (math.pi / 36)) # Get the grasp orientation (currently the front direction) goal_orientations = [] for i in goal_pitches: goal_orientations.append(quaternion_from_euler(-i, math.radians(-5.0), math.radians(90.0), axes='rxyz')) # l = 0.001 l = 0.17 goal_poses = [] for i in goal_pitches: dx = math.sin(i - math.pi) * l dy = math.cos(i - math.pi) * l goal_poses.append([z + 0.2, -place_pose[1] + dx, place_pose[0] + dy]) feasibility1 = 0 i = 0 while not feasibility1 and i < len(goal_pitches): # CLF.add_box_client('can_bottom_x', [goal_poses[i][0] - 0.15, goal_poses[i][1], goal_poses[i][2]], goal_orientations[i], [0.1, 0.005, 0.005], 'red') # CLF.add_box_client('can_bottom_y', [goal_poses[i][0] - 0.15, goal_poses[i][1], goal_poses[i][2]], goal_orientations[i], [0.005, 0.1, 0.005], 'blue') # CLF.add_box_client('can_bottom_z', [goal_poses[i][0] - 0.15, goal_poses[i][1], goal_poses[i][2]], goal_orientations[i], [0.005, 0.005, 0.1], 'green') [feasibility1, trajectory1] = CLF.move_goalpose_client('arm', 'gripper', start_state, goal_poses[i], goal_orientations[i], [], planner_name, n_attempt, c_time, n_repeat) i = i + 1 # time.sleep(1) # CLF.del_box_client('can_bottom_x') # CLF.del_box_client('can_bottom_y') # CLF.del_box_client('can_bottom_z') return [goal_poses[i], goal_orientations[i]]
def get_can_BT(self, in_can_info, in_obs_pos, in_tar_pos): tmp_can_info = copy.deepcopy(in_can_info) # print("\nCheck if candidate blocks the target") for ci in range(len(tmp_can_info)): if tmp_can_info[ci].A == 1: vfh_obs_pos = copy.deepcopy(in_obs_pos) vfh_obs_pos.append(tmp_can_info[ci].pos) vfh_obs_pos.extend(self.obs_wall) vfh_tar_pos = copy.deepcopy(in_tar_pos) ob = len(vfh_obs_pos) vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta) if vfh[3] == 0: print ci, "BT = 1 (vfh)" tmp_can_info[ci].BT = 1 # BT == 1 : The candidate blocks the target. else: # BT == 0 : The candidate does not block the target. print ci, "BT = 0 (vfh)" print "\nangle:", vfh[-1] xi, yi = tmp_can_info[ci].pos[0], tmp_can_info[ci].pos[1] CLF.add_box_client('can_check', [self.object_z, -yi, xi], [-0.707, 0.0, -0.707, 0.0], [0.06, 0.06, 0.2], 'pink') planner_name = 'RRTConnect' # planner_name = 'BiTRRT' n_attempt = 10 c_time = 0.5 n_repeat = 5 start_state = moveit_msgs.msg.RobotState() joint_state = sensor_msgs.msg.JointState() joint_state.header = std_msgs.msg.Header() joint_state.name = ['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'] joint_state.position = [3.1415927410125732, 4.537856101989746, 5.93411922454834, -0.6108652353286743, 1.7453292608261108, -0.5235987901687622] start_state.joint_state = joint_state # goal_pose: # goal_orientation: # goal_pose = [self.object_z, -yi, xi - 0.05] # goal_orientation = [-0.00145713772037, -0.998970756926, 0.0364956710831, 0.0268955302573] z = self.object_z goal_pose = [z + 0.02, -yi, xi] # Set the grasp pose: substract 17cm from the z value of the object centroid goal_pitches = [] goal_pitch = np.deg2rad(vfh[-1]) # goal_pitch = vfh[-1] + math.pi/2 goal_pitches.append(goal_pitch) # approaching_angle: vfh[-1] from the input for i in range(1): goal_pitches.append(goal_pitch + (i + 1) * (math.pi / 36)) goal_pitches.append(goal_pitch - (i + 1) * (math.pi / 36)) # Get the grasp orientation (currently the front direction) goal_orientations = [] for i in goal_pitches: goal_orientations.append(quaternion_from_euler(-i, math.radians(-5.0), math.radians(90.0), axes='rxyz')) l = 0.001 goal_poses = [] for i in goal_pitches: dx = math.sin(i - math.pi) * l dy = math.cos(i - math.pi) * l goal_poses.append([z + 0.02, -in_tar_pos[1] + dx, in_tar_pos[0] + dy]) # goal_poses.append([z + 0.07, goal_pose[1] + dx, goal_pose[2] - dy]) # plt.figure() # for i in range(len(in_obs_pos)): # plt.scatter(in_obs_pos[i][0], in_obs_pos[i][1], s=200, c='red') # plt.scatter(xi, yi, s=200, c='pink') # plt.scatter(xi + dy, yi + dx, s=200, c='blue') # # plt.figure() # for i in range(len(in_obs_pos)): # plt.scatter(in_obs_pos[i][0], in_obs_pos[i][1], s=200, c='red') # plt.scatter(xi, yi, s=200, c='pink') # plt.scatter(xi + dy, yi - dx, s=200, c='blue') # # plt.show() feasibility1 = 0 i = 0 while not feasibility1 and i < len(goal_pitches): CLF.add_box_client('can_bottom_x', [goal_poses[i][0]-0.15, goal_poses[i][1], goal_poses[i][2]], goal_orientations[i], [0.1, 0.005, 0.005], 'red') CLF.add_box_client('can_bottom_y', [goal_poses[i][0]-0.15, goal_poses[i][1], goal_poses[i][2]], goal_orientations[i], [0.005, 0.1, 0.005], 'blue') CLF.add_box_client('can_bottom_z', [goal_poses[i][0]-0.15, goal_poses[i][1], goal_poses[i][2]], goal_orientations[i], [0.005, 0.005, 0.1], 'green') [feasibility1, trajectory1] = CLF.feasible_check_obj_joint_client('arm', 'gripper', start_state, goal_poses[i], goal_orientations[i], [], planner_name, n_attempt, c_time, n_repeat) i = i + 1 time.sleep(1) CLF.del_box_client('can_bottom_x') CLF.del_box_client('can_bottom_y') CLF.del_box_client('can_bottom_z') if feasibility1 == 0: # A == 1 : The candidate is accessible. print ci, "BT = 1 (MP)" tmp_can_info[ci].BT = 1 # A == 0 : The candidate is not accessible. else: # print ci, "BT = 0 (MP)" tmp_can_info[ci].BT = 0 CLF.del_box_client('can_check') else: print ci, "A = 0 => BT = no matter" return tmp_can_info