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 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 receiveState(self, msg):
s = np.array(vrep.simxUnpackFloats(msg.data))
finish = 0
self.prevIt = self.currIt
a = (self.sendAction(s))
if type(self.prev["S"]) == np.ndarray:
r, finish = self.rewardFunction(self.prev['S'], self.prev['A'], s)
r = np.array(r).reshape(1, -1)
if finish:
print('#### SUCCESS!!!! ####')
self.agent.store(self.prev['S'].reshape(1, -1), self.prev["A"], r,
s.reshape(1, -1), a, finish)
self.currReward += np.asscalar(r)
if self.trainMode and len(self.agent.exp) >= self.agent.batch_size:
self.agent.train()
self.prev["S"] = s
self.prev["A"] = a.reshape(1, -1)
s = s.ravel()
self.currIt += 1
if self.currIt > self.c or finish:
msg = Int8()
msg.data = 1
self.restart.publish(msg)
return
def _get_signal(self, name, int_type=False):
s = self._com(remote_api.simxGetStringSignal, name, get=True)
if int_type:
data = remote_api.simxUnpackInts(s)
else:
data = remote_api.simxUnpackFloats(s)
return np.array(data)
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 receiveState(self, msg):
floats = vrep.simxUnpackFloats(msg.data)
self.goal = np.array(floats[-4:-1])
fail = floats[-1]
restart = 0
floats = floats[:self.s_n]
first, second = self.splitState(floats)
s = (np.array(floats)).reshape(1, -1)
first = torch.FloatTensor(first).view(1, -1)
second = torch.FloatTensor(second).view(1, -1)
a = (self.sendAction(s, [first, second]))
if type(self.prev["S"]) == np.ndarray:
r, restart = self.rewardFunction(s, self.prev['A'])
self.agent.store(self.prev['S'], self.prev["A"], r, s, a, restart,
self.prevLocals, (first, second))
self.currReward += r
self.prev["S"] = s
self.prev["A"] = a
self.prevLocals = (first, second)
s = s.ravel()
if self.trainMode:
self.agent.train()
self.restartProtocol(restart or fail)
return
def _get_signal(self, name, int_type=False):
s = self._com(remote_api.simxGetStringSignal, name, get=True)
if int_type:
data = remote_api.simxUnpackInts(s)
else:
data = remote_api.simxUnpackFloats(s)
return np.array(data)
def get_target(self, obj):
gain_pos, gain_ori = obj.product()
err, self.dis_signal = vrep.simxReadStringStream(
self.cid, 'dis_data', vrep.simx_opmode_buffer)
dis_data = ''
if err == vrep.simx_return_ok:
dis_data = vrep.simxUnpackFloats(self.dis_signal)
print('dis len', len(dis_data))
if len(dis_data) > 0:
print('gain_pos', gain_pos, 'dis data', dis_data)
if gain_pos[0] > 0:
if dis_data[0] < 0.25:
gain_pos[0] = 0
if gain_pos[0] < 0:
if dis_data[1] < 0.25:
gain_pos[0] = 0
if gain_pos[1] > 0:
if dis_data[2] < 0.25:
gain_pos[1] = 0
if gain_pos[1] < 0:
if dis_data[3] < 0.25:
gain_pos[1] = 0
for i in range(3):
self.t_pos[i] += gain_pos[i]
self.t_ori[i] += gain_ori[i]
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 receiveState(self, msg):
floats = vrep.simxUnpackFloats(msg.data)
self.goal = np.array(floats[-4:-1])
fail = floats[-1]
restart = 0
floats = floats[:self.s_n]
s = (np.array(floats)).reshape(1, -1)
a = (self.sendAction(s))
if type(self.prev["S"]) == np.ndarray:
r, restart = self.rewardFunction(s, self.prev['A'])
if r == 5:
print(" #### Phase ", self.phase, " Complete!")
self.phase += 1
self.agent.store(self.prev['S'], self.prev["A"],
np.array([r]).reshape(1, -1), s, a, restart)
self.currReward += r
if self.trainMode:
loss = self.agent.train()
self.prev["S"] = s
self.prev["A"] = a
self.restartProtocol(restart or fail)
return
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 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_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 receiveState(self, msg):
if self.mode == 'Plan':
floats = vrep.simxUnpackFloats(msg.data)
floats = np.array(floats).ravel()
phase = floats[-1]
states = self.split_state(floats[:-1])
a = self.sendActionForPlan(states, phase)
return
def getRobotData(self, message):
floats = vrep.simxUnpackFloats(message.data)
self.env.robotPositionUT = (floats[0], floats[1], floats[2])
self.env.robotPosition = self.env.transform(self.env.robotPositionUT)
self.env.goalPosition = self.env.transform(
(floats[3], floats[4], floats[5]))
self.orientation = (floats[6], floats[7], floats[8])
self.proxVec = (floats[9], floats[10], floats[11])
self.distance = floats[12]
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 receiveState(self, msg):
floats = vrep.simxUnpackFloats(msg.data)
restart = floats[-1]
floats = floats[:-1]
s = (np.array(floats)).reshape(1, -1)
self.agent.store(s)
self.restartProtocol(restart)
return
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 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(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 receive_state_info(self, msg):
state = np.array(vrep.simxUnpackFloats(msg.data))
if self.shut_down:
msg = Vector3()
msg.x = 1
msg.y = 1
self.pub.publish(msg)
elif self.finished(state):
if (not self.is_not_pushing_box(state)
) and state[7] < -.25 and dist(state[5:7],
np.zeros(2)) < 1: # hole
msg = Vector3()
msg.x = -2
msg.y = -2
else:
msg = Vector3()
msg.x = 0
msg.y = 0
self.pub.publish(msg)
if self.counter == 0 and not self.shut_down:
if self.finished(state):
if not self.waiting_for_target:
rospy.sleep(1)
self.publish_finished_to_map()
self.waiting_for_target = True
rospy.wait_for_message("/target" + str(self.robot_id),
Vector3)
self.waiting_for_target = False
else:
self.controller.goal = state.ravel()[:2]
if self.is_not_pushing_box(state):
action_index = 0 if abs(
state[6]
) > .5 else 1 # align yourself otherwise travel towards node
else:
if any(np.isnan(state)):
print(state)
assert not any(np.isnan(state))
action_index = self.policy.get_action(state,
testing_time=True,
probabilistic=True)
action_name = action_map[action_index]
adjusted_state_for_controls = self.controller.feature_2_task_state(
state)
left_right_frequencies = self.controller.getPrimitive(
adjusted_state_for_controls, action_name)
msg = Vector3()
msg.x = left_right_frequencies[0]
msg.y = left_right_frequencies[1]
self.pub.publish(msg)
self.counter = (self.counter + 1) % self.period
return
def receiveState(self, msg):
if self.restart_timer:
self.start_time = time.time()
self.restart_timer = False
floats = vrep.simxUnpackFloats(msg.data)
restart = 0
r = None
self.dist_box_from_goal = dist(np.array(floats[0:2]),
np.array(floats[6:8]))
features = self.feature_joint_2_feature(floats)
floats = self.feature_joint_2_joint(np.array(floats).ravel())
s = (np.array(floats)).reshape(1, -1)
angles = self.getAngles(s)
states = self.splitState(s)
self.box_height = states['box'][-1]
a = (self.sendAction(s))
if self.mode == 'GET_STATE_DATA':
if self.changeAction[0]:
self.curr_rollout1.append(features[0])
if self.changeAction[1]:
self.curr_rollout2.append(features[1])
rest = 0
for i in range(self.num_agents):
loc = 'robot' + str(i)
angle = angles[loc]
curr_state = self.getNetInput(states[loc], angle, i)
if type(self.prev['S'][i]) == np.ndarray and type(
self.prev['A'][i]) == int:
prevAngle = self.prevAngles[loc]
prev_state = self.getNetInput(self.prev['S'][i], prevAngle, i)
r, restart = self.rewardFunction(curr_state, prev_state, i)
rest = restart if restart == 1 else rest
if self.changeAction[i] or rest:
r = r if self.isValidAction(self.prev['S'][i],
self.prev['A'][i], angle,
i) or rest else -1
self.agent.store(prev_state, self.prev["A"][i],
np.array([r]).reshape(1, -1), curr_state,
a[i], restart)
self.currReward += r
if self.changeAction[i]:
self.prev["S"][i] = states[loc]
self.prev["A"][i] = int(a[i])
self.prevAngles[loc] = angles[loc]
if any(self.changeAction) and self.trainMode:
loss = self.agent.train()
if restart and r > 0 and self.mode == 'GET_STATE_DATA':
self.curr_rollout1.append(features[0])
self.curr_rollout2.append(features[1])
self.restartProtocol(rest, succeeded=r > 0 if r else False)
return
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 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 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 draw_map(self, obj):
x = []
y = []
img_np = []
minx, miny = 999, 999
err, self.laser_signal = vrep.simxReadStringStream(
self.cid, 'laser_data', vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
laser_data = vrep.simxUnpackFloats(self.laser_signal)
for i in range(0, len(laser_data), 3):
if laser_data[i + 2] > 0: #remove floor data
x.append(laser_data[i])
y.append(laser_data[i + 1])
if len(x) != 0 and self.first:
img_np = self.draw_plt(x, y)
if len(img_np) != 0:
obj.send_data(img_np)
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
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}
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')
