コード例 #1
0
class Robot:

    # Handles
    robotHandle = None
    motorHandle = [None] * 2
    wheelHandle = [None] * 2
    sonarHandle = [None] * 16

    # Ground Truths
    robotPosOrn = {
        'first': [None] * 3,
        'current': [None] * 3,
        'last': [None] * 3
    }

    # Constants
    SONAR_POSITIONS = [None] * 16
    PI = np.pi
    SONAR_ANGLES = np.array([
        PI / 2, 50 / 180.0 * PI, 30 / 180.0 * PI, 10 / 180.0 * PI,
        -10 / 180.0 * PI, -30 / 180.0 * PI, -50 / 180.0 * PI, -PI / 2, -PI / 2,
        -130 / 180.0 * PI, -150 / 180.0 * PI, -170 / 180.0 * PI,
        170 / 180.0 * PI, 150 / 180.0 * PI, 130 / 180.0 * PI, PI / 2
    ])

    L = None
    L2 = None
    R = None

    def __init__(self):

        self.sim = Simulator('127.0.0.1', 25000)
        self.sim.connect()
        ### -----------
        ### GET HANDLES

        # ROBOT
        self.robotHandle = self.sim.getHandle("Pioneer_p3dx")

        # ACTUATORS
        self.motorHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftMotor')
        self.motorHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightMotor')

        # SONARS
        for x in range(0, len(self.sonarHandle)):
            self.sonarHandle[x] = self.sim.getHandle(
                'Pioneer_p3dx_ultrasonicSensor' + str(x + 1))

        # VISION
        #self.visionHandle = self.sim.getHandle('camera')
        self.visionHandle = 0

        # WHEEL
        self.wheelHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftWheel')
        self.wheelHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightWheel')

        ### -----------
        ### INIT STREAMS
        self.sim.initObjectPositionStream(self.robotHandle, -1)
        self.sim.initObjectOrientationStream(self.robotHandle, -1)
        for x in range(0, len(self.sonarHandle)):
            self.sim.initProximitySensor(self.sonarHandle[x])
        self.sim.initFloatSignal("gyroZ")
        #self.sim.initVisionSensorImage(self.visionHandle)
        self.sim.initObjectVelocity(self.robotHandle)
        self.sim.initJointPositionStream(self.motorHandle[0])
        self.sim.initJointPositionStream(self.motorHandle[1])
        self.sim.initLaserSensor("measuredDataAtThisTime")

        ### -----------
        ### POSTIONS

        # SONAR (em relacao ao frame do robo)
        for x in range(0, len(self.sonarHandle)):
            self.SONAR_POSITIONS[x] = self.sim.getObjectPositionBlock(
                self.sonarHandle[x], self.robotHandle)

        self.L = np.linalg.norm(
            self.sim.getObjectPositionBlock(self.wheelHandle[0],
                                            self.wheelHandle[1]))

        self.L2 = -self.sim.getObjectPositionBlock(self.wheelHandle[0],
                                                   self.robotHandle)[0]

        # Robo precisa estar no chão para pegar o raio
        self.R = self.sim.getObjectPositionBlock(self.wheelHandle[0], -1)[2]
        #self.R = 0.09749898314476013

        # ROBO
        self.robotPosOrn['first'] = self.getPosOrn()

        # Odometry
        self.odometryPosOrn = {}
        self.odometryPosOrn['raw'] = self.robotPosOrn['first']
        self.odometryPosOrn['encoder'] = self.robotPosOrn['first']
        self.odometryPosOrn['compass'] = self.robotPosOrn['first']
        self.odometryPosOrn['encoder_compass'] = self.robotPosOrn['first']

        self.odometryEncoder = {}
        self.odometryEncoder['encoder'] = [None] * 2
        self.odometryEncoder['encoder'][0] = self.sim.getJointPosition(
            self.motorHandle[0])
        self.odometryEncoder['encoder'][1] = self.sim.getJointPosition(
            self.motorHandle[1])
        self.odometryEncoder['compass'] = [None] * 2
        self.odometryEncoder['compass'][0] = self.sim.getJointPosition(
            self.motorHandle[0])
        self.odometryEncoder['compass'][1] = self.sim.getJointPosition(
            self.motorHandle[1])
        self.odometryEncoder['encoder_compass'] = [None] * 2
        self.odometryEncoder['encoder_compass'][0] = self.sim.getJointPosition(
            self.motorHandle[0])
        self.odometryEncoder['encoder_compass'][1] = self.sim.getJointPosition(
            self.motorHandle[1])

        self.odometryTimeRaw = time.time()
        self.odometryTimeCompass = time.time()
        self.odometryTimeEncoderCompass = time.time()

        self.motorW = [None] * 2
        self.motorW[0] = 0
        self.motorW[1] = 0

    ### -----------
    ### DRIVE

    def move(self, left, right):
        self.computeOdometry()
        self.motorW[0] = left
        self.motorW[1] = right
        self.sim.setJointTargetVelocity(self.motorHandle[0], left)
        self.sim.setJointTargetVelocity(self.motorHandle[1], right)

    def stop(self):
        self.move(0, 0)

    def drive(self, vLinear, vAngular):
        self.move(self.vLToDrive(vLinear, vAngular),
                  self.vRToDrive(vLinear, vAngular))

    def vRToDrive(self, vLinear, vAngular):
        return (((2 * vLinear) + (self.L * vAngular)) / 2 * self.R)

    def vLToDrive(self, vLinear, vAngular):
        return (((2 * vLinear) - (self.L * vAngular)) / 2 * self.R)

    ### -----------
    ### SONAR

    def readSonars(self):
        pointCloud = []
        distances = []

        for i in range(0, len(self.sonarHandle)):
            state, distance = self.sim.readProximitySensor(self.sonarHandle[i])

            if (state == True):
                x = self.SONAR_POSITIONS[i][0] + (distance *
                                                  np.cos(self.SONAR_ANGLES[i]))
                y = self.SONAR_POSITIONS[i][1] + (distance *
                                                  np.sin(self.SONAR_ANGLES[i]))
                pointCloud.append((x, y))
                distances.append(distance)
            else:
                pointCloud.append((np.inf, np.inf))
                distances.append(np.inf)

        return distances, pointCloud

    ### -----------
    ### LASER

    def readLaser(self):
        pointCloud = self.sim.readLaserSensor("measuredDataAtThisTime")
        laser_array = np.reshape(pointCloud, (int(len(pointCloud) / 3), 3))

        return laser_array

    ### -----------
    ### IMAGE
    def getSensorViewImage(self):
        resolution, image_array = self.sim.getVisionSensorImage(
            self.visionHandle)
        image = np.array(image_array,
                         dtype=np.uint8)  #Create a numpy array with uint8 type
        image.resize([resolution[1], resolution[0], 3])

        return image

    ### -----------
    ### ODOMETRY

    def _computeMotorAngularDisplacement(self, i, name):
        posF = self.sim.getJointPosition(self.motorHandle[i])
        posI = self.odometryEncoder[name][i]
        self.odometryEncoder[name][i] = posF

        diff = posF - posI

        if (diff >= np.pi):
            diff = 2 * np.pi - diff
        elif (diff <= -np.pi):
            diff = 2 * np.pi + diff

        return diff

    def _computeOdometry(self, posOrn, thetaL, thetaR, deltaTheta):
        deltaS = self.R * (thetaR + thetaL) / 2

        if deltaTheta is None:
            deltaTheta = self.R * (thetaR - thetaL) / self.L

        angle = posOrn[2] + (deltaTheta / 2)

        return posOrn + np.array([
            deltaS * np.cos(angle) - deltaTheta * self.L2 * np.sin(angle),
            deltaS * np.sin(angle) + deltaTheta * self.L2 * np.cos(angle),
            deltaTheta
        ])

    def computeOdometry(self):

        deltaTime = time.time() - self.odometryTimeRaw
        self.odometryTimeRaw = time.time()

        thetaL = self.motorW[0] * deltaTime
        thetaR = self.motorW[1] * deltaTime

        self.odometryPosOrn['raw'] = self._computeOdometry(
            self.odometryPosOrn['raw'], thetaL, thetaR, None)

    def computeOdometryEncoder(self):
        thetaL = self._computeMotorAngularDisplacement(0, 'encoder')
        thetaR = self._computeMotorAngularDisplacement(1, 'encoder')

        self.odometryPosOrn['encoder'] = self._computeOdometry(
            self.odometryPosOrn['encoder'], thetaL, thetaR, None)

    def computeOdometryCompass(self):

        deltaTime = time.time() - self.odometryTimeCompass
        self.odometryTimeCompass = time.time()

        thetaL = self._computeMotorAngularDisplacement(0, 'compass')
        thetaR = self._computeMotorAngularDisplacement(1, 'compass')
        deltaTheta = self.sim.getFloatSignal("gyroZ") * deltaTime

        self.odometryPosOrn['compass'] = self._computeOdometry(
            self.odometryPosOrn['compass'], thetaL, thetaR, deltaTheta)

    def computeOdometryEncoderCompass(self):

        deltaTime = time.time() - self.odometryTimeEncoderCompass
        self.odometryTimeEncoderCompass = time.time()

        thetaL = self._computeMotorAngularDisplacement(0, 'encoder_compass')
        thetaR = self._computeMotorAngularDisplacement(1, 'encoder_compass')
        deltaThetaCompass = self.sim.getFloatSignal("gyroZ") * deltaTime
        deltaThetaEncoder = self.R * (thetaR - thetaL) / self.L

        self.odometryPosOrn['encoder_compass'] = self._computeOdometry(
            self.odometryPosOrn['encoder_compass'], thetaL, thetaR,
            (deltaThetaCompass + deltaThetaEncoder) / 2)

    ### -----------
    ### GETTERS

    def localToGlobalGT(self, position):
        return localToGlobal(self.getPosOrn(), position)

    def localToGlobalOdometry(self, position):
        return localToGlobal(self.getPosOrn(), position)

    def getPosOrn(self):
        #x, y = self.sim.getObjectPosition(self.robotHandle, -1)[:2]
        position = self.sim.getObjectPosition(self.robotHandle, -1)[:2]
        x = position[0]
        y = position[1]
        orn = self.sim.getObjectOrientation(self.robotHandle, -1)[2]
        return x, y, orn

    def getPosOrnOdometyRaw(self):
        return self.odometryPosOrn['raw']

    def getPosOrnOdometyCompass(self):
        return self.odometryPosOrn['compass']

    def getPosOrnOdometyEncoder(self):
        return self.odometryPosOrn['encoder']

    def getPosOrnOdometyEncoderCompass(self):
        return self.odometryPosOrn['encoder_compass']

    def getLinearVelocity(self):
        return np.linalg.norm(
            self.sim.getObjectVelocity(self.robotHandle)[1][2])
