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
