Ejemplo n.º 1
0
	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
Ejemplo n.º 2
0
    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
Ejemplo n.º 3
0
    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
Ejemplo n.º 4
0
 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)
Ejemplo n.º 5
0
    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
Ejemplo n.º 6
0
    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
Ejemplo n.º 7
0
 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)
Ejemplo n.º 8
0
    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]
Ejemplo n.º 9
0
 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')
Ejemplo n.º 10
0
    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
Ejemplo n.º 11
0
    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
Ejemplo n.º 12
0
 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
Ejemplo n.º 13
0
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
Ejemplo n.º 14
0
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
Ejemplo n.º 16
0
 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]
Ejemplo n.º 17
0
 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
Ejemplo n.º 18
0
    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
Ejemplo n.º 19
0
 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]
Ejemplo n.º 20
0
    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}
Ejemplo n.º 21
0
 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
Ejemplo n.º 25
0
 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
Ejemplo n.º 26
0
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
Ejemplo n.º 27
0
    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
Ejemplo n.º 28
0
 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
Ejemplo n.º 30
0
    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)
Ejemplo n.º 31
0
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
Ejemplo n.º 32
0
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)
Ejemplo n.º 33
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}
Ejemplo n.º 34
0
    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')