def obtener_camara_handler(clientID):
"""
Funcion que obtiene un handler de la camara, el cual se puede utilizar
para acceder al laser mas adelante. Ademas, inicializa el visor de la
camara y del laser
Args:
clientID: ID del cliente que ha establecido una conexion con el
servidor de V-REP.
Return:
Devuelve el handler de la camara
"""
# Obtener handler
_, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
# Inicializar camara y laser
vrep.simxGetVisionSensorImage(clientID, camhandle, 0,
vrep.simx_opmode_streaming)
vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_streaming)
# Esperar 1 segundo hasta que se rellene el buffer
time.sleep(1)
return camhandle
def dist_cbs(self):
## Read position of blocks from V-REP then send to remoteAPI ##
if self.firstMsgVerification:
err, pos_str = vrep.simxGetStringSignal(clientID, 'cuboidpos',
vrep.simx_opmode_streaming)
self.firstMsgVerification = False
else:
err, pos_str = vrep.simxGetStringSignal(clientID, 'cuboidpos',
vrep.simx_opmode_buffer)
pos = vrep.simxUnpackFloats(pos_str)
#for i in range(0,6):
#dist2 = pos[2]
# print "pos= ",pos
if len(pos) == 6:
dist = pos[0]
dist0 = pos[1]
dist1 = pos[2]
dist2 = pos[3]
dist3 = pos[4]
dist4 = pos[5]
else:
dist = 1.0
dist0 = 1.0
dist1 = 1.0
dist2 = 1.0
dist3 = 1.0
dist4 = 1.0
return dist, dist0, dist1, dist2, dist3, dist4
def get_lidar_data(self):
"""Returns data measured using LIDAR. Make sure robot you use has a lidar"""
error_code, ranges = vrep.simxGetStringSignal(self.client_id, 'scan ranges', STREAM_MODE)
time.sleep(0.1)
while len(ranges) == 0:
error_code, ranges = vrep.simxGetStringSignal(self.client_id, 'scan ranges', BUFFER_MODE)
ranges = vrep.simxUnpackFloats(ranges)
return ranges
def obtener_datos_laser_simulador(terminar_conexion=True):
"""
Se conecta al simulador y obtiene los datos del láser, que devuelve
como una lista de dos elementos: lista[0] es una lista con las coordenadas
X de los puntos y lista[1] una lista con las coordenadas Y. También
devuelve el clientID y robothandle.
@terminar_conexion Si vale True, se termina la simulación tras obtener
los datos del láser.
"""
vrep.simxFinish(-1) #Terminar todas las conexiones
clientID = vrep.simxStart(
'127.0.0.1', 19999, True, True, 5000, 5
) #Iniciar una nueva conexion en el puerto 19999 (direccion por defecto)
# Primero lanzar play en la escena y después ejecutar python
if clientID != -1:
print('Conexion establecida')
else:
sys.exit(
"Error: no se puede conectar. Tienes que iniciar la simulación antes de llamar a este script."
) #Terminar este script
#Guardar la referencia al robot
_, robothandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx',
vrep.simx_opmode_oneshot_wait)
#acceder a los datos del laser
_, datosLaserComp = vrep.simxGetStringSignal(clientID, 'LaserData',
vrep.simx_opmode_streaming)
# Hay que esperar un segundo antes de poder acceder a los datos del láser
time.sleep(1)
puntosx = [
] #listas para recibir las coordenadas x, y z de los puntos detectados por el laser
puntosy = []
puntosz = []
returnCode, signalValue = vrep.simxGetStringSignal(clientID, 'LaserData',
vrep.simx_opmode_buffer)
datosLaser = vrep.simxUnpackFloats(signalValue)
for indice in range(0, len(datosLaser), 3):
puntosx.append(datosLaser[indice + 1])
puntosy.append(datosLaser[indice + 2])
puntosz.append(datosLaser[indice])
if terminar_conexion:
#detenemos la simulacion
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
#cerramos la conexion
vrep.simxFinish(clientID)
# Devuelvo los puntos y el clientID
return [puntosx, puntosy], clientID, robothandle
def get_laser_scan(self):
if self.laser_init:
errorCode, signalVal = vrep.simxGetStringSignal(
self.clientID, "MySignal", vrep.simx_opmode_streaming)
else:
errorCode, signalVal = vrep.simxGetStringSignal(
self.clientID, "MySignal", vrep.simx_opmode_buffer)
assert errorCode <= 1, 'Cannot grab laser data'
data = vrep.simxUnpackFloats(signalVal)
return data[0::2], data[1::2]
def prepare(self):
self.clearSignal()
################ GET ROBOT/GOAL POSITIONS, MAP DIM WITH RESOLUTION, TRANSFORM ###########################
r, self.robotHandle = vrep.simxGetObjectHandle(
self.clientID, 'body#1', vrep.simx_opmode_oneshot_wait)
r, self.goalHandle = vrep.simxGetObjectHandle(
self.clientID, 'Goal', vrep.simx_opmode_oneshot_wait)
r = -1
while (r != vrep.simx_return_ok):
r, robotPosition = vrep.simxGetObjectPosition(
self.clientID, self.robotHandle, -1, vrep.simx_opmode_streaming
) #-1 specifies we want the absolute position
self.env.updateRobotPosition(robotPosition)
r = -1
while (r != vrep.simx_return_ok):
r, goalPosition = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
self.env.updateGoal(goalPosition)
rCode = -1
while (rCode != vrep.simx_return_ok):
rCode, angle = vrep.simxGetObjectOrientation(
self.clientID, self.robotHandle, -1,
vrep.simx_opmode_streaming) #just to prepare our vrep
robotPosition = self.env.transform(robotPosition)
goalPosition = self.env.transform(goalPosition)
self.env.updateRobotPosition(robotPosition)
self.env.updateGoal(goalPosition)
self.env.initializeMap()
################## PREPARE SENSORS ############################
rCode, proxHandle = vrep.simxGetObjectHandle(
self.clientID, 'sensor#1', vrep.simx_opmode_oneshot_wait)
if rCode != vrep.simx_return_ok or rCode != vrep.simx_return_ok:
print("Could not get proximity sensor handle. Exiting.")
sys.exit()
self.proxHandles = [proxHandle]
error, state, point, handle, vector = vrep.simxReadProximitySensor(
self.clientID, self.proxHandles[0], vrep.simx_opmode_streaming)
rCode = -1
vrep.simxGetFloatSignal(
self.clientID, "ProxDistance",
vrep.simx_opmode_streaming) #just to get things started
vrep.simxGetStringSignal(self.clientID, "threeDimData",
vrep.simx_opmode_streaming)
############# INITIALIZE PRIORITY QUEUE ######################
print("OVERALL GOAL POSITION: ", self.env.getGoal())
heapq.heapify(self.open)
self.env.setMap(goalPosition[0], goalPosition[1], 1, 0)
heapq.heappush(self.open, (self.key(goalPosition), goalPosition))
def read_laser(self): """ Gets the 572 points read by the laser sensor. Each reading contains 3 values (x, y, z) of the point relative to the sensor position. Returns: laser: List with 1716 values of x, y and z from each point. """ res, laser = vrep.simxGetStringSignal(self.clientID,"LasermeasuredDataAtThisTime", vrep.simx_opmode_streaming) laser = vrep.simxUnpackFloats(laser) while(res != vrep.simx_return_ok): res, laser = vrep.simxGetStringSignal(self.clientID,"LasermeasuredDataAtThisTime", vrep.simx_opmode_buffer) laser = vrep.simxUnpackFloats(laser) return laser
def _connect(self):
self.__clientID = vrep.simxStart(self.__host_url, self.__port, True, True, 5000, 5) # Connect to V-REP
if self.__clientID != -1:
print ('Connected to remote API server of Vrep with clientID: ', self.__clientID)
self._connectionStatus = True
vrep.simxGetStringSignal(self.__clientID, self.__signalName+'_allSens', vrep. simx_opmode_streaming) #first time call
vrep.simxGetStringSignal(self.__clientID, self.__signalName+'_camera', vrep. simx_opmode_streaming) #first time call
vrep.simxSetStringSignal(self.__clientID, self.__signalName+'_velocities', vrep.simxPackFloats([0.0, 0.0]), vrep.simx_opmode_streaming) #first time call
# if self.__synchronous:
# self.startsim()
else:
logging.error('Failed connecting to remote API server of Vrep')
def dvs(cid, handle=None, signal_name="dataFromThisTimestep",
dim_x=32, dim_y=32, offset=0, magnitude=1):
empty_str=""
raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str)
err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep",
vrep.simx_opmode_oneshot)
err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes,
vrep.simx_opmode_oneshot_wait)
image = np.zeros( (dim_x, dim_y), dtype='uint8' ) + offset
l = len(data)
for i in range( int(l/4) ):
b = list(bytearray(data[i*4:i*4+4]))
x_coord = b[0]
y_coord = b[1]
if x_coord >= 128:
x_coord -= 128
polarity = 1
else:
#polarity = 0
polarity = -1
timestamp = (b[3] * 256) + b[2]
# Distinguish between different polarities
#image[127-y_coord][127-x_coord] = polarity * magnitude
image[dim_y-1-y_coord][dim_x-1-x_coord] = polarity * magnitude
# No Distinction between different polarities
#image[127-y_coord][127-x_coord] = magnitude
return image.ravel()
def getLaserPoints(self):
if self.id != -1:
if self.sec_first_call:
mode = vrepConst.simx_opmode_streaming
res = vrep.simxGetStringSignal(self.id, self.tube_name,
vrepConst.simx_opmode_streaming)
time.sleep(0.05)
self.sec_first_call = False
else:
mode = vrepConst.simx_opmode_buffer
res = vrep.simxGetStringSignal(self.id, self.tube_name,
vrepConst.simx_opmode_buffer)
r = vrep.simxUnpackFloats(res[1])
return r
def getLaserData(self):
res, data = vrep.simxGetStringSignal(self.clientID,
'measuredDataAtThisTime',
vrep.simx_opmode_buffer)
laserData = []
if (data):
# data unpacking
laserDetectedPoints = vrep.simxUnpackFloats(data)
# data loading in robocomp laserData format
for i in range(0, len(laserDetectedPoints) - 2, 3):
x = laserDetectedPoints[i]
y = laserDetectedPoints[i + 1]
z = laserDetectedPoints[i + 2]
singleLaser = TData()
# angle and distance are relative to the laser sensor
# x-axis of the laser points in the direction of movement of robot and z-axis is out of the plane
# distance is in cm
singleLaser.dist = (np.sqrt(x**2 + y**2)) * 100
# angles is in degrees
singleLaser.angle = math.degrees((y / x))
laserData.append(singleLaser)
# print(f'distance: {distance} angle: {angle}')
return laserData
def dvs(cid,
handle=None,
signal_name="dataFromThisTimestep",
dim_x=32,
dim_y=32,
offset=0,
magnitude=1):
empty_str = ""
raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str)
err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep",
vrep.simx_opmode_oneshot)
err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes,
vrep.simx_opmode_oneshot_wait)
image = np.zeros((dim_x, dim_y), dtype='uint8') + offset
l = len(data)
for i in range(int(l / 4)):
b = list(bytearray(data[i * 4:i * 4 + 4]))
x_coord = b[0]
y_coord = b[1]
if x_coord >= 128:
x_coord -= 128
polarity = 1
else:
#polarity = 0
polarity = -1
timestamp = (b[3] * 256) + b[2]
# Distinguish between different polarities
#image[127-y_coord][127-x_coord] = polarity * magnitude
image[dim_y - 1 - y_coord][dim_x - 1 - x_coord] = polarity * magnitude
# No Distinction between different polarities
#image[127-y_coord][127-x_coord] = magnitude
return image.ravel()
def turn(self, speedr, speedl, angle):
"""
turns the unit: giving speed to the left wheel makes the robot to turn right and vice-versa
:param speedr: speed of the right wheel.
:param speedl: speed of the left wheel.
:param angle: turning angle.
"""
self._term.write('turning, angle = {}'.format(angle))
# will contain cumulative turtning angle
z = 0
while z < angle:
time.sleep(1)
# the second parameter is the velocity
# set speed
vrep.simxSetJointTargetVelocity(self._clientID, self.wheels_handles["wheel_right"], speedr,
self._operation_mode)
vrep.simxSetJointTargetVelocity(self._clientID, self.wheels_handles["wheel_left"], speedl,
self._operation_mode)
# get gyroscope data
gyro_data = vrep.simxGetStringSignal(self._clientID, self.signals['gyro_signal'], self._operation_mode)
# gyro_data_unpacked_x = (struct.unpack("f", bytearray(gyro_data[1][:4]))[0] * 180) / math.pi
# gyro_data_unpacked_y = (struct.unpack("f", bytearray(gyro_data[1][4:8]))[0] * 180) / math.pi
# extract degrees per second
gyro_data_unpacked_z = (struct.unpack("f", bytearray(gyro_data[1][8:12]))[0] * 180) / math.pi
# self._term.write('-------------------------------------------------------\n'
# '{} : X-Gyro = {} dps\n Y-Gyro = {} dps\n Z-Gyro = {} dps'
# .format(self._port, round(gyro_data_unpacked_x, 2), round(gyro_data_unpacked_y, 2),
# round(gyro_data_unpacked_z, 2)))
# add up
z += abs(gyro_data_unpacked_z)
# self._term.write('cumulative angle = {}'.format(z))
self._term.write('turn completed')
def fastSensingOverSignal(self):
"""
read all non-camera sensors in one signal call, except for pose
:return:
"""
res, values = vrep.simxGetStringSignal(self.__clientID,
self.__signalName + '_allSens',
vrep.simx_opmode_buffer)
if res == vrep.simx_return_ok:
sensValues = vrep.simxUnpackFloats(values)
self._proximitySensorValues = np.array(sensValues[:8],
dtype=np.float)
self._lightSensorValues = np.array(sensValues[8:16],
dtype=np.float)
self._groundSensorValues = np.array(sensValues[16:19],
dtype=np.float)
self._accelerometerValues = np.array(sensValues[19:22],
dtype=np.float)
self._wheelEncoderValues = np.array(sensValues[22:],
dtype=np.float)
logging.debug(
'Remote signal reception for signal EPUCK_allSens ok')
else:
logging.error(
'Remote signal reception for signal EPUCK_allSens failed')
def wait_for_signal(clientID, signal, mode=vrep.simx_opmode_oneshot_wait):
"""Waits for a signal from V-REP before continuing main client code."""
r = -1
while r != vrep.simx_return_ok:
r, data = vrep.simxGetStringSignal(clientID, signal, mode)
time.sleep(0.1)
return data
def loop(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2]
else:
self.__orientation = None
# print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
self.__mind.setInput("orientation", self.__orientation)
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
self.__position=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
if returnCode==vrep.simx_return_ok:
try:
# self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
self.__mind.setInput("velocity", self.__velocity)
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
self.__mind.setInput("sensorTrigger", sensorTrigger)
self.__mind.setInput("mostSalientItem", self.__mind.selectMostSalient("I"))
# print self.__name, self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName"))
self.__mind.setInput("attendedItem", self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName")))
self.__mind.applyRules()
self.__mind.setStates()
if self.__mind.getOutput("steering")!=None:
self.setSteering(self.__mind.getOutput("steering"))
if self.__mind.getOutput("thrust")!=None:
self.setThrust(self.__mind.getOutput("thrust"))
if self.__mind.getOutput("reward")!=None:
if self.__mind.getOutput("reward")>0.5:
self.setEmotionalExpression(1)
elif self.__mind.getOutput("reward")<-0.5:
self.setEmotionalExpression(-1)
else:
self.setEmotionalExpression(0)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
self.__cnt=self.__cnt+1
def get_signal(self, signal):
res, signal_val = vrep.simxGetStringSignal(self.client_id, signal,
vrep.simx_opmode_blocking)
if res <= 8:
return signal_val if signal_val != '' else '0'
else:
err_print("GET SWARM DATA", parse_error(res))
raise Exception("ERROR IN GET SWARM DATA")
def get_data(own=False):
'''
Función para recoger datos laser del simulador V-REP y crear un archivo
asociado con los puntos detectados.
'''
if (own):
name = input("Introduce el nombre del fichero: ")
tiempo = int(input("Introduce el intervalo de tiempo (s): "))
iteraciones = int(input("Introduce el número de iteraciones: "))
else:
name = "laser.dat"
tiempo = 1
iteraciones = 1
# Abrimos el fichero para escritura
path = "data/{}".format(name)
fd = open(path, 'w')
for i in range(0, iteraciones):
#listas para recibir las coordenadas x, y de los puntos detectados por el laser
puntosx = []
puntosy = []
returnCode, signalValue = vrep.simxGetStringSignal(
clientID, 'LaserData', vrep.simx_opmode_buffer)
time.sleep(
tiempo
) #esperamos un tiempo para que el ciclo de lectura de datos no sea muy rápido
datosLaser = vrep.simxUnpackFloats(signalValue)
for indice in range(0, len(datosLaser), 3):
puntosx.append(datosLaser[indice + 1])
puntosy.append(datosLaser[indice + 2])
plt.clf()
plt.plot(puntosx, puntosy, 'r.')
plt.axis([0, 4, -2, 2])
plt.show()
datos = "Iteracion {}_{}".format(i, len(puntosx))
for x, y in zip(puntosx, puntosy):
datos += " " + str(x) + " " + str(y)
datos += "\n"
fd.write(datos)
fd.close()
def _get_StateReward(self, prefix="", init=False):
v_opmode = vrep.simx_opmode_streaming if init else vrep.simx_opmode_buffer
state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.id_, prefix + "states",
v_opmode)[1]))
reward = vrep.simxGetFloatSignal(self.id_, prefix + "reward",
v_opmode)[1]
return state, reward
def get_hit_object_name(self):
hit_name = ''
res, hit_name = vrep.simxGetStringSignal(self.clientID, 'hit_name',
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
pass
#print ("hit ", hit_name) # display the reply from V-REP (in this case, the handle of the created dummy)
else:
print('Remote function call failed: take action')
return hit_name
def iniciarCamaraLaser(clientID):
#cargamos datos de imagene y camara
_, datosLaserComp = vrep.simxGetStringSignal(clientID, 'LaserData',
vrep.simx_opmode_streaming)
_, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
_, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camhandle, 0, vrep.simx_opmode_streaming)
time.sleep(1)
return camhandle
def __get(self):
vrep.simxGetPingTime(self.__ID)
self.state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "states",
vrep.simx_opmode_buffer)[1]))
self.reward = vrep.simxGetFloatSignal(self.__ID, "reward",
vrep.simx_opmode_buffer)[1]
self.done = bool(
vrep.simxGetIntegerSignal(self.__ID, "done",
vrep.simx_opmode_buffer)[1])
def get_prox_sensors(self):
res, data = vrep.simxGetStringSignal(self.client_id,
'EPUCK_PROXSENS' + self.suffix,
vrep.simx_opmode_oneshot_wait)
err_list = parse_error(res)
if res != 0 and self.debug:
err_print(prefix="SET SPEED LEFT: ", message=err_list)
data = vrep.simxUnpackFloats(data)
self.prox_sensors = data
return {'res': res, 'err_list': err_list, 'data': data}
def get_lidar_data(self):
point_data, dist_data = [], []
e, data = vrep.simxGetStringSignal(self.client_id, "lidarMeasuredData",
vrep.simx_opmode_buffer)
if self.check_error_code(e,
"simxGetStringSignal lidar distances error"):
buf = data.split("!#!")
if len(buf) >= 2:
dist_data = vrep.simxUnpackFloats(buf[1])
measuredData = vrep.simxUnpackFloats(buf[0])
point_data = np.reshape(measuredData,
(int(len(measuredData) / 3), 3))
return e, point_data, dist_data
def obtenerPuntosXY(clientID):
puntosx = [
] #listas para recibir las coordenadas x, y z de los puntos detectados por el laser
puntosy = []
#puntosz=[]
returnCode, signalValue = vrep.simxGetStringSignal(clientID, 'LaserData',
vrep.simx_opmode_buffer)
#esperamos un tiempo para que el ciclo de lectura de datos no sea muy rápido
datosLaser = vrep.simxUnpackFloats(signalValue)
for indice in range(0, len(datosLaser), 3):
puntosx.append(datosLaser[indice + 1])
puntosy.append(datosLaser[indice + 2])
#puntosz.append(datosLaser[indice])
return puntosx, puntosy
def _get_Space(self, soa, prefix="", dtype=np.float32):
if "state" in soa:
max_v = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.id_, prefix + "max_state",
vrep.simx_opmode_blocking)[1]))
min_v = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.id_, prefix + "min_state",
vrep.simx_opmode_blocking)[1]))
elif "action" in soa:
max_v = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.id_, prefix + "max_action",
vrep.simx_opmode_blocking)[1]))
min_v = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.id_, prefix + "min_action",
vrep.simx_opmode_blocking)[1]))
try:
rtv = spaces.Box(min_v, max_v, dtype=dtype)
except:
rtv = spaces.Box(min_v, max_v)
return rtv
def output(self):
if self.__mind.getOutput("steering")!=None:
self.setSteering(self.__mind.getOutput("steering"))
if self.__mind.getOutput("thrust")!=None:
self.setThrust(self.__mind.getOutput("thrust"))
if self.__mind.getOutput("reward")!=None:
if self.__mind.getOutput("reward")>0.5:
self.setEmotionalExpression(1)
elif self.__mind.getOutput("reward")<-0.5:
self.setEmotionalExpression(-1)
else:
self.setEmotionalExpression(0)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
def lidarScan360(clientID, servo):
scan = []
e = vrep.simxSetJointPosition(clientID, servo, -np.pi / 2,
vrep.simx_opmode_blocking)
eData = vrep.simxGetStringSignal(clientID, "mySignal",
vrep.simx_opmode_blocking)
scan = vrep.simxUnpackFloats(eData[1])
e = vrep.simxSetJointPosition(clientID, servo, np.pi / 2,
vrep.simx_opmode_blocking)
eData = vrep.simxGetStringSignal(clientID, "mySignal",
vrep.simx_opmode_blocking)
data = vrep.simxUnpackFloats(eData[1])
scan = scan[::-1]
data = data[::-1]
for x in data:
scan.append(x)
for i in range(len(scan)):
scan[i] = min(50000, 5000 * scan[i])
while len(scan) > 360:
scan.pop()
# print(scan)
return scan
def compute(self):
print 'SpecificWorker.compute...'
#computeCODE
res, data = vrep.simxGetStringSignal(self.clientID, 'MySignal',
vrep.simx_opmode_buffer)
if (data):
laserDetectedPoints = vrep.simxUnpackFloats(data)
for i in range(len(laserDetectedPoints) / 2):
singleLaser = TData()
singleLaser.dist = laserDetectedPoints[2 * i]
singleLaser.angle = laserDetectedPoints[2 * i + 1]
self.laserData.append(singleLaser)
time.sleep(2)
return True
def checkGround(self, robotPosition):
r, distance = vrep.simxGetFloatSignal(self.clientID, "GroundDistance",
vrep.simx_opmode_buffer)
r, table = vrep.simxGetStringSignal(self.clientID, "threeDimData",
vrep.simx_opmode_buffer)
if r == vrep.simx_return_ok:
table = vrep.simxUnpackFloats(table)
dim = int((len(table) / 3)**(1 / 2))
if dim * dim * 3 != len(table):
return set()
heights = np.array(table).reshape((dim, dim, 3))[:, :, 0]
cliffs = self.env.analyzeCliffs(
heights) #returns a set of relative locations of cliffs
return cliffs
else:
return set()
def loop(self):
operationMode = vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop = False
else:
operationMode = vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(
self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode == vrep.simx_return_ok:
self.__orientation = orientation[2] #Z
else:
self.__orientation = None
print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
returnCode, position = vrep.simxGetObjectPosition(
self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode == vrep.simx_return_ok:
self.__position = [0.0, 0.0]
self.__position[0] = position[0] #X
self.__position[1] = position[1] #Y
else:
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
self.__position = None
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(
self.__clientID, self.__sensorHandle, operationMode)
if returnCode == vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
simulationTime = vrep.simxGetLastCmdTime(self.__clientID)
thrust = 0.0
steering = 0.0
if simulationTime - self.__driveBackStartTime < 3000:
# driving backwards while slightly turning:
thrust = -1.0
steering = -1.0
else:
# going forward:
thrust = 1.0
steering = 0.0
if sensorTrigger:
self.__driveBackStartTime = simulationTime # We detected something, and start the backward mode
self.setSteering(steering)
self.setThrust(thrust)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(
self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage != "":
print("Debug:" + dMessage)
def procesar_ciclo(clientID, segundos, iter):
"""
Funcion que procesa un ciclo de simulacion. Obtiene los datos y los transforma
al formato JSON especificado.
Args:
clientID: ID del cliente que ha establecido una conexion con el
servidor de V-REP.
segundos: Numero de segundos que esperar para procesar los datos.
iter: Numero de iteracion.
Return:
Devuelve un objeto en formato JSON especificando la iteracion y los
puntos detectados en el eje X y en el eje Y
"""
# Obtener coordenadas x,y,z detectadas por laser
puntosx = []
puntosy = []
puntosz = []
# Obtener señal
_, signalValue = vrep.simxGetStringSignal(clientID, 'LaserData',
vrep.simx_opmode_buffer)
# Esperar para procesar los datos
print(
f"Leidos datos en iteracion {iter}. Esperando {segundos} seg. para procesarlos..."
)
time.sleep(segundos)
# Procesar datos
datosLaser = vrep.simxUnpackFloats(signalValue)
for indice in range(0, len(datosLaser), 3):
puntosz.append(datosLaser[indice])
puntosx.append(datosLaser[indice + 1])
puntosy.append(datosLaser[indice + 2])
# Obtener lectura
lectura = {"Iteracion": iter, "PuntosX": puntosx, "PuntosY": puntosy}
return lectura
def loop(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2] #Z
else:
self.__orientation = None
print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
self.__position=None
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
simulationTime=vrep.simxGetLastCmdTime(self.__clientID)
thrust=0.0
steering=0.0
if simulationTime-self.__driveBackStartTime<3000:
# driving backwards while slightly turning:
thrust=-1.0
steering=-1.0
else:
# going forward:
thrust=1.0
steering=0.0
if sensorTrigger:
self.__driveBackStartTime=simulationTime # We detected something, and start the backward mode
self.setSteering(steering)
self.setThrust(thrust)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
def _process_events(cid, attr, mode=oneshot_wait, last=None):
err, data = vrep.simxGetStringSignal(cid, attr, mode)
safe_call(vrep.simxClearStringSignal, cid, attr, mode)
x, y, signs, times = [], [], [], []
if err or len(data) == 0:
pass
elif last is not None and (len(data) == last[0] and data[:4] == last[1]):
pass
else:
last = (len(data), data[:4])
# --- Format data
data = np.array(bytearray(data), dtype=np.uint8)
data.shape = (-1, 4)
signs = data[:, 0] >= 128
data[:, 0] -= signs * 128
x, y = data[:, 0], data[:, 1]
times = 256 * data[:, 3] + data[:, 2]
return (x, y, signs, times), last
def _read_sensors(self):
"""
This method is used for read the ePuck's sensors. Don't use directly,
instead use 'step()'
"""
# Read differents sensors
for s in self._sensors_to_read:
if s == 'a':
# Accelerometer sensor in a non filtered way
if self._accelerometer_filtered:
parameters = ('A', 12, '@III')
else:
parameters = ('a', 6, '@HHH')
self._debug('WARNING: Accelerometer not yet implemented!')
elif s == 'n':
# Proximity sensors
res, prox = vrep.simxGetStringSignal(self._clientID, 'EPUCK_proxSens', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Proximity sensors readout failed: ", res)
else:
proxVals = vrep.simxUnpackFloats(prox)
# TODO: Find out actual needed scaling factor
proxVals = [int(x * 1000) for x in proxVals]
self._proximity = tuple(proxVals)
elif s == 'm':
# Floor sensors
res, floor1 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_0', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 1 sensor readout failed: ", res)
res, floor2 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_1', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 2 sensor readout failed: ", res)
res, floor3 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_2', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 3 sensor readout failed: ", res)
# Scale returned values to mimic real robot; current factor is just guessed
self._floor_sensors = (floor1*1800, floor2*1800, floor3*1800)
elif s == 'q':
# Motor position sensor
# First: Get the handles of both motor joints
res, leftMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Unable to get handle of left motor: ", res)
continue
res, rightMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Unable to get handle of right motor: ", res)
continue
# Second: Get the actual motor position (in radians)
res, leftPos = vrep.simxGetJointPosition(self._clientID, leftMotor, vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
res, rightPos = vrep.simxGetJointPosition(self._clientID, rightMotor, vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
self._motor_position = (leftPos, rightPos)
elif s == 'o':
# Light sensors
parameters = ('O', 16, '@HHHHHHHH')
self._debug('WARNING: Light sensors not yet implemented!')
elif s == 'u':
# Microphone
parameters = ('u', 6, '@HHH')
self._debug('WARNING: Microphones not yet implemented!')
elif s == 'e':
# Motor Speed
parameters = ('E', 4, '@HH')
self._debug('WARNING: Motor speed not yet implemented!')
elif s == 'i':
# Do nothing for the camera, is an independent process
pass
else:
self._debug('Unknow type of sensor to read')
# Update display
detection_display.set_data(img)
detection_display.axes.figure.canvas.draw()
cid = vrep.simxStart('127.0.0.1',20002,True,True,5000,5)
if cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % cid)
vrep.simxStartSimulation( cid, vrep.simx_opmode_oneshot )
else:
print ('Failed connecting to V-REP remote API server')
exit(1)
while True:
err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep",
vrep.simx_opmode_oneshot)
err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes,
vrep.simx_opmode_oneshot_wait)
l = len(data)
for i in range( int(l/4) ):
b = list(bytearray(data[i*4:i*4+4]))
x_coord = b[0]
y_coord = b[1]
if x_coord > 128:
x_coord -= 128
polarity = 1
else:
polarity = 0
timestamp = (b[3] * 256) + b[2]
# Distinguish between different polarities
import numpy as np
initialCall=True
print ('VREP Simulation Program')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print ('Connected to remote API server')
else:
print('Connection Failure')
sys.exit('Abort Connection')
# Object handle
_,Quadbase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait)
while True:
# Code for testing...
if initialCall:
mode = vrep.simx_opmode_streaming
initialCall = False
else:
mode = vrep.simx_opmode_buffer
errorFlag,rawStringData=vrep.simxGetStringSignal(clientID,'rawMeasuredData',mode)
if errorFlag == vrep.simx_return_ok:
rawFloatData=vrep.simxUnpackFloats(rawStringData)
print(len(rawFloatData))
else:
print('Measurements Awaiting')
time.sleep(1.0)
def run_simulation(self, trajectory):
"""
Trajectory is a list 6 pairs of vectors, each of the same length.
For each pair:
1. The first vector is the position of the motor in rad.
2. The second vector is the max velocity of the motor in rad/s.
"""
self._com(remote_api.simxStopSimulation, get=False)
if self.verbose:
print("Setting parameters...")
traj = self._prepare_traj(trajectory)
packed_data = remote_api.simxPackFloats(traj)
raw_bytes = (ctypes.c_ubyte * len(packed_data)).from_buffer_copy(packed_data)
assert remote_api.simxSetStringSignal(self.api_id, 'trajectory', raw_bytes,
remote_api.simx_opmode_oneshot_wait) == 0
self._com(remote_api.simxSetIntegerSignal, 'state', 1, get=False)
self._com(remote_api.simxGetIntegerSignal, 'state', get=True,
mode=remote_api.simx_opmode_streaming, ret_ok=(0, 1))
time.sleep(0.1)
self._com(remote_api.simxStartSimulation, get=False)
if self.verbose:
print("Simulation started.")
time.sleep(0.01)
start_time = time.time()
while not self.simulation_paused():
if time.time() - start_time > 60.0:
print("Simulation seems hung. Exiting...")
sys.exit(1)
time.sleep(0.005)
time.sleep(1.0)
if self.verbose:
print("Getting resulting parameters.")
object_sensors = self._get_signal('object_sensors')
collide_data = self._get_signal('collide_data')
contact_data = self._get_signal('contact_data')
contact_type = self._get_signal('contact_type', int_type=True)
contacts = []
for i in range(0, len(contact_type), 3):
step = contact_type[i]
obj_names = [self.handles[contact_type[i+1]],self.handles[contact_type[i+2]]]
data = contact_data[2*i:2*i+6]
contacts.append(Contact(step, obj_names, data))
first_c, last_c = None, None
max_f, max_c = 0.0, None
for c in contacts:
if c.obj_names[0] in self.tracked_objects or c.obj_names[1] in self.tracked_objects:
if first_c is None:
first_c = c
last_c = c
if max_f < c.force_norm:
max_f, max_c = c.force_norm, c
salient_contacts = {'first': first_c, 'last': last_c, 'max': max_c}
if first_c is not None:
print('first contact: {} {:.2f} N'.format('|'.join(first_c.obj_names), first_c.force_norm))
if max_c is not None:
print(' max contact: {} {:.2f} N'.format('|'.join(max_c.obj_names), max_c.force_norm))
marker_sensors = None
if self.cfg.sprims.tip:
marker_sensors = np.array(remote_api.simxUnpackFloats(
remote_api.simxGetStringSignal(self.api_id, 'marker_sensors',
remote_api.simx_opmode_oneshot_wait)))
# assert len(positions) == len(quaternions) == len(velocities)
if self.verbose:
print("End of simulation.")
joint_sensors = None
return {'object_sensors' : object_sensors,
'joint_sensors' : joint_sensors,
'marker_sensors' : marker_sensors,
'collide_data' : collide_data,
'contacts' : contacts,
'salient_contacts': salient_contacts}
