def move_to(self, tool_position, tool_orientation): sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.02 * move_direction / move_magnitude try: num_move_steps = int(np.floor(move_magnitude / 0.02)) except ValueError: return False for step_iter in range(num_move_steps): vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0], UR5_target_position[1] + move_step[1], UR5_target_position[2] + move_step[2]), vrep.simx_opmode_blocking) sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking)
def get_obj_positions(self): obj_positions = [] for object_handle in self.object_handles: sim_ret, object_position = vrep.simxGetObjectPosition( self.sim_client, object_handle, -1, vrep.simx_opmode_blocking) obj_positions.append(object_position) return obj_positions
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 check_sim(self): # Check if simulation is stable by checking if gripper is within workspace sim_ret, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking) sim_ok = self.workspace_limits[0][0] - 0.1 < gripper_position[0] < self.workspace_limits[0][1] + 0.1 and \ self.workspace_limits[1][0] - 0.1 < gripper_position[1] < self.workspace_limits[1][1] + 0.1 and \ self.workspace_limits[2][0] < gripper_position[2] < self.workspace_limits[2][1] if not sim_ok: logging.info('Simulation unstable. Restarting environment.') self.restart_sim() self.add_objects()
def restart_sim(self): sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_target', vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, (-0.5, 0, 0.3), vrep.simx_opmode_blocking) vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(1) sim_ret, self.RG2_tip_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking) sim_ret, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking) while gripper_position[ 2] > 0.4: # V-REP bug requiring multiple starts and stops to restart vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(1) sim_ret, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking)
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 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()
def push(self, position, heightmap_rotation_angle, push_vertical_offset=0.01, pushing_point_margin=0.1): logging.info('Executing: push at (%f, %f, %f)' % (position[0], position[1], position[2])) # Compute tool orientation from heightmap rotation angle tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 # Adjust pushing point to be on tip of finger position[2] = position[2] + push_vertical_offset # Compute pushing direction push_orientation = [1.0, 0.0] push_direction = np.asarray([ push_orientation[0] * np.cos(heightmap_rotation_angle) - push_orientation[1] * np.sin(heightmap_rotation_angle), push_orientation[0] * np.sin(heightmap_rotation_angle) + push_orientation[1] * np.cos(heightmap_rotation_angle) ]) # Move gripper to location above pushing point location_above_pushing_point = (position[0], position[1], position[2] + pushing_point_margin) # Compute gripper position and linear movement increments tool_position = location_above_pushing_point sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.05 * move_direction / move_magnitude try: num_move_steps = int(np.floor(move_direction[0] / move_step[0])) except ValueError: return False # Compute gripper orientation and rotation increments sim_ret, gripper_orientation = vrep.simxGetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) rotation_step = 0.3 if ( tool_rotation_angle - gripper_orientation[1] > 0) else -0.3 num_rotation_steps = int( np.floor((tool_rotation_angle - gripper_orientation[1]) / rotation_step)) # Simultaneously move and rotate gripper for step_iter in range(max(num_move_steps, num_rotation_steps)): vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0] * min(step_iter, num_move_steps), UR5_target_position[1] + move_step[1] * min(step_iter, num_move_steps), UR5_target_position[2] + move_step[2] * min(step_iter, num_move_steps)), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, gripper_orientation[1] + rotation_step * min(step_iter, num_rotation_steps), np.pi / 2), vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) # Ensure gripper is closed self.close_gripper() # Approach pushing point self.move_to(position, None) # Compute target location (push to the right) push_length = 0.1 target_x = min( max(position[0] + push_direction[0] * push_length, self.workspace_limits[0][0]), self.workspace_limits[0][1]) target_y = min( max(position[1] + push_direction[1] * push_length, self.workspace_limits[1][0]), self.workspace_limits[1][1]) push_length = np.sqrt( np.power(target_x - position[0], 2) + np.power(target_y - position[1], 2)) # Move in pushing direction towards target location self.move_to([target_x, target_y, position[2]], None) # Move gripper to location above grasp target self.move_to([target_x, target_y, location_above_pushing_point[2]], None) push_success = True return push_success
def grasp( self, position, heightmap_rotation_angle, grasp_vertical_offset=-0.04, grasp_location_margin=0.15, ): logging.info('Executing: grasp at (%f, %f, %f)' % (position[0], position[1], position[2])) # Compute tool orientation from heightmap rotation angle tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 # Avoid collision with floor position = np.asarray(position).copy() position[2] = max(position[2] + grasp_vertical_offset, self.workspace_limits[2][0] + 0.02) # Move gripper to location above grasp target location_above_grasp_target = (position[0], position[1], position[2] + grasp_location_margin) # Compute gripper position and linear movement increments tool_position = location_above_grasp_target sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.05 * move_direction / move_magnitude try: num_move_steps = int(np.floor(move_direction[0] / move_step[0])) except ValueError: return False # Compute gripper orientation and rotation increments sim_ret, gripper_orientation = vrep.simxGetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) rotation_step = 0.3 if ( tool_rotation_angle - gripper_orientation[1] > 0) else -0.3 num_rotation_steps = int( np.floor((tool_rotation_angle - gripper_orientation[1]) / rotation_step)) # Simultaneously move and rotate gripper for step_iter in range(max(num_move_steps, num_rotation_steps)): vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0] * min(step_iter, num_move_steps), UR5_target_position[1] + move_step[1] * min(step_iter, num_move_steps), UR5_target_position[2] + move_step[2] * min(step_iter, num_move_steps)), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, gripper_orientation[1] + rotation_step * min(step_iter, num_rotation_steps), np.pi / 2), vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) # Ensure gripper is open self.open_gripper() # Approach grasp target self.move_to(position, None) # Close gripper to grasp target gripper_full_closed = self.close_gripper() # Move gripper to location above grasp target self.move_to(location_above_grasp_target, None) # Check if grasp is successful gripper_full_closed = self.close_gripper() grasp_success = not gripper_full_closed # Move the grasped object elsewhere if grasp_success: if self.place_enabled: self.go_home() else: self.num_obj_clear += 1 object_positions = np.asarray(self.get_obj_positions()) object_positions = object_positions[:, 2] grasped_object_ind = np.argmax(object_positions) grasped_object_handle = self.object_handles[grasped_object_ind] vrep.simxSetObjectPosition( self.sim_client, grasped_object_handle, -1, (-0.5, 0.5 + 0.05 * float(grasped_object_ind), 0.1), vrep.simx_opmode_blocking) return grasp_success