def multiple_objects_evaluation_no_pushing():
grasping_model = load_model('trained_models/grasping.h5')
# for layer in grasping_model.layers:
# layer.name = layer.name + '_grasping'
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
while True:
# Initialize environment
panda = Panda(clientID)
obj_pos, obj_ori, handles = panda.init_env()
handles_left = len(handles)
while handles_left > 0:
pointcloud = panda.get_cloud()
grasp_poses, points = grasp_pose_generation('data.pcd')
grasp_sc = grasp_scores(grasping_model, grasp_poses, points,
pointcloud)
print('Grasping scores:')
for scores in grasp_sc:
print(scores)
# ----------------------------------------------------------------------------------------------------------
if len(grasp_sc) != 0:
max_index = np.argmax(grasp_sc)
print('Highest grasping score index: ', max_index)
panda.grasp(grasp_poses[max_index], points[max_index])
# ----------------------------------------------------------------------------------------------------------
print('Checking results')
for j in range(3):
res, current_pos = vrep.simxGetObjectPosition(
clientID, handles[j], -1,
vrep.simx_opmode_oneshot_wait)
print(current_pos)
if current_pos[2] > obj_pos[j][2] + 0.03:
vrep.simxSetObjectPosition(
clientID, handles[j], -1, [2, 2, 0.5],
vrep.simx_opmode_oneshot_wait)
obj_pos[j] = [2, 2, 0.5]
handles_left = handles_left - 1
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(3)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
for k in range(3):
vrep.simxSetObjectPosition(clientID, handles[k], -1,
obj_pos[k],
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, handles[k], -1,
obj_ori[k],
vrep.simx_opmode_blocking)
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def more_data():
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
test = 0
if clientID != -1:
while True:
# Initialize environment
panda = Panda(clientID)
obj_pos, obj_ori, handles = panda.init_env()
pointcloud = panda.get_cloud()
grasp_poses, points = grasp_pose_generation('data.pcd')
# ----------------------------------------------------------------------------------------------------------
for g_index in range(len(grasp_poses)):
print(grasp_poses[g_index], points[g_index])
panda.grasp(grasp_poses[g_index], points[g_index])
res, current_pos = vrep.simxGetObjectPosition(
clientID, handles[0], -1, vrep.simx_opmode_oneshot_wait)
print(current_pos)
if current_pos[2] > obj_pos[0][2] + 0.03:
result = 1
else:
result = 0
r_cloud = rotate_cloud(points[g_index], grasp_poses[g_index],
pointcloud)
np.savetxt('forthehorde.pcd',
r_cloud,
fmt='%1.9f',
delimiter=' ')
insertHeader('forthehorde.pcd')
copyfile(
'/home/lou00015/cnn3d/scripts/forthehorde.pcd',
'/home/lou00015/dataset/data_UR5/test' + str(test) +
'.pcd')
f = open('/home/lou00015/dataset/data_UR5/label.txt', "a+")
f.write(str(result))
f.close()
test = test + 1
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(3)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handles[0], -1,
obj_pos[0],
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, handles[0], -1,
obj_ori[0],
vrep.simx_opmode_blocking)
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def debugging():
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
grasps = ['hand0', 'hand1', 'hand2', 'hand3', 'hand4', 'hand5', 'hand6']
if clientID != -1:
# Initialize environment
panda = Panda(clientID)
panda.init_env()
# while 1:
pointcloud = panda.get_cloud()
grasp_poses, points = grasp_pose_generation('data.pcd')
for i in range(len(grasp_poses)):
grasp_pose = grasp_poses[i]
surface = points[i]
matrix = [
grasp_pose[0][0], grasp_pose[0][1], grasp_pose[0][2],
surface[0], grasp_pose[1][0], grasp_pose[1][1],
grasp_pose[1][2], surface[1], grasp_pose[2][0],
grasp_pose[2][1], grasp_pose[2][2], surface[2]
]
emptyBuff = bytearray()
panda_id = clientID
grasp = grasps[i]
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
panda_id, grasp, vrep.sim_scripttype_childscript, 'setmatrix',
[], matrix, [], emptyBuff, vrep.simx_opmode_blocking)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
panda_id, grasp, vrep.sim_scripttype_childscript,
'absposition', [], [], [], emptyBuff,
vrep.simx_opmode_blocking)
R = np.asarray(
[[retFloats[0], retFloats[1], retFloats[2], retFloats[3]],
[retFloats[4], retFloats[5], retFloats[6], retFloats[7]],
[retFloats[8], retFloats[9], retFloats[10], retFloats[11]]])
print(R)
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def multiple_objects_evaluation():
grasping_model = load_model('trained_models/grasping.h5')
pushing_model = load_model('trained_models/pushing.h5')
for layer in grasping_model.layers:
layer.name = layer.name + '_grasping'
for layer in pushing_model.layers:
layer.name = layer.name + '_pushing'
experiment_number = 0
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
# Initialize environment
panda = Panda(clientID)
num_obj = 3
obj_pos, handles = panda.init_env()
while num_obj > 0:
pointcloud = panda.get_cloud()
# ----------------------------------------------------------------------------------------------------------
# Push or grasp?
nb_clutters = segmentation('data.pcd')
print('Found %d objects' % nb_clutters)
push_poses = push_pose_generation(pointcloud, 30)
push_sc = push_scores(pushing_model, push_poses, pointcloud)
print('Pushing scores:')
for scores in push_sc:
print(scores)
push_dir = push_poses[push_sc.index(max(push_sc))]
pc = pypcd.PointCloud.from_path('cloud_cluster_0.pcd')
data = [pc.pc_data['x'], pc.pc_data['y'], pc.pc_data['z']]
data = np.transpose(data)
# v = pptk.viewer(data)
grasp_poses, points = grasp_pose_generation()
grasp_sc = grasp_scores(grasping_model, grasp_poses, points, data)
print('Grasping scores:')
for scores in grasp_sc:
print(scores)
best_grasp = max(grasp_sc)
grasp_i = grasp_sc.index(best_grasp)
print('Highest grasping score index: ', grasp_i)
# ----------------------------------------------------------------------------------------------------------
# Evaluate success rate of each action
if len(points) != 0:
if max(grasp_sc) > max(push_sc):
panda.grasp(grasp_poses[grasp_i], points[grasp_i])
else:
panda.push(push_dir)
else:
continue
# ----------------------------------------------------------------------------------------------------------
# print('Re-analyzing geometrics ...')
# # # Recording data
# label = []
for j in range(num_obj):
res, current_pos = vrep.simxGetObjectPosition(
clientID, handles[j], -1, vrep.simx_opmode_oneshot_wait)
print(current_pos)
if current_pos[2] > obj_pos[j][2] + 0.03:
# res = 1
vrep.simxSetObjectPosition(clientID, handles[j], -1,
[2, 2, 0.5],
vrep.simx_opmode_oneshot_wait)
num_obj = num_obj - 1
print('Grasp successful, %d objects left' % num_obj)
print('test completed, starting next iteration ...')
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()