def __init__(self, workspace_limits, ip_vrep): self.workspace_limits = workspace_limits s = rospy.Service('cmd_gripper', GripperCmd, self.cmdGripper) s = rospy.Service('robot_grasp', CoordAction, self.grasp) s = rospy.Service('robot_push', CoordAction, self.push) s = rospy.Service('restart_sim', Empty, self.restart_sim) # Make sure to have the server side running in V-REP: # in a child script of a V-REP scene, add following command # to be executed just once, at simulation start: # # simExtRemoteApiStart(19999) # # then start simulation, and run this program. # # IMPORTANT: for each successful call to simxStart, there # should be a corresponding call to simxFinish at the end! # MODIFY remoteApiConnections.txt # Connect to simulator self.sim_client = vrep.simxStart(ip_vrep, 20003, True, True, 5000, 5) # Connect to V-REP on port 20003 if self.sim_client == -1: print( 'Failed to connect to simulation (V-REP remote API server). Exiting.' ) exit() else: print('Connected to simulation on port {}'.format(self.sim_client)) # Start simulation sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_target', vrep.simx_opmode_blocking) sim_ret, self.cam_handle = vrep.simxGetObjectHandle( self.sim_client, 'Vision_sensor_persp', vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, (-0.5, 0, 0.3), vrep.simx_opmode_blocking) 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 restart_sim(self, req): 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) return EmptyResponse()
def gripper_fully_closed(self): """ Test if the gripper is closed :return: True if the gripper is fully closed (so, no object is holded) """ sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) while gripper_joint_position > -0.044: # Block until gripper is fully closed sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) if new_gripper_joint_position >= gripper_joint_position: return False gripper_joint_position = new_gripper_joint_position return True
def __init__(self): s = rospy.Service('get_color_depth_images', ColorDepthImages, self.get_data_for_service) s = rospy.Service('get_camera_informations', InfoCamera, self.get_informations_for_service) ipVREP = '127.0.0.1' self.sim_client = vrep.simxStart(ipVREP, 20002, True, True, 5000, 5) # Connect to V-REP on port 19997 if self.sim_client == -1: print( 'Failed to connect to simulation (V-REP remote API server). Exiting.' ) exit() else: print('Connected to simulation on port {}'.format(self.sim_client)) # 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(self.euler2rotm(cam_orientation)) self.cam_pose = np.dot( cam_trans, cam_rotm ) # Compute rigid transformation representation camera pose self.cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0, 1]]) self.cam_depth_scale = 1.0 # Get background image self.bg_color_img, self.bg_depth_img = self.get_camera_data() print(self.bg_color_img.shape) self.bg_depth_img = self.bg_depth_img * self.cam_depth_scale
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) while gripper_joint_position > -0.044: # Block until gripper is fully closed sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) if new_gripper_joint_position >= gripper_joint_position: return False gripper_joint_position = new_gripper_joint_position return True def open_gripper(self, async=False): gripper_motor_velocity = 0.5 gripper_motor_force = 20 sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, vrep.simx_opmode_blocking) # ICAM while gripper_joint_position < 0.0536: # Block until gripper is fully open # sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) 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)