def grasp_centroid_data():
wd = '/home/lou00015/data/gsp/'
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
eid = 0
nb_grasp = 25
if cid != -1:
pos = [0, 0, 0.15]
while True:
vrep.simxStartSimulation(cid, vrep.simx_opmode_blocking)
panda = Robot(cid)
obj_name, obj_hdl = add_object(cid, 'imported_part_0', pos)
time.sleep(1.0)
cloud = panda.get_pointcloud()
centroid = np.average(cloud, axis=0)
print('centroid: ', centroid)
if len(cloud) == 0:
print('no cloud found')
continue
elif centroid[2] > 0.045:
print('perception error')
continue
np.save(wd + 'cloud_' + str(eid) + '.npy', cloud) # save point cloud
cloud = np.delete(cloud, np.where(cloud[:,2]<0.015), axis=0)
pose_set, pt_set = grasp_pose_generation(45, cloud, nb_grasp)
pose = pose_set[10]
pt = centroid
emptyBuff = bytearray()
landing_mtx = [pose[0][0],pose[0][1],pose[0][2],pt[0],
pose[1][0],pose[1][1], pose[1][2],pt[1],
pose[2][0],pose[2][1],pose[2][2],pt[2]]
np.save(wd + 'action_' + str(eid) + '.npy', landing_mtx) # save action
vrep.simxCallScriptFunction(cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
ending_mtx = [pose[0][0], pose[0][1], pose[0][2], pt[0],
pose[1][0], pose[1][1], pose[1][2], pt[1],
pose[2][0], pose[2][1], pose[2][2], pt[2]+0.15]
vrep.simxCallScriptFunction(cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx,
[], emptyBuff, vrep.simx_opmode_blocking)
time.sleep(1.0)
print('executing experiment %d: ' % (eid))
print('at: ', pt)
vrep.simxCallScriptFunction(cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
while True:
res, finish = vrep.simxGetIntegerSignal(cid, "finish", vrep.simx_opmode_oneshot_wait)
if finish == 18:
res, end_pos = vrep.simxGetObjectPosition(cid, obj_hdl, -1, vrep.simx_opmode_blocking)
break
if end_pos[2]>0.05:
label = 1
else:
label = 0
print(label)
f = open(wd + 'label.txt', 'a+')
f.write(str(label))
f.close()
eid += 1
else:
print('Failed to connect to simulation (V-REP remote API server). Exiting.')
exit()
def setup_sim(self):
# Connect to simulator
self.sim_client = -1
vrep.simxFinish(-1) # Just in case, close all opened connections
logging.info('Connecting to simulation...')
while self.sim_client == -1:
self.sim_client = vrep.simxStart('127.0.0.1', self.sim_port, True,
True, 5000, 5)
if self.sim_client == -1:
logging.error(
'Failed to connect to simulation. Trying again..')
time.sleep(5)
else:
logging.info('Connected to simulation.')
self.restart_sim()
break
# Get handle to camera
sim_ret, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_persp', vrep.simx_opmode_blocking)
# Get camera pose and intrinsics in simulation
sim_ret, cam_position = vrep.simxGetObjectPosition(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
sim_ret, cam_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
cam_trans = np.eye(4, 4)
cam_trans[0:3, 3] = np.asarray(cam_position)
cam_orientation = [
-cam_orientation[0], -cam_orientation[1], -cam_orientation[2]
]
cam_rotm = np.eye(4, 4)
cam_rotm[0:3, 0:3] = np.linalg.inv(utils.euler2rotm(cam_orientation))
self.cam_pose = np.dot(
cam_trans, cam_rotm
) # Compute rigid transformation representating camera pose
self.cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240],
[0, 0, 1]])
self.cam_depth_scale = 1
# Get background image
self.bg_color_img, self.bg_depth_img = self.get_camera_data()
self.bg_depth_img = self.bg_depth_img * self.cam_depth_scale
def simulation_experiment_challenging():
cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0,
1]])
object_inventory = [
'imported_part_0', 'imported_part_2', 'imported_part_3',
'imported_part_6', 'imported_part_7', 'imported_part_8'
]
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
root = 'C:/Users/louxi/Desktop/icra2021/zb_2/'
p = Perception() # perception module p
g = Manipulation()
positions = [[0.20, 0, 0.15], [-0.20, 0, 0.15], [0, -0.20, 0.15],
[0, 0.20, 0.15]]
env_cloud = np.load(root + 'env.npy')
if cid != -1:
i = 0
planning = 0
vrep.simxStartSimulation(cid, operationMode=vrep.simx_opmode_blocking)
panda = Robot(cid)
for i in range(10):
add_object(cid, 'imported_part_' + str(i),
random.choice(positions))
while i < 51:
print('experiment number: ', i)
rgb_raw, depth_raw = panda.get_rgbd_image()
# Perception
m_list = p.segmentation(rgb_raw, depth_raw) # segment scene
target_object = random.choice(object_inventory)
print(target_object)
anchor = np.asarray(
Image.open(
os.path.join(
'C:/Users\louxi\Desktop\icra2021\siamese_data',
target_object,
'rgb_image_' + str(random.randint(0, 499)) + '.png')))
anchor = center_crop_150(anchor)
object_mask = p.classification(m_list, rgb_raw,
anchor) # load anchor here
# object_depth[np.where(object_mask < 0.5)] = 0
rgb_raw[np.where(object_mask < 0.5)] = 0
object_depth = np.copy(depth_raw)
object_depth[np.where(object_mask < 0.5)] = 0
object_cloud = get_pointcloud(object_depth, cam_intrinsics,
panda.depth_m)
ws = [-0.25, 0.25, -0.25, 0.25, 0.001, 0.10]
segmented_cloud = np.array([
pts for pts in object_cloud
if pts[0] < ws[1] and pts[0] > ws[0] and pts[1] < ws[3]
and pts[1] > ws[2] and pts[2] < ws[5] and pts[2] > ws[4]
])
if len(segmented_cloud) == 0:
continue
# pptk.viewer(segmented_cloud)
#
# fig = plt.figure(0)
# sub_1 = fig.add_subplot(1, 2, 1)
# sub_1.imshow(anchor)
# sub_2 = fig.add_subplot(1, 2, 2)
# sub_2.imshow(rgb_raw)
# plt.show()
initial_poses, dummy_points = grasp_pose_generation(
90, segmented_cloud, 200)
# collision_free_poses = g.carp(env_cloud, np.average(segmented_cloud, axis=0), initial_poses)
landing_mtx = g.grasping(segmented_cloud,
np.average(segmented_cloud, axis=0),
initial_poses)
ending_mtx = np.copy(landing_mtx)
ending_mtx[-1] = ending_mtx[-1] + 0.15
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
cid, 'landing0', vrep.sim_scripttype_childscript, 'setlanding',
[], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
print(retFloats)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
cid, 'landing1', vrep.sim_scripttype_childscript, 'setlanding',
[], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
print(retFloats)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
cid, 'landing2', vrep.sim_scripttype_childscript, 'setlanding',
[], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
print(retFloats)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
cid, 'landing3', vrep.sim_scripttype_childscript, 'setlanding',
[], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
print(retFloats)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
cid, 'landing4', vrep.sim_scripttype_childscript, 'setlanding',
[], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
print(retFloats)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
cid, 'landing5', vrep.sim_scripttype_childscript, 'setlanding',
[], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
print(retFloats)
input('s')
vrep.simxCallScriptFunction(cid, 'landing',
vrep.sim_scripttype_childscript,
'setlanding', [], landing_mtx, [],
emptyBuff, vrep.simx_opmode_blocking)
vrep.simxCallScriptFunction(cid, 'ending',
vrep.sim_scripttype_childscript,
'setending', [], ending_mtx, [],
emptyBuff, vrep.simx_opmode_blocking)
time.sleep(0.5)
vrep.simxCallScriptFunction(cid, 'Sphere',
vrep.sim_scripttype_childscript,
'grasp', [], [], [], emptyBuff,
vrep.simx_opmode_blocking)
while True:
res, finish = vrep.simxGetIntegerSignal(
cid, "finish", vrep.simx_opmode_blocking)
if finish == 18:
res, result = vrep.simxGetIntegerSignal(
cid, "collision", vrep.simx_opmode_blocking)
if result == 1:
planning = planning + 1
print('result is %d. start next experiment' % result)
break
i = i + 1
# vrep.simxStopSimulation(cid, operationMode=vrep.simx_opmode_blocking)
time.sleep(3.0)
# input('s')
print(planning)
def simulation_experiment_standard():
cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0,
1]])
object_inventory = [
'imported_part_0', 'imported_part_5', 'imported_part_2',
'imported_part_3', 'imported_part_6', 'imported_part_9'
]
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
root = 'C:/Users/louxi/Desktop/icra2021/zb_2/'
p = Perception() # perception module p
g = Manipulation()
# positions = [[0.20, 0, 0.15], [-0.20, 0, 0.15], [0, -0.20, 0.15], [0, 0.20, 0.15]]
env_cloud = np.load(root + 'env.npy')
emptyBuff = bytearray()
if cid != -1:
i = 0
planning = 0
vrep.simxStartSimulation(cid, operationMode=vrep.simx_opmode_blocking)
panda = Robot(cid)
# for i in range(10):
# add_object(cid, 'imported_part_' + str(i), random.choice(positions))
# for i in range(10):
# add_object(cid, 'imported_part_' + str(i), [random.uniform(-0.10, 0.10), random.uniform(-0.10, 0.10), 0.15])
while True:
# res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing0', vrep.sim_scripttype_childscript, 'getlanding', [],
# [], [], emptyBuff, vrep.simx_opmode_blocking)
# print(retFloats)
# np.save('pose_0.npy', np.array(retFloats))
# time.sleep(1)
# res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing1', vrep.sim_scripttype_childscript, 'getlanding', [],
# [], [], emptyBuff, vrep.simx_opmode_blocking)
# print(retFloats)
# np.save('pose_1.npy', np.array(retFloats))
#
# time.sleep(1)
#
# res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing2', vrep.sim_scripttype_childscript, 'getlanding', [],
# [], [], emptyBuff, vrep.simx_opmode_blocking)
# print(retFloats)
# np.save('pose_2.npy', np.array(retFloats))
#
# time.sleep(1)
#
# res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing3', vrep.sim_scripttype_childscript, 'getlanding', [],
# [], [], emptyBuff, vrep.simx_opmode_blocking)
# print(retFloats)
# np.save('pose_3.npy', np.array(retFloats))
#
# time.sleep(1)
#
# res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing4', vrep.sim_scripttype_childscript, 'getlanding', [],
# [], [], emptyBuff, vrep.simx_opmode_blocking)
# print(retFloats)
# np.save('pose_4.npy', np.array(retFloats))
#
# time.sleep(1)
#
# res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing5', vrep.sim_scripttype_childscript, 'getlanding', [],
# [], [], emptyBuff, vrep.simx_opmode_blocking)
# print(retFloats)
# np.save('pose_5.npy', np.array(retFloats))
#
# time.sleep(1)
#
# input('s')
# objects_nb = input('number')
objects_nb = '0'
rgb_raw, depth_raw = panda.get_rgbd_image()
# im = Image.fromarray(rgb_raw)
# im.save('imported_part_5.png')
# input('save?')
# Perception
m_list = p.segmentation(rgb_raw, depth_raw) # segment scene
anchor = np.asarray(
Image.open(
'C:/Users\louxi\Desktop\icra2021\siamese_data\inventory/' +
object_inventory[int(objects_nb)] + '.png'))
anchor = center_crop_150(anchor)
object_mask = p.classification(m_list, rgb_raw,
anchor) # load anchor here
# object_depth[np.where(object_mask < 0.5)] = 0
rgb_raw[np.where(object_mask < 0.5)] = 0
# plt.imshow(rgb_raw)
# plt.show()
object_depth = np.copy(depth_raw)
object_depth[np.where(object_mask < 0.5)] = 0
object_cloud = get_pointcloud(object_depth, cam_intrinsics,
panda.depth_m)
ws = [-0.25, 0.25, -0.25, 0.25, 0.001, 0.10]
segmented_cloud = np.array([
pts for pts in object_cloud
if pts[0] < ws[1] and pts[0] > ws[0] and pts[1] < ws[3]
and pts[1] > ws[2] and pts[2] < ws[5] and pts[2] > ws[4]
])
# pptk.viewer(segmented_cloud)
recognized = 'yes'
if recognized == 'yes':
initial_poses, dummy_points = grasp_pose_generation(
60, segmented_cloud, 200)
# collision_free_poses = g.carp(env_cloud, np.average(segmented_cloud, axis=0), initial_poses)
# print(len(collision_free_poses))
pt = np.average(segmented_cloud, axis=0)
print('landing' + str(i))
pose = np.load('pose_' + str(i) + '.npy')
i = i + 1
# landing_mtx = [pose[0][0], pose[0][1], pose[0][2], pt[0],
# pose[1][0], pose[1][1], pose[1][2], pt[1],
# pose[2][0], pose[2][1], pose[2][2], pt[2]]
landing_mtx = pose
print(landing_mtx)
# vis_grasp(pose, pt, segmented_cloud)
ending_mtx = np.copy(landing_mtx)
ending_mtx[-1] = ending_mtx[-1] + 0.15
# recognized = input('correct?')
if recognized == 'yes':
vrep.simxCallScriptFunction(
cid, 'landing', vrep.sim_scripttype_childscript,
'setlanding', [], landing_mtx, [], emptyBuff,
vrep.simx_opmode_blocking)
vrep.simxCallScriptFunction(
cid, 'ending', vrep.sim_scripttype_childscript,
'setending', [], ending_mtx, [], emptyBuff,
vrep.simx_opmode_blocking)
time.sleep(0.5)
vrep.simxCallScriptFunction(
cid, 'Sphere', vrep.sim_scripttype_childscript,
'grasp', [], [], [], emptyBuff,
vrep.simx_opmode_blocking)
while True:
res, finish = vrep.simxGetIntegerSignal(
cid, "finish", vrep.simx_opmode_blocking)
if finish == 18:
res, result = vrep.simxGetIntegerSignal(
cid, "collision", vrep.simx_opmode_blocking)
if result == 1:
planning = planning + 1
print('result is %d. start next experiment' %
result)
break
time.sleep(3.0)
else:
continue
return cloud
# def get_object_pointcloud(self):
# result, state, data = vrep.simxReadVisionSensor(self.cid, self.object_depth_handle, vrep.simx_opmode_blocking)
# data = data[1]
# pcl = []
# for i in range(2, len(data), 4):
# p = [data[i], data[i + 1], data[i + 2], 1]
# pcl.append(np.matmul(self.depth_m, p))
# cloud = remove_clipping(pcl)
# return cloud
def wait(self):
running = True
while running:
res, signal = vrep.simxGetIntegerSignal(
self.cid, "finish", vrep.simx_opmode_oneshot_wait)
if signal == 18:
running = False
else:
running = True
if __name__ == '__main__':
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if cid != -1:
vrep.simxStartSimulation(cid, vrep.simx_opmode_blocking)
panda = Robot(cid)
cloud = panda.get_pointcloud()
v = pptk.viewer(cloud)
def gsp_test():
wd = '/home/lou00015/data/gsp_test/'
model = GSP3d().cuda()
model.load_state_dict(torch.load('gsp.pt'))
model.eval()
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
eid = 0
nb_grasp = 300
if cid != -1:
pos = [0, 0, 0.15]
while True:
vrep.simxStartSimulation(cid, vrep.simx_opmode_blocking)
panda = Robot(cid)
obj_name, obj_hdl = add_object(cid, 'imported_part_0', pos)
time.sleep(1.0)
cloud = panda.get_pointcloud()
centroid = np.average(cloud, axis=0)
if len(cloud) == 0:
print('no cloud found')
continue
elif centroid[2] > 0.045:
print('perception error')
continue
# np.save(wd + 'cloud_' + str(eid) + '.npy', cloud) # save point cloud
cloud = np.delete(cloud, np.where(cloud[:, 2] < 0.015), axis=0)
v = voxelize(cloud - centroid, 0.1)
pose_set, pt_set = grasp_pose_generation(45, cloud, nb_grasp)
x1, x2 = [], []
emptyBuff = bytearray()
for i in range(nb_grasp):
pose = pose_set[i]
pt = pt_set[i]
landing_mtx = np.asarray([
pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0],
pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1],
pose[2][2], pt[2]
])
x1.append(v)
x2.append(landing_mtx)
x1, x2 = np.stack(x1), np.stack(x2)
X1 = torch.tensor(x1.reshape((x2.shape[0], 1, 32, 32, 32)),
dtype=torch.float,
device=device)
X2 = torch.tensor(x2.reshape((x2.shape[0], 12)),
dtype=torch.float,
device=device)
yhat = model(X1, X2)
yhat = yhat.detach().cpu().numpy()
scores = np.asarray(yhat)
scores = scores.reshape((nb_grasp, ))
g_index = np.argmax(scores)
print('Highest score: {}, the {}th.'.format(
str(scores[g_index]), str(g_index)))
pose = pose_set[g_index]
pt = centroid
landing_mtx = np.asarray([
pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0],
pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1],
pose[2][2], pt[2]
])
vrep.simxCallScriptFunction(cid, 'landing',
vrep.sim_scripttype_childscript,
'setlanding', [], landing_mtx, [],
emptyBuff, vrep.simx_opmode_blocking)
ending_mtx = [
pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0],
pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1],
pose[2][2], pt[2] + 0.15
]
vrep.simxCallScriptFunction(cid, 'ending',
vrep.sim_scripttype_childscript,
'setending', [], ending_mtx, [],
emptyBuff, vrep.simx_opmode_blocking)
time.sleep(1.0)
print('executing experiment %d: ' % g_index)
print('at: ', pt)
vrep.simxCallScriptFunction(cid, 'Sphere',
vrep.sim_scripttype_childscript,
'grasp', [], [], [], emptyBuff,
vrep.simx_opmode_blocking)
while True:
res, finish = vrep.simxGetIntegerSignal(
cid, "finish", vrep.simx_opmode_oneshot_wait)
if finish == 18:
res, end_pos = vrep.simxGetObjectPosition(
cid, obj_hdl, -1, vrep.simx_opmode_blocking)
break
if end_pos[2] > 0.05:
label = 1
else:
label = 0
print(label)
# f = open(wd + 'label.txt', 'a+')
# f.write(str(label))
# f.close()
eid += 1
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()