def __init__(self, clientID, suffix=""): # vrep.simxFinish(-1) # just in case, close all opened connections # self._clientID = vrep.simxStart('127.0.0.1',19997, True, True, 5000, 5) # Connect to V-REP self._clientID = clientID self.suffix = suffix _, self._left_joint = vrep.simxGetObjectHandle(self._clientID, 'ePuck_leftJoint' + suffix, vrep.simx_opmode_oneshot_wait) _, self._right_joint = vrep.simxGetObjectHandle(self._clientID, 'ePuck_rightJoint' + suffix, vrep.simx_opmode_oneshot_wait) _, self._light_sensor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_lightSensor' + suffix, vrep.simx_opmode_oneshot_wait) _, self._camera = vrep.simxGetObjectHandle(self._clientID, 'ePuck_camera' + suffix, vrep.simx_opmode_oneshot_wait) self._prox_handles = [] for i in range(1,9): _, p = vrep.simxGetObjectHandle(self._clientID, 'ePuck_proxSensor' + str(i) + suffix, vrep.simx_opmode_oneshot_wait) self._prox_handles.append(p) # First calls with simx_opmode_streaming for i in range(8): vrep.simxReadProximitySensor(self._clientID, self._prox_handles[i], vrep.simx_opmode_streaming) _, self.camera_resolution, _ = vrep.simxGetVisionSensorImage(self._clientID, self._camera, options=0, operationMode=vrep.simx_opmode_streaming) _, self.light_sensor_resolution, _ = vrep.simxGetVisionSensorImage(self._clientID, self._light_sensor, options=0, operationMode=vrep.simx_opmode_streaming) self._body = vrep.simxGetObjectHandle(self._clientID, "ePuck_bodyElements" + suffix, vrep.simx_opmode_oneshot_wait) self.wheel_diameter = 4.25 * 10 ** -2 self.base_lenght = 7 * 10 ** -2 self._prox_aliases = {"all" : range(8), "all-but-rear" : range(6), "front" : [2, 3], "rear" : [6, 7], "front-left" : [0, 1, 2], "front-right": [3, 4, 5]} self.no_detection_value = 2000. self._fwd_spd, self._rot_spd = 0., 0. self._left_spd, self._right_spd = 0., 0. self.fwd_spd, self.rot_spd = 0., 0. vrep.simxGetFloatSignal(self._clientID, "CurrentTime", vrep.simx_opmode_streaming) self.freq = 100 self._behaviors = {} self._runnings = {} self._to_detach = {} self._sensations = {} self._conditions = {} self._registered_objects = {} _, _, _ , _, _ = vrep.simxGetObjectGroupData(self._clientID, vrep.sim_object_shape_type, 0, vrep.simx_opmode_streaming) sleep(0.5) self.register_all_scene_objects()
def get_image(self, cam_handle): (code, res, im) = vrep.simxGetVisionSensorImage(self.ID, cam_handle, 0, const_v.simx_opmode_buffer) # print(code) img = np.array(im, dtype=np.uint8) img.resize([res[0], res[1], 3]) img = cv2.flip(img, 0) return img
def get_image_vrep_buffering(self): ret, res, image = vrep.simxGetVisionSensorImage( self.vrep_connection.client_id, self.vision_sensor_handle, 0, vrep.simx_opmode_buffer) if ret == vrep.simx_return_ok: return image, res else: raise Exception( "Failed to get vision sensor image. Vision sensor name: " + self.vision_sensor_name)
def raw_image(self, is_grey_scale=False): """ Retrieves the image of a vision sensor. @return the image data """ num_of_clr = 3 if is_grey_scale: num_of_clr = 1 code, resolution, image = v.simxGetVisionSensorImage( self._id, self._handle, int(is_grey_scale), self._def_op_mode) return resolution, image
def activate_vrep_image_streaming(self): ret, res, image = vrep.simxGetVisionSensorImage( self.vrep_connection.client_id, self.vision_sensor_handle, 0, vrep.simx_opmode_streaming) if ret == vrep.simx_return_ok: print('Started streaming image') return image, res elif ret == vrep.simx_return_remote_error_flag or ret == vrep.simx_return_local_error_flag: raise Exception( "Failed to stream vision sensor image. Vision sensor name: " + self.vision_sensor_name) else: print('Possible started streaming image') return [], [0, 0]
def get_vision_sensor_image(self, handle, first_call=False): opmode = vrep.simx_opmode_streaming if first_call else vrep.simx_opmode_buffer status, resolution, image = vrep.simxGetVisionSensorImage( self.id, handle, 0, opmode) self.check_return_code(status, tolerance=vrep.simx_return_novalue_flag) return resolution, image
def __init__(self): fourcc = cv2.VideoWriter_fourcc(*'mp4v') self.out = cv2.VideoWriter('output.mp4', fourcc, 15.0, (1024, 1024)) self.DoF = 6 # self.action_bound = [[-15,15],[-10,110],[-30,30],[-120,120],[-180,180],[-180,180]] self.action_bound = [[-180, 180], [-180, 180], [-180, 180], [-180, 180], [-180, 180], [-180, 180]] self.action_range = [-5, 5] self.action_dim = self.DoF self.action_space = gym.spaces.Box(shape=(self.DoF, ), low=-5, high=5) self.observation_space = gym.spaces.Box(shape=(3 + 6 + self.DoF * 2, ), low=-180, high=180) self.action_dim = self.action_space.shape[0] self.state_dim = self.observation_space.shape[0] p = subprocess.Popen(['ps', '-A'], stdout=subprocess.PIPE) out, err = p.communicate() for line in out.splitlines(): if b'vrep' in line: pid = int(line.split(None, -1)[0]) os.kill(pid, signal.SIGKILL) self.vrep_root = "/home/ali/Downloads/VREP" self.scene_file = "/home/ali/RL_code/env/rozum_model.ttt" # os.chdir(self.vrep_root) os.system("./vrep.sh -s " + self.scene_file + " &") vrep.simxFinish(-1) time.sleep(1) # get the ID of the running simulation self.ID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000, 5) # check the connection if self.ID != -1: print("Connected") else: sys.exit("Error") # get handles # for camera self.cam_handle = self.get_handle('Vision_sensor') (code, res, im) = vrep.simxGetVisionSensorImage(self.ID, self.cam_handle, 0, const_v.simx_opmode_streaming) self.render_handle = self.get_handle('render') (code, res, im) = vrep.simxGetVisionSensorImage(self.ID, self.render_handle, 0, const_v.simx_opmode_streaming) # joints self.joint_handles = [] for i in range(self.DoF): tmp = self.get_handle("joint%d" % (i)) self.joint_handles.append(tmp) code, angle = vrep.simxGetJointPosition( self.ID, tmp, const_v.simx_opmode_streaming) # gripper tip self.tip_handle = self.get_handle("Tip") (code, pose) = vrep.simxGetObjectPosition(self.ID, self.tip_handle, -1, const_v.simx_opmode_streaming) self.or_handle = self.get_handle("RG2_baseVisible") (code, pose) = vrep.simxGetObjectOrientation(self.ID, self.or_handle, -1, const_v.simx_opmode_streaming) # cube self.cube_handle = self.get_handle("Cube") (code, pose) = vrep.simxGetObjectPosition(self.ID, self.cube_handle, -1, const_v.simx_opmode_streaming) (code, pose) = vrep.simxGetObjectOrientation(self.ID, self.cube_handle, -1, const_v.simx_opmode_streaming) # get the goal handle self.goal_handle = self.get_handle("Goal") (code, pose) = vrep.simxGetObjectPosition(self.ID, self.goal_handle, -1, const_v.simx_opmode_streaming) # angles' array self.angles = self.get_angles() # gripper handles (used in closing and opening gripper) self.gripper_motor = self.get_handle('RG2_openCloseJoint') # task part self.task_part = 0 self.init_angles = self.get_angles() self.init_orientation = self.get_orientation(self.or_handle) self.init_pose_cube = self.get_position(self.cube_handle) # print(self.init_pose_cube) self.init_goal_pose = self.get_position(self.goal_handle) # print(self.init_goal_pose) self.open_gripper() self.reset() self.tip_position = self.get_position(self.tip_handle) self.goal_l = (80, 0, 0) self.goal_u = (120, 255, 255) self.cube_l = (55, 50, 50) self.cube_u = (80, 255, 255) self.er_kernel = np.ones((2, 2), np.uint8) self.di_kernel = np.ones((2, 22), np.uint8) self.task_part = 0 self.part_1_center = np.array([300.0, 335.0]) self.part_2_center = np.array([320.0, 290.0]) self.part_1_area = 0.25 self.part_2_area = 0.75
def step(self, action): # Activate the motors vrep.simxSetJointTargetVelocity(self.client_id, self.left_motor_handle, action[0], vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.client_id, self.right_motor_handle, action[1], vrep.simx_opmode_blocking) # Get observations observations = {} observations['proxy_sensor'] = [] observations['light_sensor'] = [] # Fetch the vals of proxy sensors for sensor in self.proxy_sensors: _, _, detectedPoint, _, _ = vrep.simxReadProximitySensor( self.client_id, sensor, vrep.simx_opmode_blocking) # Append to list of values observations['proxy_sensor'].append(np.linalg.norm(detectedPoint)) # Fetch the vals of light sensors for sensor in self.light_sensors: # Fetch the initial value in the suggested mode _, _, image = vrep.simxGetVisionSensorImage( self.client_id, sensor, 1, vrep.simx_opmode_blocking) # extract image from list image = image[0] if len(image) else -1 # Append to the list of values observations['light_sensor'].append(image) # vrep gives a positive value for the black strip and negative for the # floor so convert it into 0 and 1 observations['light_sensor'] = np.asarray(observations['light_sensor']) observations['light_sensor'] = np.sign(observations['light_sensor']) # Assign reward reward = {} # For light sensors # If any of the center 2 sensors is 1 give high reward if (observations['light_sensor'][[3, 4]] > 0).any(): reward['light_sensor'] = 5 # If any of second, third, sixth or seventh is 1 elif (observations['light_sensor'][[1, 2, 5, 6]] > 0).any(): reward['light_sensor'] = 2 # If first or last are high elif (observations['light_sensor'][[0, 7]] > 0).any(): reward['light_sensor'] = 0 # Bot is completly out of line else: reward['light_sensor'] = -5 # For proximity sensors reward['proxy_sensor'] = 0 # Should be rewarded for movement r = np.sum(np.sign(action)) * 2 reward['light_sensor'] += r reward['proxy_sensor'] += r # reward['combined'] += r return observations, reward
def floor_sensor(self): tresh = 0. _, _, image = vrep.simxGetVisionSensorImage(self._clientID, self._light_sensor, options=0, operationMode=vrep.simx_opmode_buffer) return image[0] > tresh, image[21] > tresh, image[93] > tresh
def light_sensor_image(self): _, resolution, image = vrep.simxGetVisionSensorImage(self._clientID, self._light_sensor, options=0, operationMode=vrep.simx_opmode_buffer) image = array(image) image.resize(resolution[0], resolution[1], 3) return image
clientID, sensor1, vrep.simx_opmode_buffer) #returnCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor1,vrep.simx_opmode_streaming) print returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector # vrep.simxSetJointTargetVelocity(clientID, left_Motor, 0, vrep.simx_opmode_streaming) ##time.sleep(5) vrep.simxSetJointTargetVelocity(clientID, right_Motor, 0, vrep.simx_opmode_streaming) ##vrep.simxGetCollectionHandle() (clientID,'Pioneer_p3dx',vrep.simxServiceCall()) a = vrep.simxGetObjects(clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait) #print a #vrep.simxGetVisionSensorImage(clientID,cam1,,simx_opmode_streaming) returnCode, resolution, image = vrep.simxGetVisionSensorImage( clientID, cam1, 0, vrep.simx_opmode_streaming) time.sleep(1) returnCode, resolution, image = vrep.simxGetVisionSensorImage( clientID, cam1, 0, vrep.simx_opmode_buffer) import numpy as np from matplotlib import pyplot as plt im = np.array(image, dtype=np.uint8) im.resize([resolution[0], resolution[1], 3]) print im.shape plt.imshow(im, origin='lower') #for i in range(0,len(a)): # returnCode, childObjectHandle=vrep.simxGetObjectChild(clientID,carro,i,vrep.simx_opmode_blocking) # print childObjectHandle
def __init__(self): self.DoF = 6 self.action_bound = [-5, 5] self.action_dim = self.DoF # os.chdir("/Vrep_server") # os.system("git pull") # os.chdir("/") # # self.vrep_root = "/V-REP_PRO_EDU_V3_5_0_Linux/" # self.scene_file = "/Vrep_server/env/rozum_model.ttt" # # os.chdir(self.vrep_root) # os.system("xvfb-run --auto-servernum --server-num=1 -s \"-screen 0 640x480x24\" ./vrep.sh -h -s " + self.scene_file + " &") vrep.simxFinish(-1) time.sleep(1) # get the ID of the running simulation self.ID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000, 5) # check the connection if self.ID != -1: print("Connected") else: sys.exit("Error") # get handles # for camera self.cam_handle = self.get_handle('Vision_sensor') (code, res, im) = vrep.simxGetVisionSensorImage(self.ID, self.cam_handle, 0, const_v.simx_opmode_streaming) # joints self.joint_handles = [] for i in range(self.DoF): tmp = self.get_handle("joint%d" % (i)) self.joint_handles.append(tmp) code, angle = vrep.simxGetJointPosition( self.ID, tmp, const_v.simx_opmode_streaming) # gripper tip self.tip_handle = self.get_handle("Tip") (code, pose) = vrep.simxGetObjectPosition(self.ID, self.tip_handle, -1, const_v.simx_opmode_streaming) # cube self.cube_handle = self.get_handle("Cube") (code, pose) = vrep.simxGetObjectPosition(self.ID, self.cube_handle, -1, const_v.simx_opmode_streaming) # get the goal handle self.goal_handle = self.get_handle("Goal") (code, pose) = vrep.simxGetObjectPosition(self.ID, self.goal_handle, -1, const_v.simx_opmode_streaming) # angles' array self.angles = self.get_angles() # gripper handles (used in closing and opening gripper) self.gripper_motor = self.get_handle('RG2_openCloseJoint') # task part self.task_part = 0 self.init_angles = self.get_angles() self.init_pose_cube = self.get_position(self.cube_handle) # print(self.init_pose_cube) self.init_goal_pose = self.get_position(self.goal_handle) # print(self.init_goal_pose) self.open_gripper() self.reset() self.tip_position = self.get_position(self.tip_handle) self.goal_l = (80, 0, 0) self.goal_u = (120, 255, 255) self.cube_l = (55, 50, 50) self.cube_u = (80, 255, 255) self.er_kernel = np.ones((2, 2), np.uint8) self.di_kernel = np.ones((2, 22), np.uint8) self.task_part = 0 self.part_1_center = np.array([300.0, 335.0]) self.part_2_center = np.array([320.0, 290.0]) self.part_1_area = 0.25 self.part_2_area = 0.75
def camera_image(self): _, resolution, image = vrep.simxGetVisionSensorImage(epuck._clientID, self._camera, options=0, operationMode=vrep.simx_opmode_buffer) image.resize(resolution[0], resolution[1], 3) return image