def cleanUp(self):
     print "About to stop simulation connected to self.simulID: ", self.simulID
     vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot)
     vrep.simxSynchronousTrigger(self.simulID)                    
     vrep.simxFinish(self.simulID)
     vrep.simxFinish(-1)
     print "Disconnected from V-REP"

 def close(self, kill=False):
     if self.connected:
         remote_api.simxStopSimulation(self.api_id, remote_api.simx_opmode_oneshot_wait)
         remote_api.simxFinish(self.api_id)
     self.connected = False
     if kill and self.vrep_proc is not None:
         os.killpg(self.vrep_proc.pid, signal.SIGTERM)

    def restart(self,earlyStop = False):
        if (self.cid != None):
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
        vrep.simxFinish(-1)  # just in case, close all opened connections
        time.sleep(1)
        self.connect()
        time.sleep(1)

        vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
        if earlyStop:
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
            vrep.simxFinish(-1)  # just in case, close all opened connections
            return
        vrep.simxStartSimulation(self.cid, self.mode())
        self.runtime = 0
        err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
                                               vrep.simx_opmode_oneshot_wait)
        err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
                                               vrep.simx_opmode_oneshot_wait)

        err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)

        err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
        self.getTargetStart()
        for i in range(4):
            self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
                                                            'Quadricopter_propeller_respondable' + str(i) + str(1),
                                                            self.mode())

    def __init__(self):

        self.ip = '127.0.0.1'
        self.port = 19997
        self.scene = './simu.ttt'
        self.gain = 2
        self.initial_position = [3,3,to_rad(45)]

        self.r = 0.096 # wheel radius
        self.R = 0.267 # demi-distance entre les r

        print('New pioneer simulation started')
        vrep.simxFinish(-1)
        self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)

        if self.client_id!=-1:
            print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
            res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
            res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
            res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
            res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

            self.set_position(self.initial_position)
            vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)

        else:
            print('Unable to connect to %s:%s' % (self.ip, self.port))

 def close_connection(self):
     if self.clientID != -1:
         # BeforeQSize closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
         vrep.simxGetPingTime(self.clientID)
     
         # Now close the connection to V-REP:
         vrep.simxFinish(self.clientID)
         print ('Program ended')

    def __init__(self, ip='127.0.0.1', port=19997):
        # Close eventual old connections
        vrep.simxFinish(-1)

        self.clientID = -1
        self.opmode = vrep.simx_opmode_oneshot_wait
        # Connect to V-REP remote server
        self.connect(ip, port)

def vrepConnect(clientID, port):
	clientID=vrep.simxStart('127.0.0.1',port,True,True,1000,5)
	if clientID!=-1:
		print("Open Connection on port:"+str(port)+' with clientID: '+str(clientID))
	else: 
		print("Failed connecting through port:"+str(port))
		vrep.simxFinish(clientID)
	return clientID

 def disconnection(self):
     """
     Make disconnection with v-rep simulator
     """
     # stop the simulation:
     vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
     
     # Now close the connection to V-REP:	
     vrep.simxFinish(self.clientID)

	def __init__(self, **kwargs):
		vrep.simxFinish(-1)
		print "Start"
		print ('Connected to remote API server')
		self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
		self.opmode = vrep.simx_opmode_oneshot_wait
		self.model = vrep.simxGetObjectHandle(self.clientID, '2W1A', self.opmode)
		self.nGeneration = kwargs.get("nGeneration", 1)
		if self.clientID == -1:
			sys.exit('Failed connecting to remote API server')

    def _init_client_id(self):
        vrep.simxFinish(-1)

        self.client_id = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)

        if self.client_id != -1:
            print 'Connected to remote API server'

        else:
            print 'Connection not successful'
            sys.exit('Could not connect')

