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 getProximitySensors(self): self.sensorValues = [-1] * 8 # empty table of size 8 for i in range(8): _, a, b, _, _ = vrep.simxReadProximitySensor( self.clientID, self.proximitySensorsHandles[i], vrep.simx_opmode_streaming) #vrep.simx_opmode_buffer) if a: self.sensorValues[i] = b return self.sensorValues
def get_info_sensor(self): sensor_dist = [] for sensor in self.sensor_list: err_code, detectionState, detectedPoint, detectedObjectHandle,\ detectedSurfaceNormalVector = vrep.simxReadProximitySensor(self.clientId,self.sensor_list[sensor], vrep.simx_opmode_streaming ) err_code, detectionState, detectedPoint, detectedObjectHandle, \ detectedSurfaceNormalVector = vrep.simxReadProximitySensor(self.clientId, self.sensor_list[sensor], vrep.simx_opmode_buffer) if err_code != -1 and (detectionState): data = {} data['name'] = sensor data['distance'] = self.norm(detectedPoint) * 100 sensor_dist.append(data) return sensor_dist
def read(self) -> (bool, Vec3): """ Reads the state of a proximity sensor. @return detection state and detected point @rtype (bool, Vec3) """ code, state, point, handle, snv = v.simxReadProximitySensor( self._id, self._handle, self._def_op_mode) return state, Vec3(point[0], point[1], point[2])
def get_distance(self): """Queries the current distance from the sonar. We use a proximity sensor in V-REP to model the sonar. Returns: Distance in m. If there was an error, it returns 3.3 m. """ return_code, detection_state, detected_point, detected_object_handle, detected_surface_normal_vector = \ vrep.simxReadProximitySensor(self._clientID, self._sensor, vrep.simx_opmode_oneshot_wait) if detection_state: return math.sqrt( math.pow(detected_point[0], 2) + math.pow(detected_point[1], 2) + math.pow(detected_point[2], 2)) else: return 3.3
def read_prox_sensor(self, handle, first_call=False): opmode = vrep.simx_opmode_streaming if first_call else vrep.simx_opmode_buffer status, state, coord, _, _ = vrep.simxReadProximitySensor( self.id, handle, opmode) self.check_return_code(status, tolerance=vrep.simx_return_novalue_flag) return state, coord
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 proximeters(self, group="all", mode="no_object_id"): distances = [] objects = [] vrep.simxPauseCommunication(self._clientID, True) for i in range(8): res, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(self._clientID, self._prox_handles[i], vrep.simx_opmode_buffer) if detectionState: distances.append(1000 * sqrt(sum([x ** 2 for x in detectedPoint]))) if (detectedObjectHandle - 1) in self._registered_objects: objects.append(self._registered_objects[detectedObjectHandle - 1]) else: objects.append("None") else: distances.append(self.no_detection_value) objects.append("None") vrep.simxPauseCommunication(self._clientID, False) if mode == "no_object_id": return array(distances)[self._prox_aliases[group]] else: return array(distances)[self._prox_aliases[group]], array(objects)[self._prox_aliases[group]]
#vrep.simx_opmode_oneshot #simxGetObjectHandle errorCode, carro = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) errorCode, left_Motor = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) errorCode, right_Motor = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) errorCode, sensor1 = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx_ultrasonicSensor1', vrep.simx_opmode_oneshot_wait) errorCode, cam1 = vrep.simxGetObjectHandle(clientID, 'cam2', vrep.simx_opmode_oneshot_wait) # #vrep.simxReadProximitySensor returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor( clientID, sensor1, vrep.simx_opmode_streaming) time.sleep(1) returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor( 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)
def proximeters(self, group="all"): distances = [] vrep.simxPauseCommunication(self._clientID, True) for i in range(8): res, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(self._clientID, self._prox_handles[i], vrep.simx_opmode_buffer) if detectionState: distances.append(1000 * sqrt(sum([x ** 2 for x in detectedPoint]))) else: distances.append(self.no_detection_value) vrep.simxPauseCommunication(self._clientID, False) return array(distances)[self._prox_aliases[group]]