コード例 #2
0
class Robot:

    # Handles 
    robotHandle = None
    motorHandle = [None] * 2
    wheelHandle = [None] * 2
    sonarHandle = [None] * 16    
    
    # Constants
    SONAR_POSITIONS = [None] * 16   
    PI = np.pi
    SONAR_ANGLES = np.array([PI/2, 50/180.0*PI, 30/180.0*PI, 10/180.0*PI, 
                       -10/180.0*PI, -30/180.0*PI, -50/180.0*PI, -PI/2, -PI/2, 
                       -130/180.0*PI, -150/180.0*PI, -170/180.0*PI, 170/180.0*PI,
                       150/180.0*PI, 130/180.0*PI, PI/2]) 
    
    L = None
    L2 = None
    R = None
    
    def __init__(self):

        self.sim = Simulator('127.0.0.1', 25000)
        self.sim.connect()        
        ### -----------
        ### GET HANDLES
        
        # ROBOT
        self.robotHandle = self.sim.getHandle("Pioneer_p3dx")
        
        # ACTUATORS
        self.motorHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftMotor')
        self.motorHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightMotor')
    
        # SONARS
        for x in range(0, len(self.sonarHandle)):
            self.sonarHandle[x] = self.sim.getHandle('Pioneer_p3dx_ultrasonicSensor'+str(x+1))
            
        # VISION
        #self.visionHandle = self.sim.getHandle('camera')
        self.visionHandle = 0
            
        # WHEEL
        self.wheelHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftWheel')
        self.wheelHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightWheel')

        ### -----------
        ### INIT STREAMS
        self.sim.initObjectPositionStream(self.robotHandle, -1)
        self.sim.initObjectOrientationStream(self.robotHandle, -1)
        for x in range(0, len(self.sonarHandle)):
            self.sim.initProximitySensor(self.sonarHandle[x])
        self.sim.initFloatSignal("gyroZ")
        #self.sim.initVisionSensorImage(self.visionHandle)
        self.sim.initObjectVelocity(self.robotHandle)
        self.sim.initJointPositionStream(self.motorHandle[0])
        self.sim.initJointPositionStream(self.motorHandle[1])
        self.sim.initLaserSensor("measuredDataAtThisTime")
        

        ### -----------
        ### POSTIONS    
        
        # SONAR (em relacao ao frame do robo)
        for x in range(0, len(self.sonarHandle)):
            self.SONAR_POSITIONS[x] = self.sim.getObjectPositionBlock(
                    self.sonarHandle[x], self.robotHandle) 
            
        self.L = np.linalg.norm(self.sim.getObjectPositionBlock(
                self.wheelHandle[0], self.wheelHandle[1]))
        
        
        self.L2 = -self.sim.getObjectPositionBlock(self.wheelHandle[0], self.robotHandle)[0]
                
        # Robo precisa estar no chão para pegar o raio
        self.R = self.sim.getObjectPositionBlock(self.wheelHandle[0], -1)[2]
        #self.R = 0.09749898314476013
                        
        # ROBO
        firstRobotPosOrn = self.getPosOrn()
        
        # Odometry
        self.odometryPosOrn = {}
        self.odometryPosOrn['raw']              = firstRobotPosOrn
        self.odometryPosOrn['encoder']          = firstRobotPosOrn
        self.odometryPosOrn['compass']          = firstRobotPosOrn
        
        self.odometryEncoder = {}
        self.odometryEncoder['encoder'] = [None] * 2
        self.odometryEncoder['encoder'][0] = self.sim.getJointPosition(self.motorHandle[0])
        self.odometryEncoder['encoder'][1] = self.sim.getJointPosition(self.motorHandle[1])
        self.odometryEncoder['compass'] = [None] * 2
        self.odometryEncoder['compass'][0] = self.sim.getJointPosition(self.motorHandle[0])
        self.odometryEncoder['compass'][1] = self.sim.getJointPosition(self.motorHandle[1])
        
        self.odometryTimeRaw = time.time()
        self.odometryTimeCompass = time.time()
        self.odometryTimeEncoderCompass = time.time()
 
        self.motorW = [None] * 2
        self.motorW[0] = 0
        self.motorW[1] = 0
        
        # Controllers
        self.wallFollowPID = PIDController(0.4, 1, 0, 2, windowSize=1000)
        self.obstacleAvoidFuzzy = OAFController()
        self.GoToGoalPID = PIDController(0.1, 4, 0.2, 0.4, windowSize=1000)
        self.AnglePID = PIDController(0.4, 1, 0, 2, windowSize=1000)   
        
        self.subsumptionStrategy = SubsumptionStrategy()
        self.subsumptionStrategy.add(AvoidObstaclesFuzzy(self))
        self.subsumptionStrategy.add(FollowLeftWallPID(self))
        self.subsumptionStrategy.add(FollowRightWallPID(self))
        

    
    ### -----------
    ### DRIVE  
    
    def move(self, left, right):
        self.computeOdometry()
        self.motorW[0] = left
        self.motorW[1] = right
        self.sim.setJointTargetVelocity(self.motorHandle[0], left)
        self.sim.setJointTargetVelocity(self.motorHandle[1], right)
        
    def stop(self):
        self.move(0, 0)
        
    def drive(self, vLinear, vAngular):
        self.move(self.vLToDrive(vLinear,vAngular), self.vRToDrive(vLinear,vAngular)) 
        
    def vRToDrive(self, vLinear, vAngular):
        return (((2*vLinear)+(self.L*vAngular))/2*self.R)
    
    def vLToDrive(self, vLinear, vAngular):
        return (((2*vLinear)-(self.L*vAngular))/2*self.R)
    
    ### -----------
    ### SONAR  

    def readSonars(self):
        pointCloud = []
        distances = []
        
        for i in range(0, len(self.sonarHandle)):
            state, distance = self.sim.readProximitySensor(self.sonarHandle[i])

            if (state == True):
                x = self.SONAR_POSITIONS[i][0] + (distance * np.cos(self.SONAR_ANGLES[i]))
                y = self.SONAR_POSITIONS[i][1] + (distance * np.sin(self.SONAR_ANGLES[i]))
                pointCloud.append((x, y))
                distances.append(distance)
            else:
                pointCloud.append((np.inf, np.inf))
                distances.append(np.inf)
 
        return distances, pointCloud
    
    
    ### -----------
    ### LASER

    def readLaser(self):
        pointCloud = self.sim.readLaserSensor("measuredDataAtThisTime")
        laser_array = np.reshape(pointCloud, (int(len(pointCloud)/3),3))

        return laser_array

    ### -----------
    ### IMAGE
    def getSensorViewImage(self):
        resolution, image_array = self.sim.getVisionSensorImage(self.visionHandle)
        image = np.array(image_array, dtype=np.uint8)#Create a numpy array with uint8 type
        image.resize([resolution[1], resolution[0],3])

        return image


    ### -----------
    ### ODOMETRY  
    
    def _computeMotorAngularDisplacement(self, i, name):
        posF = self.sim.getJointPosition(self.motorHandle[i])
        posI = self.odometryEncoder[name][i]
        self.odometryEncoder[name][i] = posF
        
        diff = posF - posI
        
        if (diff >= np.pi):
            diff = 2*np.pi - diff
        elif (diff <= -np.pi):
            diff = 2*np.pi + diff
        
        return diff
    
    def _computeOdometry(self, posOrn, thetaL, thetaR, deltaTheta):
        deltaS = self.R * (thetaR + thetaL) / 2
        
        if deltaTheta is None:
            deltaTheta = self.R * (thetaR - thetaL) / self.L
        
        angle = posOrn[2] + (deltaTheta/2)
        
        return posOrn + np.array(
                [deltaS * np.cos(angle) - deltaTheta* self.L2 * np.sin(angle), 
                 deltaS * np.sin(angle) + deltaTheta* self.L2 * np.cos(angle), 
                 deltaTheta])
        
    def computeOdometry(self):
        
        deltaTime = time.time() - self.odometryTimeRaw
        self.odometryTimeRaw = time.time() 
        
        thetaL = self.motorW[0] * deltaTime
        thetaR = self.motorW[1] * deltaTime
        
        self.odometryPosOrn['raw'] = self._computeOdometry(
                self.odometryPosOrn['raw'], thetaL, thetaR, None)
        
    def computeOdometryEncoder(self):
        thetaL = self._computeMotorAngularDisplacement(0, 'encoder')
        thetaR = self._computeMotorAngularDisplacement(1, 'encoder')

        self.odometryPosOrn['encoder'] = self._computeOdometry(
                self.odometryPosOrn['encoder'], thetaL, thetaR, None)
         
    def computeOdometryCompass(self):
        
        deltaTime = time.time() - self.odometryTimeCompass
        self.odometryTimeCompass = time.time() 
        
        thetaL = self._computeMotorAngularDisplacement(0, 'compass')
        thetaR = self._computeMotorAngularDisplacement(1, 'compass')
        deltaTheta = self.sim.getFloatSignal("gyroZ") * deltaTime
        
        self.odometryPosOrn['compass'] = self._computeOdometry(
                self.odometryPosOrn['compass'], thetaL, thetaR, deltaTheta)
        
    ### -----------
    ### GETTERS  
    
    def localToGlobalGT(self, position):
        return localToGlobal(self.getPosOrn(), position)
    
    def localToGlobalOdometry(self, position):
        return localToGlobal(self.getPosOrn(), position)
    
    def getPosOrn(self):
        x, y = self.sim.getObjectPosition(self.robotHandle, -1)[:2]
        orn = self.sim.getObjectOrientation(self.robotHandle, -1)[2]
        return x, y, orn
    
    def getPosOrnOdometyRaw(self):
        return self.odometryPosOrn['raw']
    
    def getPosOrnOdometyCompass(self):
        return self.odometryPosOrn['compass']
    
    def getPosOrnOdometyEncoder(self):
        return self.odometryPosOrn['encoder']
    
    def getPosOrnOdometyEncoderCompass(self):
        return self.odometryPosOrn['encoder_compass']
    
    def getLinearVelocity(self):
        return np.linalg.norm(self.sim.getObjectVelocity(self.robotHandle)[1][2])
    
    
    ### -----------------
    ### BEHAVIORS ACTIONS
    
    def avoidObstaclesFuzzy(self):
        
        distancesL = []
        
        for i in range(1, 4):
            state, distance = self.sim.readProximitySensor(self.sonarHandle[i])
            if (state == True):
                distancesL.append(np.abs(distance*np.cos(self.SONAR_ANGLES[i])))
            else:
                distancesL.append(1)
                
        distancesR = []
        
        for i in range(4, 7):
            state, distance = self.sim.readProximitySensor(self.sonarHandle[i])
            if (state == True):
                distancesR.append(np.abs(distance*np.cos(self.SONAR_ANGLES[i])))
            else:
                distancesR.append(1)
                
        vLinear, vAngular = self.obstacleAvoidFuzzy.compute(10*np.min(distancesL), 10*np.min(distancesR))
        self.drive(10*vLinear, 20*vAngular)
        
    def followWallPID(self, left):
        
        if (left):
            sonarId = 0
        else:
            sonarId = 7
            
        state, distance = self.sim.readProximitySensor(self.sonarHandle[sonarId])
        if (state == False):
            distance = 0.8
            
        value = self.wallFollowPID.compute(distance)
        
        value1 = 2 *( 1+value)
        value2 = 2 *( 1-value)
        
        if (left):
            self.move(value1, value2)
        else:
            self.move(value2, value1)
            
        return distance
    
    
    def GoToGoal(self,x_final,y_final):
        
        # Get actual position
        x, y, orn = self.getPosOrn()
        #print('x: ',x)
        #print('y: ',y)
        
        # Compute angle to objective
        orn_final = math.atan2((y_final-y),(x_final-x))
        
        # PID control to adjust velocity to final objective
        distance = math.sqrt((x_final-x)**2+(y_final-y)**2)
        value = self.GoToGoalPID.compute(distance)
        
        # PID control to adjust angular velocity to correct angle
        angle = orn - orn_final
        angle_correction = self.AnglePID.compute(angle)
        
        # Limit PID distance values
        if(value>+8):
            value = 8
        elif(value<-8):
            value = -8
        else:
            value = value
            
        # Limit PID angle values
        if(angle_correction>+2):
            angle_correction = 2
        elif(angle_correction<-2):
            angle_correction = -2
        else:
            angle_correction = angle_correction           
            
        #print('Distance: ',distance)
        print('PID: ',value)
        
        # Orientation control
        if(distance>0.05):
            if (orn > orn_final - 0.2 and orn < orn_final + 0.2):
                #self.move(-value,-value)
                if (orn < orn_final):
                    self.move(-angle_correction-value,+angle_correction-value)
                else:
                    self.move(+angle_correction-value,-angle_correction-value)                  
            else:
                if (orn < orn_final):
                    self.move(-angle_correction,+angle_correction)
                else:
                    self.move(+angle_correction,-angle_correction) 
        
        else:
            self.stop()                    
        
        # Delete or modify to plot something interesting
        sonarId = 0        
        state, distance = self.sim.readProximitySensor(self.sonarHandle[sonarId])
        return distance, value
    
    
    ### -----------
    ### BEHAVIORS  
    def stepSubsumptionStrategy(self):
        strategy = self.subsumptionStrategy.step()
        if (strategy is None):
            self.move(2, 2)
            
        return strategy