def start_simulator():
    print 'Program started'
    vrep.simxFinish(-1)
    clientID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000, 5)

    if clientID != -1:
        print 'Connected to remote API server'
    else:
        print 'Failed connecting to remote API server'
        sys.exit('Program Ended')

    return clientID

 def __init__(self, address, port):
     print ('Program started')
     vrep.simxFinish(-1) 
     self.clientID=vrep.simxStart(address,port,True,True,5000,5)
     if self.clientID!=-1:
         print "Conexion establecida, todo en orden"
     else:
         print "Conexion con el simulador fallida, reinicia el simulador"
     error,self.motorFL=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
     error,self.motorFR=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
     error,self.camera=vrep.simxGetObjectHandle(self.clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait)
     error,self.robot=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx',vrep.simx_opmode_oneshot_wait)

 def start(self):
     print ('Program started')
     vrep.simxFinish(-1) # just in case, close all opened connections
     self.clientID = vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
     if self.clientID != -1:
         print ('Connected to remote API server')
         self.LWMotor_hdl = vrep.simxGetObjectHandle(self.clientID,'LeftWheel_Motor', vrep.simx_opmode_oneshot_wait) # LeftWheel Motor handle
         self.RWMotor_hdl = vrep .simxGetObjectHandle(self.clientID,'RightWheel_Motor', vrep.simx_opmode_oneshot_wait) # RightWheel Motor handle
         self.Robot_hdl = vrep.simxGetObjectHandle(self.clientID, 'Cubot', vrep.simx_opmode_oneshot_wait)
         print ('Handle acquired !!')
     else:
         print ('Error : connection to vrep failed')

 def env_init(self):
     print ('VREP Environmental Program Started')
     vrep.simxFinish(-1) # just in case, close all opened connections
     self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
     if self.clientID!=-1:
         print ('Connected to remote API server')
         self.fmu = FMU()
         self.start_simulation() 
     else:
         print('Connection Failure')
         sys.exit('Abort Connection')
         
     return self.makeTaskSpec()

def run360():
    print "connecting to vrep with address", IPADDRESS, "on port", PORT

    clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
        
    print "hello vrep! ", clientID

    if clientID == -1:
        print "failed to connect to vrep server"
        return

    # get the motor joint handle
    error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
    
    print "starting the motor..."
    
    # set the velocity of the joint
    vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait);

    print "spinning 360 degrees..."
    
    # set up to stream joint positions
    vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming);
    
    passed90Degrees = False

    # The control loop:
    while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server..
        (error,position) = vrep.simxGetJointPosition(
            clientID,
            jointHandle,
            vrep.simx_opmode_buffer
            )

        # Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
        if error==vrep.simx_error_noerror : 
            # here we have the newest joint position in variable jointPosition! 
            # break when we've done a 360
            if passed90Degrees and position >= 0 and position < math.pi/2.0:
                break
            elif not passed90Degrees and position > math.pi/2.0:
                passed90Degrees = True

    print "stoppping..."
            
    vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait);

    if clientID != -1:
        vrep.simxFinish(clientID)

    print "done!"

 def VREPConnect(self):
     vrep.simxFinish(-1)
     "Connect to Simulation"
     self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
     eCode = vrep.simxSynchronous(self.simulID, True)
     if eCode != 0:
         print "Could not get V-REP to synchronize operation with me"
 
     if not self.simulID == -1:
         eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
         vrep.simxSynchronousTrigger(self.simulID)                    
         print "my SimulID is  ", self.simulID 
     else:
         sys.exit("Failed to connect to VREP simulation. Bailing out")

    def close(self):
        """
        Close the connection with the robot. Same as 'disconnect()'
        :return: 0 if all ok
        :rtype: int
        """
        if self.is_connected():
            # Stop the robot
            self.stop()

            # Close the socket
            vrep.simxFinish(self._clientID)
            return 0            
        else:
            return -1

def Initialize():
  # just in case, close all opened connections
  vrep.simxFinish(-1) 
  
  # connect to local host port 19999
  clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)

  if clientID!=-1:  #check if client connection successful
    print ('Connected to remote API server')

  else:
    print ('Connection not successful')
    sys.exit('Could not connect')

  return clientID

    def __init__(self, sim_dt=0.05, nengo_dt=0.001, sync=True):
        vrep.simxFinish(-1) # just in case, close all opened connections
        self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
        self.sync = sync

        if self.cid != -1:
            print ('Connected to V-REP remote API server, client id: %s' % self.cid)
            vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
            if self.sync:
              vrep.simxSynchronous( self.cid, True )
        else:
            print ('Failed connecting to V-REP remote API server')
            exit(1)
        self.count = 0
        self.sim_dt = sim_dt
        self.nengo_dt = nengo_dt

def finishSimulation(clientID):
    errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
    errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
    error=errorStop or errorClose
    errorFinish=vrep.simxFinish(clientID)
    error=error or errorFinish
    return error