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 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]]