コード例 #3
0
class Robot:

    # Handles 
    robotHandle = None
    motorHandle = [None] * 2
    wheelHandle = [None] * 2
    sonarHandle = [None] * 16    
    
    # Constants
    SONAR_POSITIONS = [None] * 16   
    PI = np.pi
    SONAR_ANGLES = np.array([PI/2, 50/180.0*PI, 30/180.0*PI, 10/180.0*PI, 
                       -10/180.0*PI, -30/180.0*PI, -50/180.0*PI, -PI/2, -PI/2, 
                       -130/180.0*PI, -150/180.0*PI, -170/180.0*PI, 170/180.0*PI,
                       150/180.0*PI, 130/180.0*PI, PI/2]) 
    
    L = None
    L2 = None
    R = None
    
    # Map Ground truth
    gridMap = None
    
    def __init__(self, extendArea):

        self.sim = Simulator('127.0.0.1', 25001)
        self.sim.connect()        
        ### -----------
        ### GET HANDLES
        
        # ROBOT
        self.robotHandle = self.sim.getHandle("Pioneer_p3dx")
        
        # ACTUATORS
        self.motorHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftMotor')
        self.motorHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightMotor')
    
        # SONARS
        for x in range(0, len(self.sonarHandle)):
            self.sonarHandle[x] = self.sim.getHandle('Pioneer_p3dx_ultrasonicSensor'+str(x+1))
            
        # VISION
        #self.visionHandle = self.sim.getHandle('camera')
        self.visionHandle = 0
            
        # WHEEL
        self.wheelHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftWheel')
        self.wheelHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightWheel')

        ### -----------
        ### INIT STREAMS
        self.sim.initObjectPositionStream(self.robotHandle, -1)
        self.sim.initObjectOrientationStream(self.robotHandle, -1)
        for x in range(0, len(self.sonarHandle)):
            self.sim.initProximitySensor(self.sonarHandle[x])
        self.sim.initFloatSignal("gyroZ")
        #self.sim.initVisionSensorImage(self.visionHandle)
        self.sim.initObjectVelocity(self.robotHandle)
        self.sim.initJointPositionStream(self.motorHandle[0])
        self.sim.initJointPositionStream(self.motorHandle[1])
        self.sim.initLaserSensor("measuredDataAtThisTime")
        

        ### -----------
        ### POSTIONS    
        
        # SONAR (em relacao ao frame do robo)
        for x in range(0, len(self.sonarHandle)):
            self.SONAR_POSITIONS[x] = self.sim.getObjectPositionBlock(
                    self.sonarHandle[x], self.robotHandle) 
            
        self.L = np.linalg.norm(self.sim.getObjectPositionBlock(
                self.wheelHandle[0], self.wheelHandle[1]))
        
        
        self.L2 = -self.sim.getObjectPositionBlock(self.wheelHandle[0], self.robotHandle)[0]
                
        # Robo precisa estar no chão para pegar o raio
        self.R = self.sim.getObjectPositionBlock(self.wheelHandle[0], -1)[2]
        #self.R = 0.09749898314476013
                        
        # ROBO
        firstRobotPosOrn = self.getPosOrn()
        
        # Odometry
        self.odometryPosOrn = {}
        self.odometryPosOrn['raw']              = firstRobotPosOrn
        self.odometryPosOrn['encoder']          = firstRobotPosOrn
        self.odometryPosOrn['compass']          = firstRobotPosOrn
        
        self.odometryEncoder = {}
        self.odometryEncoder['encoder'] = [None] * 2
        self.odometryEncoder['encoder'][0] = self.sim.getJointPosition(self.motorHandle[0])
        self.odometryEncoder['encoder'][1] = self.sim.getJointPosition(self.motorHandle[1])
        self.odometryEncoder['compass'] = [None] * 2
        self.odometryEncoder['compass'][0] = self.sim.getJointPosition(self.motorHandle[0])
        self.odometryEncoder['compass'][1] = self.sim.getJointPosition(self.motorHandle[1])
        
        self.odometryTimeRaw = time.time()
        self.odometryTimeCompass = time.time()
        self.odometryTimeEncoderCompass = time.time()
 
        self.motorW = [None] * 2
        self.motorW[0] = 0
        self.motorW[1] = 0
        
        
        # Map grid
        x, y, _ = self.getPosOrn()
        mapImage = self.sim.getVisionSensorImageBlock(self.sim.getHandle('mapSensor'))
        self.gridMap = GridMap(mapImage, extendArea)
        x, y, _ = self.getPosOrn()
        x, y = self.gridMap.convertToMapUnit(x, y)
        self.gridMap.removeCluster(x, y)
        self.gridMap.show()
        
    
        # Controllers
        self.goToGoalPID = PIDController(0, 50, 0, 15, windowSize=1000)
        self.anglePID = PIDController(0, 10, 0, 10, windowSize=1000)   


    ### -----------
    ### DRIVE  
    
    def move(self, left, right):
        self.computeOdometry()
        self.motorW[0] = left
        self.motorW[1] = right
        self.sim.setJointTargetVelocity(self.motorHandle[0], left)
        self.sim.setJointTargetVelocity(self.motorHandle[1], right)
        
    def stop(self):
        self.move(0, 0)
        
    def drive(self, vLinear, vAngular):
        self.move(self.vLToDrive(vLinear,vAngular), self.vRToDrive(vLinear,vAngular)) 
        
    def vRToDrive(self, vLinear, vAngular):
        return (((2*vLinear)+(self.L*vAngular))/2*self.R)
    
    def vLToDrive(self, vLinear, vAngular):
        return (((2*vLinear)-(self.L*vAngular))/2*self.R)
    
    ### -----------
    ### SONAR  

    def readSonars(self):
        pointCloud = []
        distances = []
        
        for i in range(0, len(self.sonarHandle)):
            state, distance = self.sim.readProximitySensor(self.sonarHandle[i])

            if (state == True):
                x = self.SONAR_POSITIONS[i][0] + (distance * np.cos(self.SONAR_ANGLES[i]))
                y = self.SONAR_POSITIONS[i][1] + (distance * np.sin(self.SONAR_ANGLES[i]))
                pointCloud.append((x, y))
                distances.append(distance)
            else:
                pointCloud.append((np.inf, np.inf))
                distances.append(np.inf)
 
        return distances, pointCloud
    
    
    ### -----------
    ### LASER

    def readLaser(self):
        pointCloud = self.sim.readLaserSensor("measuredDataAtThisTime")
        laser_array = np.reshape(pointCloud, (int(len(pointCloud)/3),3))

        return laser_array

    ### -----------
    ### IMAGE
    def getSensorViewImage(self):
        resolution, image_array = self.sim.getVisionSensorImage(self.visionHandle)
        image = np.array(image_array, dtype=np.uint8)#Create a numpy array with uint8 type
        image.resize([resolution[1], resolution[0],3])

        return image


    ### -----------
    ### ODOMETRY  
    
    def _computeMotorAngularDisplacement(self, i, name):
        posF = self.sim.getJointPosition(self.motorHandle[i])
        posI = self.odometryEncoder[name][i]
        self.odometryEncoder[name][i] = posF
        
        diff = posF - posI
        
        if (diff >= np.pi):
            diff = 2*np.pi - diff
        elif (diff <= -np.pi):
            diff = 2*np.pi + diff
        
        return diff
    
    def _computeOdometry(self, posOrn, thetaL, thetaR, deltaTheta):
        deltaS = self.R * (thetaR + thetaL) / 2
        
        if deltaTheta is None:
            deltaTheta = self.R * (thetaR - thetaL) / self.L
        
        angle = posOrn[2] + (deltaTheta/2)
        
        return posOrn + np.array(
                [deltaS * np.cos(angle) - deltaTheta* self.L2 * np.sin(angle), 
                 deltaS * np.sin(angle) + deltaTheta* self.L2 * np.cos(angle), 
                 deltaTheta])
        
    def computeOdometry(self):
        
        deltaTime = time.time() - self.odometryTimeRaw
        self.odometryTimeRaw = time.time() 
        
        thetaL = self.motorW[0] * deltaTime
        thetaR = self.motorW[1] * deltaTime
        
        self.odometryPosOrn['raw'] = self._computeOdometry(
                self.odometryPosOrn['raw'], thetaL, thetaR, None)
        
    def computeOdometryEncoder(self):
        thetaL = self._computeMotorAngularDisplacement(0, 'encoder')
        thetaR = self._computeMotorAngularDisplacement(1, 'encoder')

        self.odometryPosOrn['encoder'] = self._computeOdometry(
                self.odometryPosOrn['encoder'], thetaL, thetaR, None)
         
    def computeOdometryCompass(self):
        
        deltaTime = time.time() - self.odometryTimeCompass
        self.odometryTimeCompass = time.time() 
        
        thetaL = self._computeMotorAngularDisplacement(0, 'compass')
        thetaR = self._computeMotorAngularDisplacement(1, 'compass')
        deltaTheta = self.sim.getFloatSignal("gyroZ") * deltaTime
        
        self.odometryPosOrn['compass'] = self._computeOdometry(
                self.odometryPosOrn['compass'], thetaL, thetaR, deltaTheta)
        
    ### -----------
    ### GETTERS  
    
    def localToGlobalGT(self, position):
        return localToGlobal(self.getPosOrn(), position)
    
    def localToGlobalOdometry(self, position):
        return localToGlobal(self.getPosOrn(), position)
    
    def getPosOrn(self):
        x, y = self.sim.getObjectPosition(self.robotHandle, -1)[:2]
        orn = self.sim.getObjectOrientation(self.robotHandle, -1)[2]
        return x, y, orn
    
    def getPosOrnOdometyRaw(self):
        return self.odometryPosOrn['raw']
    
    def getPosOrnOdometyCompass(self):
        return self.odometryPosOrn['compass']
    
    def getPosOrnOdometyEncoder(self):
        return self.odometryPosOrn['encoder']
    
    def getPosOrnOdometyEncoderCompass(self):
        return self.odometryPosOrn['encoder_compass']
    
    def getLinearVelocity(self):
        return np.linalg.norm(self.sim.getObjectVelocity(self.robotHandle)[1][2])
    
    ### -----------------
    ### Path Planning

    def potentialPlanningOffline(self, gx, gy):
        self.gridMap.calcPotentialMap(gx, gy)
        x, y, _ = self.getPosOrn()
        x, y = self.gridMap.convertToMapUnit(x, y)
        self.pathPlanning = self.gridMap.calcPotentialPath(x, y)
        return [self.gridMap.convertToReal(*pos) for pos in self.pathPlanning]
        
    def potentialAStarPlanningOffline(self, gx, gy):
        self.gridMap.calcPotentialMap(gx, gy)
        x, y, _ = self.getPosOrn()
        x, y = self.gridMap.convertToMapUnit(x, y)
        self.pathPlanning = self.gridMap.calcPotentialAStarPath(x, y, gx, gy) 
    
        return [self.gridMap.convertToReal(*pos) for pos in self.pathPlanning]

    def doPlanning(self):
        if (len(self.pathPlanning)):
            nextStep = self.pathPlanning[0]
            x, y = self.gridMap.convertToReal(*nextStep)
            distance = self.goToGoal(x, y)
            if (distance<0.2):
                self.pathPlanning = self.pathPlanning[1:]
            return True
        else:
            self.stop()
            return False
        
        
        
    
    ### -----------------
    ### BEHAVIORS ACTIONS
    def FollowPath(self,x_final,y_final):
        
        # Get actual position
        x, y, orn = self.getPosOrn()
        #print('x: ',x)
        #print('y: ',y)
        
        # Compute angle to objective
        orn_final = math.atan2((y_final-y),(x_final-x))
        
        # PID control to adjust velocity to final objective
        distance = math.sqrt((x_final-x)**2+(y_final-y)**2)
        value = self.GoToGoalPID.compute(distance)
        
        # PID control to adjust angular velocity to correct angle
        angle = orn - orn_final
        angle_correction = self.AnglePID.compute(angle)
        
        # Limit PID distance values
        if(value>+2):
            value = 2
        elif(value<-2):
            value = -2
        else:
            value = value
            
        # Limit PID angle values
        if(angle_correction>+2):
            angle_correction = 2
        elif(angle_correction<-2):
            angle_correction = -2
        else:
            angle_correction = angle_correction           
            
        # Orientation control
        if(distance>0.5): #0.05
            if (orn > orn_final - 0.5 and orn < orn_final + 0.5): #0.2
                #self.move(-value,-value)
                if (orn < orn_final):
                    self.move(-angle_correction-value,+angle_correction-value)
                else:
                    self.move(+angle_correction-value,-angle_correction-value)                  
            else:
                if (orn < orn_final):
                    self.move(-angle_correction,+angle_correction)
                    #print("-+")
                else:
                    self.move(+angle_correction,-angle_correction) 
                    #print("+-")
            Arrived = 'false'  
        else:
            #self.stop()
            Arrived = 'true'                    
        return Arrived        
    
    
    
    def goToGoal(self,x_final,y_final):
        
        # Get actual position
        x, y, orn = self.getPosOrn()
        
        # Compute angle to objective
        orn_final = math.atan2((y_final-y),(x_final-x))

        # PID control to adjust velocity to final objective
        distance = math.sqrt((x_final-x)**2+(y_final-y)**2)
        linearVelocity = -self.goToGoalPID.compute(distance)
        
        
        # PID control to adjust angular velocity to correct angle
        angle = orn - orn_final
        
        if angle > np.pi:
            angle = angle - 2 * np.pi
        elif angle < -np.pi:
            angle = angle + 2 * np.pi
            
        angleVelocity = self.anglePID.compute(angle)
        
        
                
        # Limit PID
        if (linearVelocity > 10):
            linearVelocity = 10
                                
        # Linear velocity should be smaller if it's not in correct direction
        linearVelocity = (np.cos(angle)**15) * linearVelocity
        
        if (linearVelocity < 0):
            linearVelocity = 0
        
        # Limit PID
        if (angleVelocity > 10):
            angleVelocity = 10
        elif (angleVelocity < -10):
            angleVelocity = -10      
            
        
        self.drive(5 * linearVelocity, 10 * angleVelocity)  
    
        return distance