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 readDummyPath(self):
     fp = open(self.dummyPath, 'r')
     lines = fp.readlines()  # 1行毎にファイル終端まで全て読む(改行文字も含まれる)
     fp.close()
     for line in lines:
         buf = line.split(",")
         for x in buf:
             params = x.split(":")
             if len(params) >= 2:
                 name = params[0]
                 try:
                     portNb = 19998 # int(params[1])
                     self.dummyID = vrep.simxStart("127.0.0.1", portNb, True, True, 2000, 5)
                     if self.dummyID == -1:
                         print >> sys.stderr, "Fatal: No client ID while creating Dummy Communicator."
                     else:
                         self.setClientID(self.dummyID)
                 except ValueError:
                     print >> sys.stderr, "Fatal: non integer value while creating Dummy Communicator."
                     time.sleep(1)
                     exit()
             else:
                 name = params[0]
                 returnCode, handle = vrep.simxGetObjectHandle(self.dummyID, name, vrep.simx_opmode_oneshot_wait)
                 if returnCode != vrep.simx_return_ok:
                     print >> sys.stderr, "Fatal: Error obtaining a handle for " + name + "!"
                 else:
                     print name, handle
                     item = VRepItem(name, self.dummyID, handle)
                     self.addItem(item)

 def connectVREP(self, ipAddr=LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep IP"), port=int(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep port"))):
     self.getObjectPositionFirstTime = True
     #vrep.simxFinish(-1) # just in case, close all opened connections #TODO this could be a problem when the connection pool is implemented
     time1 = time()
     clientID = vrep.simxStart(ipAddr,port,True,True,5000,5)
     time2 = time()
     print "LOGGING time for connecting VREP in senconds: ", time2 - time1
     return clientID

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 __init__(self, port):
     super(VREP, self).__init__()
     # Add simExtRemoteApiStart(<port>) to an init script of VREP
     # subscribe our client
     self.id = vrep.simxStart('127.0.0.1', port, True, False, 5000, 5)
     self._handle = {}
     self._p = []
     self._v = []
     self._s = []

	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 connect(self):
     global SYNC
     self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)  # Connect to V-REP
     if self.cid != -1:
         print ('Connected to V-REP remote API serv'
                '\er, client id: %s' % self.cid)
         vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot)
         if SYNC:
             vrep.simxSynchronous(self.cid, True)
     else:
         print ('Failed connecting to V-REP remote API server')
         exit()

 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_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 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 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 connectToVREP(self, VREP_World=None):
     "Connect to VREP and load the correct world if needed"
     "FIXME: SHOULD LAUNCH VREP IF NOT RUNNING"
     VREP_exec = 'vrep'
     if self.VREPSimulation == None:
         self.VREPSimulation = VREP_World
     
     '1. check that V-Rep is running and see whether we are already connected to it. Otherwise connect'
     if VREP_exec not in check_output(["ps","-f", "--no-headers",  "ww", "-C", "vrep"]):
         raise Exception(("V-REP is not running! Please start V-REP with scene: %s" % self.VREPSimulation))
     else:
         "Check if we are connected with the passed clientId already"
         if self._VREP_clientId is not None:
             print "ClientId = " ,self._VREP_clientId
             connId = vrep.simxGetConnectionId(self._VREP_clientId)
             print "My connId is " , connId
             if connId == -1:                                 # we are not: set client Id to none and re-connect
                 print "Disconnecting all existing connections to V-REP"
                 vrep.simxFinish(-1)
                 self._VREP_clientId = None            
         while self._VREP_clientId is None:
             self._VREP_clientId = vrep.simxStart(self._host, self._kheperaPort,True,True, 5000,5)
             if not self._VREP_clientId == -1:
                 eCode = vrep.simxSynchronous(self._VREP_clientId, True)
                 if eCode != 0:
                     raise Exception("Failed to connect to VREP simulation. Bailing out")
 #     print " we are connected with clientId ", self._VREP_clientId
     "2. Check the correct world is running"
     if self.VREPSimulation is not None: 
         VREP_Scene = split(self.VREPSimulation)[1]
         if VREP_Scene not in check_output(["ps","-f", "--no-headers",  "ww", "-C", "vrep"]):
             eCode = vrep.simxLoadScene(self._VREP_clientId, self.VREPSimulation, 0, vrep.simx_opmode_oneshot_wait)
             if eCode != 0:
                 raise Exception(("Could not load into V-REP the world",  self.VREPSimulation))     
 
 
     "3. Make sure VREP has bees set to the correct directory for saving data"
     self.setDataDir(self.dataDir)
             
     '4. Start simulation'
     eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
     if eCode != 0:
         raise Exception("VREP simulation cannot get started")
     else:
         print "V-REP simulation is running with clientId: ", self._VREP_clientId
         return self._VREP_clientId 

    def __init__(self, ip, port):
        """Initializes connectivity to V-REP environment, initializes environmental variables.
        """
        vrep.simxFinish(-1)
        self.clientID = vrep.simxStart(ip, port, True, True, 5000, 5)
        if self.clientID != -1:
            print('Successfully connected to V-REP')
        else:
            raise RuntimeError('Could not connect to V-REP')

        # Some artificial delays to let V-REP stabilize after an action
        self.sleep_sec = config.SLEEP_VAL
        self.sleep_sec_min = config.SLEEP_VAL_MIN

        # id of the Robot Arm in V-REP environment
        self.main_object = 'uarm'
        # Initial state of the Gripper
        self.gripper_enabled = False

        # Get hands to various objects in the scene
        err, self.cylinder_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_pickupCylinder2',
                                                             vrep.simx_opmode_blocking)
        if err != vrep.simx_return_ok:
            raise RuntimeError("Could not get handle to the cylinder object. Halting program.")

        err, self.bin_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_bin',
                                                        vrep.simx_opmode_blocking)
        if err != vrep.simx_return_ok:
            raise RuntimeError("Could not get handle to the Bin object. Halting program.")

        err, self.gripper_handle = vrep.simxGetObjectHandle(self.clientID, 'uarmGripper_motor1Method2',
                                                            vrep.simx_opmode_blocking)
        if err != vrep.simx_return_ok:
            raise RuntimeError("Could not get handle to the Gripper object. Halting program.")

        # Distance between cylinder and bin when former is inside later
        # This was measured after putting the cylinder in the bin
        self.cylinder_bin_distance = config.CYLINDER_BIN_DISTANCE

        # Various values, to be set in the start_sim() function, because their values can be obtained
        # only after the simulation starts
        self.cylinder_height = None
        self.cylinder_z_locus = None
        self.bin_position = None
        self.objects = None
        self.object_positions = None

    def __init__(self, n_robots, base_name):
        self.name = "line follower tester"
        self.n_robots = n_robots
        self.base_name = base_name

        self.robot_names = [self.base_name]
        for i in range(self.n_robots-1):
            self.robot_names.append(self.base_name+'#'+str(i))

        # Establish connection to V-REP
        vrep.simxFinish(-1)  # just in case, close all opened connections

        # Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
        # http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
        self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)

        # Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
        # (requires the simulation to be running)

        if self.clientID != -1:
            print ('Connected to remote API server')
        else:
            print ('Failed connecting to remote API server')
            sys.exit('Could not connect')

        # Reset running simulation
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
        time.sleep(0.2)

        # Get initial robots' positions
        self.robot_handles = []
        self.robot_pos0 = []
        for rbt_name in self.robot_names:
            res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
            self.robot_handles.append(rbt_tmp)
            # Initialize data stream
            vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)

        time.sleep(0.2)
        for rbt in self.robot_handles:
            res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
            self.robot_pos0.append(pos)

 def initialize_vrep_client(self):
     #Initialisation for Python to connect to VREP
     print 'Python program started'
     count = 0
     num_tries = 10
     while count < num_tries:
         vrep.simxFinish(-1) # just in case, close all opened connections
         self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
         if self.clientID!=-1:
             print 'Connected to V-REP'
             break
         else:
             "Trying again in a few moments..."
             time.sleep(3)
             count += 1
     if count >= num_tries:
         print 'Failed connecting to V-REP'
         vrep.simxFinish(self.clientID)