def run_grasp(self, grasp_list, num, use_gdn=True):
if len(grasp_list) == 0:
print('No any grasp detected!')
grasp_iter = min(num, len(grasp_list))
for i in range(grasp_iter):
if use_gdn:
grasp_matrix = np.dot(grasp_list[i], self.original_matrix_gdn)
else:
grasp_matrix = np.dot(grasp_list[i], self.original_matrix)
#visualize the grasping pose
rot_matrix = grasp_matrix[:3, :3]
quat = mat2quat(rot_matrix)
quat = np.array([quat[1], quat[2], quat[3], quat[0]])
self.set_object_quat('staples_pose', quat)
self.set_object_position('staples_pose', grasp_matrix[:3, 3])
send_matrix = list(grasp_matrix.flatten())[0:12]
res, retInts, path, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'GraspMovement', [], send_matrix, [], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
running = True
while running:
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI',
vrep.sim_scripttype_childscript, 'isRunning', [], [],
['UR5_GraspMatrix'], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
if retInts[0] == 0:
running = False
def insertion(self, pose_matrix):
send_matrix = list(pose_matrix.flatten())[0:12]
res, retInts, path, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'Insertion', [], send_matrix, [], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
running = True
while running:
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'isRunning', [], [], ['UR5_InsertionMatrix'], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
if retInts[0] == 0:
running = False
def open_gripper(self, mode):
mode = int(mode)
send_matrix = [mode]
res, retInts, path, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'Open', [], send_matrix, [], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
running = True
while running:
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'isRunning', [], [], ['UR5_InsertionMatrix'], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
if retInts[0] == 0:
running = False
def get_object_matrix(self, obj_name):
sim_ret, object_handle = vrep.simxGetObjectHandle(
self.clientID, obj_name, vrep.simx_opmode_oneshot_wait)
res, retInts, object_matrix, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'getObjectPose', [object_handle], [], [], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
object_matrix = np.array(object_matrix + [0, 0, 0, 1],
dtype=np.float64).reshape(4, 4)
return object_matrix
def get_camera_pos(self, cam_name="Vision_sensor"):
sim_ret, cam_handle = vrep.simxGetObjectHandle(
self.clientID, cam_name, vrep.simx_opmode_oneshot_wait)
sim_ret, cam_pos = vrep.simxGetObjectPosition(
self.clientID, cam_handle, -1, vrep.simx_opmode_oneshot_wait)
sim_ret, cam_quat = vrep.simxGetObjectQuaternion(
self.clientID, cam_handle, -1, vrep.simx_opmode_oneshot_wait)
res, retInts, cam_matrix, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'getObjectPose', [cam_handle], [], [], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
return cam_pos, cam_quat, cam_matrix
def get_camera_matrix(self, cam_name='Vision_sensor'):
#res,retInts,self.robot_matrix,retStrings,retBuffer=vrep.simxCallScriptFunction(self.clientID,'RemoteAPI',vrep.sim_scripttype_childscript,'getObjectPose',[robot_handle],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
sim_ret, cam_handle = vrep.simxGetObjectHandle(
self.clientID, cam_name, vrep.simx_opmode_oneshot_wait)
res, retInts, self.cam_matrix, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'getObjectPose', [cam_handle], [], [], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
sim_ret, self.cam_pos = vrep.simxGetObjectPosition(
self.clientID, self.cam_handle, -1, vrep.simx_opmode_oneshot_wait)
sim_ret, self.cam_quat = vrep.simxGetObjectQuaternion(
self.clientID, self.cam_handle, -1, vrep.simx_opmode_oneshot_wait)
self.cam_matrix = np.array(self.cam_matrix,
dtype=np.float64).reshape(3, 4)
#print('='*20+'\ncam matrix')
#print(cam_matrix)
return self.cam_matrix