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))
Esempio n. 2
0
 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)
Esempio n. 3
0
 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
Esempio n. 5
0
 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 = []
Esempio n. 6
0
	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')
Esempio n. 7
0
    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')
Esempio n. 8
0
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
Esempio n. 9
0
 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()
Esempio n. 10
0
 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)
Esempio n. 11
0
 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')
Esempio n. 12
0
 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()
Esempio n. 13
0
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!"
Esempio n. 14
0
 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")
Esempio n. 15
0
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
Esempio n. 16
0
    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
Esempio n. 17
0
 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 
Esempio n. 18
0
    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
Esempio n. 19
0
    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)
Esempio n. 20
0
def reset_vrep():
    print 'Start to connect vrep'
    # Close eventual old connections
    vrep.simxFinish(-1)
    # Connect to V-REP remote server
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
    opmode = vrep.simx_opmode_oneshot_wait
    # Try to retrieve motors and robot handlers
    # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
    ret_l, LMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", opmode)
    ret_r, RMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", opmode)
    ret_a, RobotHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx", opmode)
    vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
    vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
    time.sleep(1)    
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
    vrep.simxFinish(clientID)
    print 'Connection to vrep reset-ed!'
Esempio n. 21
0
    def connect(self):
        #os.chdir(VREP_HOME)
        #subprocess.call([os.path.join(VREP_HOME,'vrep.sh'), VREP_scene_file], shell = True, cwd = VREP_HOME)
        "Close existing connections"
        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")
Esempio n. 22
0
 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)
Esempio n. 23
0
 def connection(self):
     """
     Make connection with v-rep simulator
     """
     print ('Waiting for connection...')
     vrep.simxFinish(-1) # just in case, close all opened connections
     self.clientID=vrep.simxStart('127.0.0.1',self.port,True,True,5000,5) # Connect to V-REP
     if self.clientID!=-1:
         print ('Connected to remote API server')
         
         # enable the synchronous mode on the client:
         if self.synchronous:
             vrep.simxSynchronous(self.clientID,True)
         
         # start the simulation:
         vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
         
     else:
         raise IOError('Failed connecting to remote API server')
def vrepSim(clientID, action):
	vrep.simxFinish(-1)
	localhost = "127.0.0.1"
	clientID=vrep.simxStart(localhost,19997,True,True,1000,5)
	if clientID!=-1:
		print('Connected to remote API server')
		if action=="start":
			err = vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
			if (not err):
				print('Sim Started')
		elif action=="stop":
			err = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
			if (not err):
				print('Sim Stopped')		
		print("Disconnected from remote API server")
	else:
		print('Failed connecting to remote API server')
	vrep.simxFinish(clientID)
	return clientID
    def __init__(self):
        vrep.simxFinish(-1)  # just in case, close all opened connections
        time.sleep(0.5)
        self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # tenta conectar no simulador, se salva o clientID

        # paramos a simulacao, se estiver em andamento, e comecamos denovo
       # vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
        #time.sleep(0.5)
        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)

        # modo da API, como eh False, esta no modo assincrono(os ticks da simulacao rodam em velocidade independente)
        vrep.simxSynchronous(self.clientID, False)
        print("connected with id ", self.clientID)

        self.oldtarget = None
        self.camera = None

        self.setup()
        self.lastimageAcquisitionTime = 0
	def __init__(self,port_number):

		vrep.simxFinish(-1)
		self.clientID = vrep.simxStart('127.0.0.1', port_number, True, True, 5000, 5)

		# start simulation
		rc = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
		
		# object handles
		res, self.quad_obj = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait)
		res, self.camera   = vrep.simxGetObjectHandle(self.clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)
		res, self.target   = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait)
		
		# Initialise data streaming from V-REP
		err, resolution, image  = vrep.simxGetVisionSensorImage(self.clientID, self.camera, 0, vrep.simx_opmode_streaming)
		_,pos                   = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_streaming)
		_,target_pos            = vrep.simxGetObjectPosition(self.clientID, self.target, -1, vrep.simx_opmode_streaming)
		
		time.sleep(2)

		# Variables
		_,self.last_pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_buffer)
 def initializeVrepClient(self):
     """ Initialization for Python to connect to VREP.
     We obtain a clientID after connecting to VREP, which is saved to
         `self.clientID`
     This method is only called if we controlling one robot with the remote API
     """
     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(self.ip,self.port,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)
Esempio n. 28
0
    def connect(self):
        """
        Connect to the V-REP simulator
        
        :return: If the connection was successful
        :rtype: boolean
        """

        if self.is_connected():
            self._debug('Already connected')
            return False
        
        self._clientID = vrep.simxStart(self._hostname, self._port, True, True, 5000, 5)
        
        if self._clientID == -1:
            self._debug("Failed to connect to remote V-REP server")
            return False

        self._debug("Connected to V-REP server")

        self.reset()
        return True
Esempio n. 29
0
    def __init__(self, cfg, verbose=False, calcheck=True, setup=True):
        self.cfg      = cfg
        self.verbose  = verbose
        self.calcheck = calcheck
        self.setup    = setup

        self.connected       = False
        self.scene_loaded    = False
        self.tracked_objects = []
        self.tracked_handles = []
        self.handles         = {}
        self.info = {}

        if not calcheck:
            assert not cfg.execute.prefilter, 'Can\'t skip the calibration check and prefilter collisions. Choose.'

        self.vrep_proc = None
        self.mac_folder = os.path.expanduser(cfg.execute.simu.mac_folder)
        self.launch_sim()
        self.objects_pos = {}

        remote_api.simxFinish(-1)
        trycount = 0
        while trycount < 5:
            self.api_id = remote_api.simxStart('127.0.0.1', self.port, True, False, 5000, 5)
            if self.api_id != -1:
                break
            trycount += 1
        assert self.api_id != -1
        time.sleep(1)

        self.scene = None

        if setup and cfg.execute.simu.load:
            if self.cfg.execute.is_simulation:
                self.setup_scene('robot')
            else:
                self.setup_scene('solomarker')
    def init_vrep_connection(self):
        vrep.simxFinish(-1) # just in case, close all opened connections
        self.client_id=vrep.simxStart(self.ip,self.port,True,True,5000,5) # Connect to V-REP

        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)

            #get motor handler
            self.motor_handlers = []
            for motor in self.robot.motors:
                res, motor_handler = vrep.simxGetObjectHandle(
                    self.client_id,
                    motor.name,
                    vrep.simx_opmode_oneshot_wait
                )
                self.motor_handlers.append(motor_handler)
            self.motor_handlers.sort()
            print(self.motor_handlers)

            #get effector handler
            res, self.effector_handler = vrep.simxGetObjectHandle(
                self.client_id,
                'effector',
                vrep.simx_opmode_oneshot_wait
            )

            #get collision handler
            self.collision_handlers = []
            for collision in self.collision_list:
                res, collision_handler = vrep.simxGetObjectHandle(
                    self.client_id,
                    collision,
                    vrep.simx_opmode_oneshot_wait
                )
                self.collision_handlers.append(collision_handler)

            vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
Esempio n. 31
0
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter):
    try:
        import vrep
    except:
        print('--------------------------------------------------------------')
        print('"vrep.py" could not be imported. This means very probably that')
        print('either "vrep.py" or the remoteApi library could not be found.')
        print('Make sure both are in the same folder as this file,')
        print('or appropriately adjust the file "vrep.py"')
        print('--------------------------------------------------------------')
        print('')

    import time
    import numpy as np
    from captains_log_v1 import log_data
    import os

    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D

    # output limits:
    #min_output=0
    #max_output=8.335
    # program parameters:
    global i
    i = 0
    xe = []
    ye = []
    ze = []
    xs = []
    ys = []
    zs = []
    x_qc = []
    y_qc = []
    z_qc = []
    u = []
    v1 = []
    v2 = []
    v3 = []
    v4 = []
    #global variables:
    cumul = 0
    last_e = 0
    pAlphaE = 0
    pBetaE = 0
    psp2 = 0
    psp1 = 0
    prevEuler = 0

    cumulAlpha = 0
    cumulBeta = 0

    cumulAlphaPos = 0
    cumulBetaPos = 0

    particlesTargetVelocities = [0, 0, 0, 0]
    #speed weight:
    vParam = -2
    #parameters for vertical control
    Kpv = 2
    Kiv = 0
    Kdv = 2
    #parameters for horizontal control:
    Kph = 0.4
    Kih = 0.1
    Kdh = 1.5
    Kph_pos1 = 0.4
    Kih_pos1 = 0.001
    Kdh_pos1 = 0.05
    Kph_pos0 = 0.4
    Kih_pos0 = 0.001
    Kdh_pos0 = 0.05
    #parameters for rotational control:
    Kpr = 0.05
    Kir = 0
    Kdr = 0.9

    print('Program started')
    vrep.simxFinish(-1)  # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
                              5)  # Connect to V-REP

    if clientID != -1:
        print('Connected to remote API server')

        # enable the synchronous mode on the client:
        vrep.simxSynchronous(clientID, True)

        # start the simulation:
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

        #functional/handle code:
        emptyBuff = bytearray()
        [returnCode,
         targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target,
                                               vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle] = vrep.simxGetObjectHandle(clientID,
                                                    Quadricopter_base,
                                                    vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter,
                                               vrep.simx_opmode_blocking)

        # main loop:
        while True:
            # vertical control:
            [returnCode,
             targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
                                                     vrep.simx_opmode_blocking)
            [returnCode,
             pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)
            [returnCode, l,
             w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
                                             vrep.simx_opmode_blocking)

            e = targetPos[2] - pos[2]
            cumul = cumul + e
            diff_e = e - last_e
            Pvert = Kpv * e
            Ivert = Kiv * cumul
            Dvert = Kdv * diff_e
            thrust = 5.335 + Pvert + Ivert + Dvert + l[2] * vParam  # get thrust
            last_e = e

            ## plot height:

            # horizontal control:
            [returnCode,
             sp] = vrep.simxGetObjectPosition(clientID, targetObj,
                                              qc_base_handle,
                                              vrep.simx_opmode_blocking)
            [rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
                clientID, Quadricopter, vrep.sim_scripttype_childscript,
                'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
                clientID, Quadricopter, vrep.sim_scripttype_childscript,
                'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, rc, rc,
             rc] = vrep.simxCallScriptFunction(clientID, Quadricopter,
                                               vrep.sim_scripttype_childscript,
                                               'qc_Get_Object_Matrix', [], [],
                                               [], emptyBuff,
                                               vrep.simx_opmode_blocking)
            [errorCode,
             M] = vrep.simxGetStringSignal(clientID, 'mtable',
                                           vrep.simx_opmode_oneshot_wait)
            if (errorCode == vrep.simx_return_ok):
                m = vrep.simxUnpackFloats(M)

            alphaE = vy[2] - m[11]
            cumulAlpha = cumulAlpha + alphaE
            diff_alphaE = alphaE - pAlphaE
            alphaCorr = Kph * alphaE + Kih * cumulAlpha + Kdh * diff_alphaE  #alpha correction

            betaE = vx[2] - m[11]
            cumulBeta = cumulBeta + betaE
            diff_betaE = betaE - pBetaE
            betaCorr = -Kph * betaE - Kih * cumulBeta - Kdh * diff_betaE  #beta correction

            pAlphaE = alphaE
            pBetaE = betaE

            cumulAlphaPos = cumulAlphaPos + sp[1]
            cumulBetaPos = cumulBetaPos + sp[0]
            alphaPos = Kph_pos1 * (
                sp[1]) + Kih_pos1 * cumulAlphaPos + Kdh_pos1 * (
                    sp[1] - psp2)  #alpha position correction
            betaPos = Kph_pos0 * (
                sp[0]) + Kih_pos0 * cumulBetaPos + Kdh_pos0 * (
                    sp[0] - psp1)  #beta position correction

            alphaCorr = alphaCorr + alphaPos
            betaCorr = betaCorr - betaPos

            psp2 = sp[1]
            psp1 = sp[0]

            # plot vx, vy, x, y:

            # rotational control:
            [returnCode,
             euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
                                                    qc_base_handle,
                                                    vrep.simx_opmode_blocking)
            [returnCode, orientation
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)

            Prot = Kpr * euler[2]
            Drot = Kdr * (euler[2] - prevEuler)
            rotCorr = Prot + Drot
            prevEuler = euler[2]

            ## plot rot:

            # set propeller velocities:
            propeller1_PTV = thrust * (1 - alphaCorr + betaCorr - rotCorr)
            propeller2_PTV = thrust * (1 - alphaCorr - betaCorr + rotCorr)
            propeller3_PTV = thrust * (1 + alphaCorr - betaCorr - rotCorr)
            propeller4_PTV = thrust * (1 + alphaCorr + betaCorr + rotCorr)
            particlesTargetVelocities = [
                propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV
            ]

            C_parameters_vert = [0, 0, 0]
            C_parameters_horr = [0, 0, 0, 0, 0, 0, 0]
            C_parameters_rot = [0, 0, 0]
            vert_comp = [0, 0, 0, 0]
            horr_comp = [0, 0, 0, 0, [0, 0, 0], 0, 0, 0, 0, 0, 0]
            rot_comp = [[0, 0, 0], 0]

            C_parameters_vert[0] = Kpv
            C_parameters_vert[1] = Kiv
            C_parameters_vert[2] = Kdv

            C_parameters_horr[0] = Kph
            C_parameters_horr[1] = Kih
            C_parameters_horr[2] = Kdh
            C_parameters_horr[3] = Kph_pos0
            C_parameters_horr[4] = Kdh_pos0
            C_parameters_horr[5] = Kph_pos1
            C_parameters_horr[6] = Kdh_pos1

            C_parameters_rot[0] = Kpr
            C_parameters_rot[1] = Kir
            C_parameters_rot[2] = Kdr

            vert_comp[0] = targetPos
            vert_comp[1] = pos
            vert_comp[2] = e
            vert_comp[3] = thrust

            horr_comp[0] = alphaE
            horr_comp[1] = betaE
            horr_comp[2] = cumulAlpha
            horr_comp[3] = cumulBeta
            horr_comp[4] = sp
            horr_comp[5] = cumulAlphaPos
            horr_comp[6] = cumulBetaPos
            horr_comp[7] = alphaCorr
            horr_comp[8] = betaCorr
            horr_comp[9] = vx
            horr_comp[10] = vy

            rot_comp[0] = euler
            rot_comp[1] = rotCorr

            ## PLOTTING:
            ze.append(e)  # otstapuvanje od z-oska
            xe.append(sp[0])  # otstapuvanje od x-oska
            ye.append(sp[1])  # otstapuvawe od y-oska
            xs.append(targetPos[0])
            ys.append(targetPos[1])
            zs.append(targetPos[2])
            x_qc.append(pos[0])
            y_qc.append(pos[1])
            z_qc.append(pos[2])
            fig = plt.figure()
            ax = fig.add_subplot(111, projection='3d')
            #            ax.xaxis.label.set_color('blue')
            #            ax.yaxis.label.set_color('blue')
            #            ax.zaxis.label.set_color('blue')
            #            ax.tick_params(axis='xs',colors='blue')
            #            ax.tick_params(axis='ys',colors='blue')
            #            ax.tick_params(axis='zs',colors='blue')
            ax.plot_wireframe(xs, ys, zs, color='blue')
            #            plt.hold(True)
            #            ax.xaxis.label.set_color('red')
            #            ax.yaxis.label.set_color('red')
            #            ax.zaxis.label.set_color('red')
            #            ax.tick_params(axis='x_qc',colors='red')
            #            ax.tick_params(axis='y_qc',colors='red')
            #            ax.tick_params(axis='z_qc',colors='red')
            ax.plot_wireframe(x_qc, y_qc, z_qc, color='red')
            plt.show()

            u.append(thrust)
            v1.append(propeller1_PTV)
            v2.append(propeller2_PTV)
            v3.append(propeller3_PTV)
            v4.append(propeller4_PTV)

            plt.plot(v1, color='blue')
            plt.hold(True)
            plt.plot(v2, color='red')
            plt.hold(True)
            plt.plot(v3, color='green')
            plt.hold(True)
            plt.plot(v4, color='pink')
            plt.hold(True)
            plt.axis([0, 25, 0, 15])
            plt.show()

            plt.plot(xe, color='blue')
            plt.axis([0, 100, -4, 4])
            plt.show()
            plt.plot(ye, color='red')
            plt.axis([0, 100, -4, 4])
            plt.show()
            plt.plot(ze, color='green')
            plt.axis([0, 100, -4, 4])
            plt.show()

            #        print '////////////'
            #        print '------------'
            #        print '-Controller parameters-'
            #        print 'Vertical control parameters=',[Kpv,Kiv,Kdv]
            #        print 'Horizontal control parameters=',[Kph,Kih,Kdh,Kph_pos0,Kdh_pos0,Kph_pos1,Kdh_pos1]
            #        print 'Rotational control parameters=',[Kpr,Kir,Kdr]
            #        print '------------'
            #        print '-Vertical component-'
            #        print 'targetPos=',targetPos
            #        print 'pos=',pos
            #        print 'e=',e
            #        print 'thrust=',thrust
            #        print '------------'
            #        print '-Horizontal component-'
            #        print 'cumulAlpha=',cumulAlpha
            #        print 'cumulBeta=',cumulBeta
            #        print 'cumulAlphaPos=',cumulAlphaPos
            #        print 'cumulBetaPos=',cumulBetaPos
            #        print 'alphaE=',alphaE
            #        print 'betaE=',betaE
            #        print 'alphaCorr=',alphaCorr
            #        print 'betaCorr=',betaCorr
            #        print 'orientation=',orientation
            #        print 'sp_X=',sp[0]
            #        print 'sp_Y=',sp[1]
            #        print '------------'
            #        print '-Rotational component-'
            #        print 'vx=',vx
            #        print 'vy=',vy
            #        print 'gamma=',euler[2]
            #        print 'rotCorr=',rotCorr
            #        print '------------'
            #        print 'Velocity_propeller_1 = ',particlesTargetVelocities[0]
            #        print 'Velocity_propeller_2 = ',particlesTargetVelocities[1]
            #        print 'Velocity_propeller_3 = ',particlesTargetVelocities[2]
            #        print 'Velocity_propeller_4 = ',particlesTargetVelocities[3]
            #        print '------------'
            #        print '////////////'

            ## WRITE TO TEXT:
            log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot,
                     vert_comp, horr_comp, rot_comp, particlesTargetVelocities,
                     i)
            i += 1
            # send propeller velocities to output:
            [res, retInts, retFloats, retStrings,
             retBuffer] = vrep.simxCallScriptFunction(
                 clientID, Quadricopter, vrep.sim_scripttype_childscript,
                 'qc_propeller_v', [], particlesTargetVelocities, [],
                 emptyBuff, vrep.simx_opmode_blocking)
            vrep.simxSynchronousTrigger(clientID)

        # stop the simulation:
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

        # Now close the connection to V-REP:
        vrep.simxFinish(clientID)

    else:
        print('Failed connecting to remote API server')
Esempio n. 32
0
def main():
    # Start V-REP connection
    try:
        vrep.simxFinish(-1)
        print("Connecting to simulator...")
        clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
        if clientID == -1:
            print("Failed to connect to remote API Server")
            vrep_exit(clientID)
    except KeyboardInterrupt:
        vrep_exit(clientID)
        return

    # Setup V-REP simulation
    print("Setting simulator to async mode...")
    vrep.simxSynchronous(clientID, True)
    dt = .001
    vrep.simxSetFloatingParameter(clientID,
                                  vrep.sim_floatparam_simulation_time_step,
                                  dt,  # specify a simulation time step
                                  vrep.simx_opmode_oneshot)

    # Load V-REP scene
    print("Loading scene...")
    vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)

    # Get quadrotor handle
    err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
    print(err,quad_handle)

    # Initialize quadrotor position and orientation
    vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
    vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
    vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)

    # Start simulation
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

    # Initialize rotors
    print("Initializing propellers...")
    for i in range(len(propellers)):
        vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)

    # Get quadrotor initial position and orientation
    err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
    err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
    err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
            quad_handle, vrep.simx_opmode_buffer)

    pos_start = np.asarray(pos_start)
    euler_start = np.asarray(euler_start)*10.
    vel_start = np.asarray(vel_start)
    angvel_start = np.asarray(angvel_start)

    pos_old = pos_start
    euler_old = euler_start
    vel_old = vel_start
    angvel_old = angvel_start
    pos_new = pos_old
    euler_new = euler_old
    vel_new = vel_old
    angvel_new = angvel_old

    pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
    euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
    vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
    angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new

    pos_error_all = pos_error
    euler_error_all = euler_error

    n_input = 6
    n_forces=4
    init_f=7.

    state = [0 for i in range(n_input)]
    state = torch.from_numpy(np.asarray(state, dtype=np.float32)).view(-1, n_input)

    propeller_vels = [init_f, init_f, init_f, init_f]
    delta_forces = [0., 0., 0., 0.]

    extended_state=torch.cat((state,torch.from_numpy(np.asarray([propeller_vels], dtype=np.float32))),1)
    memory = ReplayMemory(ROLLOUT_LEN)
    test_num = 1
    timestep = 1
    while (vrep.simxGetConnectionId(clientID) != -1):

        # Set propeller thrusts
        print("Setting propeller thrusts...")
        # Only PD control bc can't find api function for getting simulation time
        propeller_vels = pid(pos_error,euler_new,euler_error[2],vel_error,angvel_error)
        #propeller_vels = apply_forces(propeller_vels, delta_forces) # for dqn

        # Send propeller thrusts
        print("Sending propeller thrusts...")
        [vrep.simxSetFloatSignal(clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in
         zip(propellers, propeller_vels)]

        # Trigger simulator step
        print("Stepping simulator...")
        vrep.simxSynchronousTrigger(clientID)

        # Get quadrotor initial position and orientation
        err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
        err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
        err, vel_new, angvel_new = vrep.simxGetObjectVelocity(clientID,
                quad_handle, vrep.simx_opmode_buffer)

        pos_new = np.asarray(pos_new)
        euler_new = np.asarray(euler_new)*10
        vel_new = np.asarray(vel_new)
        angvel_new = np.asarray(angvel_new)
        #euler_new[2]/=100

        valid = is_valid_state(pos_start, pos_new, euler_new)

        if valid:
            next_state = torch.FloatTensor(np.concatenate([euler_new, pos_new - pos_old]))
            #next_state = torch.FloatTensor(euler_new )

            next_extended_state=torch.FloatTensor([np.concatenate([next_state,np.asarray(propeller_vels)])])
        else:
            next_state = None
            next_extended_state = None

        reward=np.float32(0)

        memory.push(extended_state, torch.from_numpy(np.asarray([delta_forces],dtype=np.float32)), next_extended_state,
                                torch.from_numpy(np.asarray([[reward]])))
        state = next_state
        extended_state=next_extended_state
        pos_old = pos_new
        euler_old = euler_new
        vel_old = vel_new
        angvel_old = angvel_new
        print("Propeller Velocities:")
        print(propeller_vels)
        print("\n")

        pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
        euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
        euler_error %= 2*np.pi
        for i in range(len(euler_error)):
            if euler_error[i] > np.pi:
                euler_error[i] -= 2*np.pi
        vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
        angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new

        pos_error_all = np.vstack([pos_error_all,pos_error])
        euler_error_all = np.vstack([euler_error_all,euler_error])

        print("Errors (pos,ang):")
        print(pos_error,euler_error)
        print("\n")

        timestep += 1
        if not valid or timestep > ROLLOUT_LEN:
            np.savetxt('csv/pos_error{0}.csv'.format(test_num),
                       pos_error_all,
                       delimiter=',',
                       fmt='%8.4f')
            np.savetxt('csv/euler_error{0}.csv'.format(test_num),
                       euler_error_all,
                       delimiter=',',
                       fmt='%8.4f')

            print('RESET')
            # reset
            vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
            time.sleep(0.1)

            # Setup V-REP simulation
            print("Setting simulator to async mode...")
            vrep.simxSynchronous(clientID, True)
            dt = .001
            vrep.simxSetFloatingParameter(clientID,
                                          vrep.sim_floatparam_simulation_time_step,
                                          dt,  # specify a simulation time step
                                          vrep.simx_opmode_oneshot)

            print("Loading scene...")
            vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)

            # Get quadrotor handle
            err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)

            # Initialize quadrotor position and orientation
            vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
            vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
            vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)

            # Start simulation
            vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

            for i in range(len(propellers)):
                vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)

            err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
            err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
            err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
                    quad_handle, vrep.simx_opmode_buffer)

            pos_start = np.asarray(pos_start)
            euler_start = np.asarray(euler_start)*10.
            vel_start = np.asarray(vel_start)
            angvel_start = np.asarray(angvel_start)*10.

            pos_old = pos_start
            euler_old = euler_start
            vel_old = vel_start
            angvel_old = angvel_start
            pos_new = pos_old
            euler_new = euler_old
            vel_new = vel_old
            angvel_new = angvel_old

            pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
            euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
            vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
            angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new

            pos_error_all = np.vstack([pos_error_all,pos_error])
            euler_error_all = np.vstack([euler_error_all,euler_error])

            state = [0 for i in range(n_input)]
            state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)

            propeller_vels = [init_f, init_f, init_f, init_f]

            extended_state = torch.cat((state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
            print('duration: ',len(memory))
            memory = ReplayMemory(ROLLOUT_LEN)
            test_num += 1
            timestep = 1
Esempio n. 33
0
    def __init__(self):
        rospy.init_node('Dummy', anonymous=True)
        rospy.Subscriber("/restart", Int8, self.receiveStatus, queue_size=1)
        fin = rospy.Publisher('/finished', Int8, queue_size=1)
        report_sim = rospy.Publisher('/simulation', String, queue_size=1)
        starting = rospy.Publisher('/starting', Int16, queue_size=1)
        vrep.simxFinish(-1)  #clean up the previous stuff
        clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
        if clientID == -1:
            print("Could not connect to server")
            sys.exit()

        first = True
        counter = 0
        while (counter < episodes):
            print("Episode Number ", counter + 1)
            r = 1
            if True:  #not first:
                if (counter) % 50 == 0 and counter != 0:
                    print('Sleep for 60')
                    #time.sleep(60)
                else:
                    time.sleep(3)
                simulation_index = np.random.randint(len(VREP_SCENES))
                sim_name, sim_path = VREP_SCENES[simulation_index]
                msg = String()
                msg.data = sim_name
                report_sim.publish(msg)
                vrep.simxLoadScene(clientID, sim_path, 0,
                                   vrep.simx_opmode_blocking)
                time.sleep(2)
                while r != 0:
                    r = vrep.simxStartSimulation(clientID,
                                                 vrep.simx_opmode_oneshot)
                msg = Int16()
                msg.data = counter + 1
                starting.publish(msg)
            start = time.time()
            self.restart = False
            elapsed = 0
            while (not self.restart and elapsed < maxTime):
                curr = time.time()
                elapsed = curr - start
            vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
            if not self.restart:  #timed out!
                msg = Int8()
                msg.data = 2
                fin.publish(msg)
                print(' #### Timed out!')
            is_running = True
            while is_running:
                error_code, ping_time = vrep.simxGetPingTime(clientID)
                error_code, server_state = vrep.simxGetInMessageInfo(
                    clientID, vrep.simx_headeroffset_server_state)
                is_running = server_state & 1
            counter += 1
            first = False
        time.sleep(2)
        msg = Int8()
        msg.data = 1
        fin.publish(msg)
Esempio n. 34
0
    def init_client(self):
        self.logger = logging.getLogger("learner")
        # Stop any previous simulation
        vrep.simxStopSimulation(-1, vrep.simx_opmode_oneshot_wait)
        # Start a client
        self.client_id = vrep.simxStart(self.server_ip, self.server_port, True,
                                        True, 5000, 5)
        if self.client_id == -1:
            self.logger.critical('Failed connecting to remote API server')
            self.logger.critical('EXIT')
            thread.exit()

        # Enable synchronous mode
        e = vrep.simxSynchronous(self.client_id, True)
        self.logger.info("simxSynchronous=%d" % e)
        if e != 0:
            self.logger.critical('Failed enabling remote API synchronous mode')
            self.logger.critical('EXIT')
            thread.exit()

        # Start simulation
        e = vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_blocking)
        self.logger.info("simxStartSimulation=%d" % e)
        if e != 0:
            self.logger.critical('Failed to start simulation')
            self.logger.critical('EXIT')
            thread.exit()

        # Print ping time
        sec, msec = vrep.simxGetPingTime(self.client_id)
        self.logger.info("Started simulation on %s:%d" %
                         (self.server_ip, self.server_port))
        self.logger.info("Ping time: %f" % (sec + msec / 1000.0))

        # Obtain handle for body (for orientation, positions etc)
        _, self.body_handle = vrep.simxGetObjectHandle(
            self.client_id, 'Ant_body', vrep.simx_opmode_blocking)

        # Obtain joint handles
        _, Ant_joint1Leg1 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint1Leg1',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint2Leg1 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint2Leg1',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint3Leg1 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint3Leg1',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint1Leg2 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint1Leg2',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint2Leg2 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint2Leg2',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint3Leg2 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint3Leg2',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint1Leg3 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint1Leg3',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint2Leg3 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint2Leg3',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint3Leg3 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint3Leg3',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint1Leg4 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint1Leg4',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint2Leg4 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint2Leg4',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint3Leg4 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint3Leg4',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint1Leg5 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint1Leg5',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint2Leg5 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint2Leg5',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint3Leg5 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint3Leg5',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint1Leg6 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint1Leg6',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint2Leg6 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint2Leg6',
                                                     vrep.simx_opmode_blocking)
        _, Ant_joint3Leg6 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_joint3Leg6',
                                                     vrep.simx_opmode_blocking)

        # Obtain body joint handles
        _, Ant_neckJoint1 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_neckJoint1',
                                                     vrep.simx_opmode_blocking)
        _, Ant_neckJoint2 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_neckJoint2',
                                                     vrep.simx_opmode_blocking)
        _, Ant_neckJoint3 = vrep.simxGetObjectHandle(self.client_id,
                                                     'Ant_neckJoint3',
                                                     vrep.simx_opmode_blocking)
        _, Ant_leftJawJoint = vrep.simxGetObjectHandle(
            self.client_id, 'Ant_leftJawJoint', vrep.simx_opmode_blocking)
        _, Ant_rightJawJoint = vrep.simxGetObjectHandle(
            self.client_id, 'Ant_rightJawJoint', vrep.simx_opmode_blocking)

        # Order : [Ant_joint{1-3}Leg{1-6}, Ant_neckJoint{1-3}, Ant_leftJawJoint, Ant_rightJawJoint]
        self.handles = [
            Ant_joint1Leg1, Ant_joint2Leg1, Ant_joint3Leg1, Ant_joint1Leg2,
            Ant_joint2Leg2, Ant_joint3Leg2, Ant_joint1Leg3, Ant_joint2Leg3,
            Ant_joint3Leg3, Ant_joint1Leg4, Ant_joint2Leg4, Ant_joint3Leg4,
            Ant_joint1Leg5, Ant_joint2Leg5, Ant_joint3Leg5, Ant_joint1Leg6,
            Ant_joint2Leg6, Ant_joint3Leg6, Ant_neckJoint1, Ant_neckJoint2,
            Ant_neckJoint3, Ant_leftJawJoint, Ant_rightJawJoint
        ]

        self.joint_count = len(self.handles)
        self.signal_values()
        self.wait_till_stream()

        # log these for consistency
        self.start_pos = self.get_position()
        self.start_orientation = self.get_orientation()
Esempio n. 35
0
def foo(portNumb, instructions):
    clientID = vrep.simxStart('127.0.0.1', portNumb, True, True, 5000, 5)
    if clientID != -1:
        print("se pudo establecer la conexión con la api del simulador")

        #Recover handlers for robot parts
        LUM, LLM, RUM, RLM, head = recoverRobotParts(clientID)

        #Set Initial Target Velocity to 0
        LUMSpeed = 0
        LLMSpeed = 0
        RUMSpeed = 0
        RLMSpeed = 0
        setVelocity(clientID, LUM, LUMSpeed)
        setVelocity(clientID, LLM, LLMSpeed)
        setVelocity(clientID, RUM, RUMSpeed)
        setVelocity(clientID, RLM, RLMSpeed)

        #Set simulation to be Synchonous instead of Asynchronous
        vrep.simxSynchronous(clientID, True)

        #Setting Time Step to 50 ms (miliseconds)
        dt = 0.05
        #WARNING!!! - Time step should NEVER be set to custom because it messes up the simulation behavior!!!!
        #vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_blocking)

        #Start simulation if it didn't start
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

        #This are for controlling where I'm in the instructions while simulation is running
        secuenceIndex = 0
        runInfo = []
        headSecuenceTrace = []
        lowerSpeed, upperSpeed = 0, 0
        secuenceTimes = []
        for secuence in instructions:
            instructionIndex = 0
            headTrace = []
            extraPoints = 0
            runtime = 0
            for instruction in secuence:
                instructionIndex += 1

                moveRobot(clientID, instruction[0], LUM, LLM, RUM, RLM)

                #This is what makes the simulation Synchronous
                initialTime = 0.0
                actualTime = initialTime + dt
                runtime += dt
                #Condition to stop simulation
                hasFallen = False
                vrep.simxSynchronousTrigger(clientID)

                #Retrive head position
                headPosition = vrep.simxGetObjectPosition(
                    clientID, head, -1, vrep.simx_opmode_oneshot)
                #headTrace.append((headPosition,runtime))
                while ((actualTime - initialTime) < (instruction[1] / 10)):
                    #Make de simulation run one step (dt determines how many seconds pass by between step and step)
                    vrep.simxSynchronousTrigger(clientID)
                    #Advance time in my internal counter
                    actualTime = actualTime + dt
                    runtime += dt
                    #TODO do I still need the extra points for time?
                    extraPoints += dt
                    #Retrive head position
                    headPosition = vrep.simxGetObjectPosition(
                        clientID, head, -1, vrep.simx_opmode_oneshot)
                    headTrace.append((headPosition, runtime))

                    #Verify that the model hasn't fallen
                    if (headPosition[0] == 0 and headPosition[1][2] < 0.65):
                        #print("Posición de la cabeza:", headPosition[1][2])
                        #print("tiempo: ", runtime)
                        hasFallen = True
                        break
                if (hasFallen):
                    break
            if (hasFallen):
                print("Secuence: ", secuenceIndex, " has fallen!!")
            else:
                print("Secuence: ", secuenceIndex,
                      " has finished without falling")
            #print(secuence)

            secuenceTimes.append(extraPoints)
            #Here I collect the data for the whole secuence
            #filter not valid positions
            headTrace = list(filter(lambda x: x[0][0] == 0, headTrace))
            #add to whole run trace info
            headSecuenceTrace.append(headTrace)

            fallenFactor = 0
            if hasFallen:
                fallenFactor = -50
            #format: (index, score, headtrace((valid,(x,y,z),time))
            runInfo.append(
                (secuenceIndex,
                 sum(
                     map(
                         lambda x: math.log(1 / distancia(
                             x[0][1], puntoMovil(x[1]))), headTrace)) +
                 fallenFactor, headTrace))
            print("puntaje obtenido", runInfo[-1][1])
            secuenceIndex += 1
            #Stop_Start_Simulation
            vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
            #This sleep is necesary for the simulation to finish stopping before starting again
            time.sleep(2)
            vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
        #Should always end by finishing connetions
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
        vrep.simxFinish(clientID)

        sortedScore = sorted(runInfo, key=lambda x: x[1], reverse=True)
        return sortedScore
    else:
        print(
            "No se pudo establecer conexión con la api del simulador en el puerto: ",
            portNumb)
        print("Verificar que el simulador está abierto")
Esempio n. 36
0
    def __init__(self):
        # Closes all opened connections
        vrep.simxFinish(-1)

        # Connects to V-REP
        self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
Esempio n. 37
0
def truncated_Fourier(coeficients, time):
    value = coeficients[0] / 2.0

    for i in xrange(N_COEF):
        value += coeficients[2 * i + 2] * math.cos(
            (i + 1) * coeficients[1] * time)
        value += coeficients[2 * i + 3] * math.sin(
            (i + 1) * coeficients[1] * time)

    return value


#Conexao com o vrep
vrep.simxFinish(-1)  # just in case, close all opened connections
clientID = vrep.simxStart(
    '127.0.0.1', 19997, True, True, 5000,
    5)  # Conecta com o VREP. Por padrao ele ja abre essa porta.
if clientID == -1:
    exit(10)


#Reset da simulacao
def reset_simulation(clientID):
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
    time.sleep(1)  # um pequeno sleep entre o stop e o start
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)


#Juntas do NAO
Head_Yaw = []
Head_Pitch = []
Esempio n. 38
0

def setMotorSpeeds(clientID, handles, speeds):
    vrep.simxSetJointTargetVelocity(clientID, handles[0], speeds[0],
                                    vrep.simx_opmode_streaming)
    vrep.simxSetJointTargetVelocity(clientID, handles[1], speeds[1],
                                    vrep.simx_opmode_streaming)
    vrep.simxSetJointTargetVelocity(clientID, handles[2], speeds[2],
                                    vrep.simx_opmode_streaming)
    vrep.simxSetJointTargetVelocity(clientID, handles[3], speeds[3],
                                    vrep.simx_opmode_streaming)


if __name__ == "__main__":
    vrep.simxFinish(-1)  #clean up the previous stuff
    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    counter = 0
    with vrepInterface.VRepInterface.open() as vr:
        vr.simxStartSimulation(vrep.simx_opmode_oneshot_wait)
        try:
            while (1):
                sensorHandle, sensorHandle_left, sensorHandle_right = getSensorHandles(
                    clientID)
                follower_frontMotorHandles = getFollowerMotorHandles(clientID)
                leader_frontMotorHandles = getLeaderMotorHandles(clientID)

                error, blarg, middle_image = vrep.simxReadVisionSensor(
                    clientID, sensorHandle, vrep.simx_opmode_oneshot_wait)
                error, blarg, left_image = vrep.simxReadVisionSensor(
                    clientID, sensorHandle_left, vrep.simx_opmode_oneshot_wait)
                error, blarg, right_image = vrep.simxReadVisionSensor(
Esempio n. 39
0
 def start(self,port):
     vrep.simxFinish(-1)
     self.clientID=vrep.simxStart('127.0.0.1',port,True,True,5000,5)
Esempio n. 40
0
import numpy as np
import time
import math as m
import sys
import vrep # access all the VREP elements
from q2R import q2R

vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # start a connection
if clientID!=-1:
	print ('Connected to remote API server')
else:
	print('Not connected to remote API server')
	sys.exit("No connection")

# Getting handles for the motors
err, motorL = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_blocking)
err, motorR = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_blocking)
err, robot  = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_blocking)

# Assigning handles to the ultrasonic sensors
usensor = []
for i in range(1,17):
    err, s = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor'+str(i), vrep.simx_opmode_blocking)
    usensor.append(s)

# Sensor initialization
for i in range(16):
    err, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(clientID, usensor[i], vrep.simx_opmode_streaming)

err, psr = vrep.simxGetObjectPosition(clientID, usensor[2], robot, vrep.simx_opmode_streaming)
Esempio n. 41
0
    print('')

import time
import cv2
import numpy as np
import matplotlib.pyplot as plt
import math

PI = math.pi

print("Simulação iniciada!!")
vrep.simxFinish(-1)  #fecha todas as conexões existentes

port = 25100

clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5)

if clientID != -1:
    print("Conectado ao servidor remote API na porta ", port)

    errCode, left_motor_handle = vrep.simxGetObjectHandle(
        clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)

    if errCode != vrep.simx_return_ok:
        print("Erro em obter o handle")

    errCode, right_motor_handle = vrep.simxGetObjectHandle(
        clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

    if errCode != vrep.simx_return_ok:
        print("Erro em obter o handle")
Esempio n. 42
0
def run_benchmark_training(sim_number, sim_time):
    print("start running benchmark model")
    prefs.codegen.target = "numpy"
    start_scope()

    # number of sensor in robot
    N_sensor = 8
    N_vision = 2

    # robot radius
    r = 0.0586

    # CD neurons number
    N_CD = 36
    # vision detection_neurons number
    N_VD = 10

    # HD neuron number
    N_HD = 72
    # coordinate neuron
    N_x_axis, N_y_axis = 32, 32
    N_PI = N_x_axis * N_y_axis

    # Simulation time
    # Speed cell number
    N_speed = 6

    m = 5  # 60
    iter = 0

    # width and length of the arena x=width/ y=lenght
    # 1 square in vrep = 0.5*0.5
    x_scale = 2
    y_scale = 2
    # distance unit per neuron
    N = 200 / (N_x_axis * N_y_axis)

    #define the initial mismatch encounter
    last_mismatch = 0

    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Collision detection Neural Architecture--------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark, wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex, Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson, spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse, Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron, Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred, color_notred_synapses, color_red_synapses = CD_net(
        N_CD, N_vision, N_VD)

    # gaussian input for 8 collison sensors
    stimuli = np.array([gaussian_spike(N_CD, j * N_CD / N_sensor, 10, 0.2) for j in range(N_sensor)])

    # gaussion distribution for 2 vision input(wall or landmark)
    stimuli_vision = np.array([gaussian_spike(N_VD, j * N_VD / N_vision, 10, 0.2) for j in range(N_vision)])

    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Head Direction and position Neural Architecture----------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse, str_IHD_synapse, sti_PI, sti, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron, IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons, West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse, PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons, WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse, IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse = HD_PI_integrated_net(
        N_HD, N_speed, N_x_axis, N_y_axis, sim_time)

    ##-----------------head direction correcting matrix network-------------#
    # Poisson_IMU,IMU_Neuron,HD_errormatrix_neurons,HD_positive_error,HD_negative_error,IMU_poi_synapses,IMU_errors_connecting_synapses,HD_error_connect_synapses,Error_negative_synapses,Error_positive_synapses,Ex_speed_pool_Neurons,Inh_speed_pool_Neurons,Positive_ex_pool_synapses,Negative_inh_pool_synapses,statemon_positiveHD_error,statemon_negativeHD_error,spikemon_positiveHD_error,spikemon_negativeHD_error,spiketrain_pos=HD_error_correcting(N_HD,HD_Neuron)


    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Fusi Synapse between Position - Landmark Neuron----------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##

    '''measure collision weight by fusi synapse landmark'''
    landmark_PI_plastic = fusi_landmark(PI_Neurons, Landmark)
    w_landmark_plas_shape = np.shape(landmark_PI_plastic.w_fusi_landmark)[0]
    w_plastic_landmark = landmark_PI_plastic.w_fusi_landmark
    all_fusi_weights_landmark = landmark_PI_plastic.w_fusi_landmark

    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Fusi Synapse between Position - Wall Neuron----------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    '''measure collision weight by fusi synapse landmark'''
    wall_PI_plastic = fusi_wall(PI_Neurons, Wall)
    w_plas_shape_wall = np.shape(wall_PI_plastic.w_fusi_wall)[0]
    w_plastic_wall = wall_PI_plastic.w_fusi_wall
    all_fusi_weights_wall = wall_PI_plastic.w_fusi_wall

    ##--------------------------------------------------------------------------------------------------------------------##
    ##-------------------------------------------Mismatch Network--------------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    '''store previously estimated landmark position by fusi synapse estimatedlandmark'''

    preeq = '''
            v_post += 1*(w_piest >= 0.5)
            '''
    PI_Neurons_est_synapse = Synapses(PI_Neurons, Estimated_landmark_neuron, 'w_piest : 1', on_pre=preeq)
    PI_Neurons_est_synapse.connect()

    Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron, Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse, NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse, Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch = mismatch_net(
        Non_landmark_neuron, Estimated_landmark_neuron, Landmark)

    ##--------------------------------------------------------------------------------------------------------------------##
    ##----------------------------------------------Network--------------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    print("adding network")
    # Network
    PINet = Network()
    # add collision network
    PINet.add(Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark,
              wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron,
              Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse,
              Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex,
              Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson,
              spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse,
              Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron,
              Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex,
              Red, Notred, color_notred_synapses, color_red_synapses
              )

    # add position network
    PINet.add(IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse,
              str_IHD_synapse, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron,
              IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron,
              HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse,
              Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD,
              Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse,
              PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons,
              West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse,
              PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse,
              PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons,
              WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse,
              IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse,
              IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse)
    # add fusi plastic synapses for landmark,wall neurons with PI neurons
    PINet.add(landmark_PI_plastic)
    PINet.add(wall_PI_plastic)
    # add mismatch network
    PINet.add(PI_Neurons_est_synapse, Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse,
              spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron,
              Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse,
              NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse,
              Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch)

    ##-----------------------------------------------------------------------------------------##
    ##---------------------------------Robot controller----------------------------------------##
    ##-----------------------------------------------------------------------------------------##
    # Connect to the server
    print('finished adding network')

    vrep.simxFinish(-1)  # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    if clientID != -1:
        print("Connected to remote API server")
    else:
        print("Not connected to remote API server")
        sys.exit("Could not connect")
    vrep.simxSynchronous(clientID, 1)

    ##----------------------------------Controller initialized------------------------------------------##
    # set motor
    err_code, l_motor_handle = vrep.simxGetObjectHandle(clientID, "KJunior_motorLeft", vrep.simx_opmode_blocking)
    err_code, r_motor_handle = vrep.simxGetObjectHandle(clientID, "KJunior_motorRight", vrep.simx_opmode_blocking)

    # Compass output=orientation
    # define robot
    err_code, robot = vrep.simxGetObjectHandle(clientID, 'KJunior', vrep.simx_opmode_blocking)

    # define Angles
    err_code, Angles = vrep.simxGetObjectOrientation(clientID, robot, -1, vrep.simx_opmode_streaming)

    # define object position
    err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1, vrep.simx_opmode_streaming)

    # get sensor
    sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8 = getSensor(clientID)
    # read point
    detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
        sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8, clientID)
    # Distance from sensor to obstacle
    sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
        detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7,
        detectedPoint8)
    # get sensor for mismatch landmark detection(get sensor point and distance)
    err_code, sensorMismatch = vrep.simxGetObjectHandle(clientID, "Proximity_sensor", vrep.simx_opmode_blocking)
    err_code,detectionStateMismatch,detectedPointMismatch,detectedObjectHandleMismatch,detectedSurfaceNormalVectorMismatch=vrep.simxReadProximitySensor(clientID,sensorMismatch,vrep.simx_opmode_streaming)
    mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)

    # get vision sensor
    err_code, camera = vrep.simxGetObjectHandle(clientID, "Vision_sensor", vrep.simx_opmode_blocking)
    err_code, resolution, image = vrep.simxGetVisionSensorImage(clientID, camera, 1, vrep.simx_opmode_streaming)

    ##---------------------------------Sensor initial condition (for CD)------------------------------------##
    # sensor value of every sensor for every neuron
    # Use inv_filter to filter out noise
    # sensor_val = np.array([np.repeat(inv_filter(sensor_val1),N_CD), np.repeat(inv_filter(sensor_val2),N_CD),np.repeat(inv_filter(sensor_val3),N_CD),np.repeat(inv_filter(sensor_val7),N_CD),np.repeat(inv_filter(sensor_val8),N_CD)])
    sensor_val = np.array([np.repeat(inv_filter(sensor_val1), N_CD), np.repeat(inv_filter(sensor_val2), N_CD),
                           np.repeat(inv_filter(sensor_val3), N_CD), np.repeat(inv_filter(sensor_val4), N_CD),
                           np.repeat(inv_filter(sensor_val5), N_CD), np.repeat(inv_filter(sensor_val6), N_CD),
                           np.repeat(inv_filter(sensor_val7), N_CD), np.repeat(inv_filter(sensor_val8), N_CD)])
    sensor_val_vision = np.zeros([N_vision, N_VD])
    # sum of each sensor value * its gaussian distribution --> sum to find all activity for each neurons --> WTA
    All_stimuli = np.sum(sensor_val * stimuli, axis=0)
    All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)

    # find the winner
    winner = WTA(All_stimuli)
    for w in winner:
        Poisson_synapse.w_poi[w] = All_stimuli[w]
    winner_vision = WTA(All_stimuli_vision)
    for w in winner_vision:
        PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w]

    ##------------------------------------------------------------------------------------------------##

    # initial speed of wheel
    l_steer = 0.24
    r_steer = 0.24
    # record the initial time
    t_int = time.time()
    # record compass angle
    compass_angle = np.array([])
    # record x and y axis at each time
    x_axis = np.array([])
    y_axis = np.array([])
    # path that is passing
    r = np.array([])
    # all_time
    all_time = np.array([])
    # collision index that the robot collided
    collision_index = np.array([])
    collision_index_during_run = []

    # record weight parameter
    # record every m sec
    weight_record_time = 1
    weight_during_run_landmark = np.zeros([np.shape(w_plastic_landmark)[0], 3 * int(sim_time / 5)])
    weight_during_run_wall = np.zeros([np.shape(w_plastic_wall)[0], 3 * int(sim_time / 5)])
    time_step_list = np.array([])

    # start simulation
    while (time.time() - t_int) < sim_time:

        t1 = time.time() - t_int
        # record proximity sensor at each time step
        sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
            detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6,
            detectedPoint7, detectedPoint8)
        all_sensor = np.array([sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5])
        all_sensor[all_sensor < 4.1e-20] = np.infty
        activated_sensor = np.argmin(all_sensor)

        # obtain vision sensor values
        pixelimage = set(image)

        if all_sensor[activated_sensor] < 0.1:
            if activated_sensor == 3:
                r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
            elif activated_sensor == 4:
                r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
            elif activated_sensor == 0:
                r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
            elif activated_sensor == 1:
                r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
            elif activated_sensor == 2:
                r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
            else:
                l_steer = 0.24
                r_steer = 0.24
                zeta = 0
        else:
            l_steer = 0.24
            r_steer = 0.24
            zeta = 0

        ####-------------------- Record weight------------------------####
        weight_during_run_landmark[:, iter] = w_plastic_landmark
        weight_during_run_wall[:, iter] = w_plastic_wall
        collision_index_during_run.append(collision_index)

        ####-------------------- Start recording spike (CD) ------------------------####
        # cleaning noises for the collision distance sensor value
        sensor_val = np.array([np.repeat(inv_filter(sensor_val1), N_CD), np.repeat(inv_filter(sensor_val2), N_CD),
                               np.repeat(inv_filter(sensor_val3), N_CD), np.repeat(inv_filter(sensor_val4), N_CD),
                               np.repeat(inv_filter(sensor_val5), N_CD), np.repeat(inv_filter(sensor_val6), N_CD),
                               np.repeat(inv_filter(sensor_val7), N_CD), np.repeat(inv_filter(sensor_val8), N_CD)])
        # get the vision sensor value
        sensor_val_vision = generate_vision_val(pixelimage, N_VD)

        # reset weight to 0 again
        Poisson_synapse.w_poi = 0
        PoissonVision_synapse.w_vision_poi = 0

        err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming)
        err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming)

        # All stimuli and WTA
        All_stimuli = (np.sum(sensor_val * stimuli, axis=0))
        winner = WTA(All_stimuli)
        for w in winner:
            Poisson_synapse.w_poi[w] = All_stimuli[w]

        All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
        winner_vision = WTA(All_stimuli_vision)
        for w in winner_vision:
            PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w] / 10

        # --------mismatching detecting---------##
        PI_Neurons_est_synapse.w_piest = landmark_PI_plastic.w_fusi_landmark

        ####-------------------- End recording spike ----------------------------####

        ####-------------------- Start recording Head direction ------------------------####

        # Choose neuron that is the nearest to the turning speed/direction
        # if turn left
        if r_steer == 0:
            neuron_index = nearest_neuron_speed(zeta, N_speed)
            for syn in range(N_speed):
                Left_drive_synapse[syn].w_left_drive = 0
                Right_drive_synapse[syn].w_right_drive = 0
            Left_drive_synapse[neuron_index].w_left_drive = 5
            Right_drive_synapse[neuron_index].w_right_drive = 0
            Go_straight.w_str = -3
            directional_PI_synapse.w_dir_PI = 0
            Stay_stay_layer.w_stay_stay = 2
        # if turn right
        elif l_steer == 0:
            neuron_index = nearest_neuron_speed(zeta, N_speed)
            for syn in range(N_speed):
                Left_drive_synapse[syn].w_left_drive = 0
                Right_drive_synapse[syn].w_right_drive = 0
            Left_drive_synapse[neuron_index].w_left_drive = 0
            Right_drive_synapse[neuron_index].w_right_drive = 5
            Go_straight.w_str = -3
            directional_PI_synapse.w_dir_PI = 0  # if turn position PI stay
            Stay_stay_layer.w_stay_stay = 2
        # if go straight
        else:
            for syn in range(N_speed):
                Left_drive_synapse[syn].w_left_drive = 0
                Right_drive_synapse[syn].w_right_drive = 0
            Go_straight.w_str = 10
            directional_PI_synapse.w_dir_PI = 4  # if move position PI run
            Stay_stay_layer.w_stay_stay = -4

        ##-----------------reset IHD (Compass) -----------------##
        # Get actual heading direction
        err_code, Angles = vrep.simxGetObjectOrientation(clientID, robot, -1, vrep.simx_opmode_streaming)
        heading_dir = getHeadingdirection(Angles)
        # print("IMU head direction is", heading_dir, "angle from IMU is", Angles)
        compass_angle = np.append(compass_angle, heading_dir)

        # recalibrate head direction to nearest neuron
        recal = nearest_neuron_head(heading_dir, N_HD)

        # set reset by compass weight upon angle atm
        Reset_synapse.w_reset = np.array(gaussian_spike(N_HD, recal, 30, 0.03))
        # print("reset synapse",Reset_synapse.w_reset)

        ##----------------Active searching for landmark when encountering mismatch -----------------##
        if len(spikemon_Mismatch) > last_mismatch:
            print('encounter mismatch')
            err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
                clientID, sensorMismatch, vrep.simx_opmode_streaming)
            mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)

            while mismatch_sensordistance  <= 0.01:
                l_steer = 0
                r_steer = 0.5
                err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer,vrep.simx_opmode_streaming)
                err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer,vrep.simx_opmode_streaming)
                err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
                    clientID, sensorMismatch, vrep.simx_opmode_streaming)
                mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
            current_error_distance =  mismatch_sensordistance
            l_steer = 0.24
            r_steer = 0.24
            err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming)
            err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming)
        last_mismatch = len(spikemon_Mismatch)

        ##----------------- Read position -----------------##
        err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1, vrep.simx_opmode_streaming)
        # print("position value is",Position)
        x_pos = Position[0]
        y_pos = Position[1]

        # recalibrate head direction to nearest neuron
        x_axis = np.append(x_axis, x_pos)
        y_axis = np.append(y_axis, y_pos)

        # recalibrate
        recal_x_axis = nearest_neuron_x_axis(x_pos, N_x_axis, x_scale)
        recal_y_axis = nearest_neuron_y_axis(y_pos, N_y_axis, y_scale)
        recal_index = N_x_axis * recal_y_axis + recal_x_axis

        r = np.append(r, recal_index)  # is an array keeping all index that neuron fire during the run

        ##----------------- reset position neuron with IMU recalibration ---------------------##
        '''coment this line to disable IMU'''
        #PI_Reset_synapse.w_poi_PI = np.array(gaussian_spike(N_PI, recal_index, 20, 0.01))

        ##----------------- Recording Position Index when collision -----------------##
        if l_steer == 0 or r_steer == 0:
            collision_index = np.append(collision_index, recal_index)

        ##----------------- Collect time-----------------##
        all_time = np.append(all_time, time.time() - t_int)

        # run
        PINet.run(15 * ms)

        print("current plas landmark synapese index", np.where(w_plastic_landmark >= 0.5))
        print('wall synpases values index bigger', np.where(w_plastic_wall >= 0.5))
        print('count mismatch', len(spikemon_Mismatch))
        print('count landmark', len(spikemon_landmark))

        print('####-------------------- Read sensor (new round) ----------------------------####')

        # Start new measurement in the next time step
        detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
            sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8, clientID)
        err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
            clientID, sensorMismatch, vrep.simx_opmode_streaming)
        err_code, resolution, image = vrep.simxGetVisionSensorImage(clientID, camera, 1, vrep.simx_opmode_streaming)

        # record time step and final time
        t2 = time.time() - t_int
        delta_t = t2 - t1
        final_time = time.time()
        time_step_list = np.append(time_step_list, delta_t)
        print("delta-t", delta_t,iter)
        print("final time t2", int(t2))
        iter += 1

    ##---------------end of simulation ---------------##

    print("Done")

    # saving data
    # setting pathway
    pathway = "/Users/jieyab/SNN/for_plotting/"


    exp_number = str(sim_number)
    np.save(pathway + "r" + exp_number, r)
    np.save(pathway + "spikemon_CD_i" + exp_number, spikemon_CD.i)
    np.save(pathway + "spikemon_CD_t" + exp_number, spikemon_CD.t / ms)
    np.save(pathway + "spikemon_HD_i" + exp_number, spikemon_HD.i)
    np.save(pathway + "spikemon_HD_t" + exp_number, spikemon_HD.t / ms)
    np.save(pathway + "spikemon_PI_i" + exp_number, spikemon_PI.i)
    np.save(pathway + "spikemon_PI_t" + exp_number, spikemon_PI.t / ms)

    np.save(pathway + "spikemon_noncollision_i" + exp_number, spikemon_noncollison.i)
    np.save(pathway + "spikemon_noncollision_t" + exp_number, spikemon_noncollison.t / ms)

    np.save(pathway + "spikemon_wall_i" + exp_number, spikemon_wall.i)
    np.save(pathway + "spikemon_wall_t" + exp_number, spikemon_wall.t / ms)
    np.save(pathway + "spikemon_nonlandmark_i" + exp_number, spikemon_nonlandmark.i)
    np.save(pathway + "spikemon_nonlandmark_t" + exp_number, spikemon_nonlandmark.t / ms)
    np.save(pathway + "spikemon_landmark_i" + exp_number, spikemon_landmark.i)
    np.save(pathway + "spikemon_landmark_t" + exp_number, spikemon_landmark.t / ms)
    np.save(pathway + "weight_during_run_landmark" + exp_number, weight_during_run_landmark)
    np.save(pathway + "weight_during_run_wall" + exp_number, weight_during_run_wall)
    np.save(pathway + "weight_landmark" + exp_number, w_plastic_landmark)
    np.save(pathway + "weight__wall" + exp_number, w_plastic_wall)
    np.save(pathway + "spikemon_mismatch_t" + exp_number, spikemon_Mismatch.t / ms)
    np.save(pathway + "spikemon_mismatch_v" + exp_number, spikemon_Mismatch.v)
    np.save(pathway + "collision_index" + exp_number, collision_index)
    np.save(pathway + "collision_index_during_run" + exp_number, collision_index_during_run)

    np.save(pathway + "compass_angle" + exp_number, compass_angle)
    np.save(pathway + "all_time" + exp_number, all_time)

    np.save(pathway + "spikemon_estimatedlandmark_i" + exp_number, spikemon_estimatedlandmark.i)
    np.save(pathway + "spikemon_estimatedlandmark_t" + exp_number, spikemon_estimatedlandmark.t / ms)
    np.save(pathway + "spikemon_nonestimatedlandmark_i" + exp_number, spikemon_nonestimatedlandmark.i)
    np.save(pathway + "spikemon_nonestimatedlandmark_t" + exp_number, spikemon_nonestimatedlandmark.t / ms)
    np.save(pathway + "spikemon_red_i" + exp_number, spikemon_red.i)
    np.save(pathway + "spikemon_red_t" + exp_number, spikemon_red.t / ms)
    np.save(pathway + "spikemon_notred_i" + exp_number, spikemon_notred.i)
    np.save(pathway + "spikemon_notred_t" + exp_number, spikemon_notred.t / ms)
    np.save(pathway + "step_time" + exp_number, time_step_list)

    return clientID
Esempio n. 43
0
        ax4.xaxis.set_ticks_position('bottom')
        ax4.set_xlabel('state')
        ax4.set_ylabel('action')
        plt.tight_layout()
        plt.show()
        plt.draw()


if __name__ == '__main__':
    try:
        client_id
    except NameError:
        client_id = -1
    e = vrep.simxStopSimulation(client_id, vrep.simx_opmode_oneshot_wait)
    vrep.simxFinish(-1)
    client_id = vrep.simxStart('127.0.0.1', 25000, True, True, 5000, 5)
    assert client_id != -1, 'Failed connecting to remote API server'

    # print ping time
    sec, msec = vrep.simxGetPingTime(client_id)
    print "Ping time: %f" % (sec + msec / 1000.0)

    robot = Robot(client_id)
    agent = Agent(robot, alpha=0.5, gamma=0.9, epsilon=0.9, q_init=0)

    num_episodes = 10
    len_episode = 200
    return_history = []
    try:
        for episode in range(num_episodes):
            print "start simulation # %d" % episode
Esempio n. 44
0
def init(port=19997):
    vrep.simxFinish(-1)  # 关闭所有连接
    clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5)  # 开启连接
    if clientID == -1:
        raise NameError("Could not connect Vrep")
    return clientID
Esempio n. 45
0
"""
"""
    Vrep y OpenCV en Python

    @author: Jose Antonio Ruiz Millan
"""
import vrep
import sys
import cv2
import numpy as np
import time
import matplotlib.pyplot as plt

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)

if clientID != -1:
    print('Conexion establecida')

else:
    sys.exit("Error: no se puede conectar")  #Terminar este script

#Guardar la referencia de los motores
_, left_motor_handle = vrep.simxGetObjectHandle(clientID,
                                                'Pioneer_p3dx_leftMotor',
                                                vrep.simx_opmode_oneshot_wait)
_, right_motor_handle = vrep.simxGetObjectHandle(clientID,
                                                 'Pioneer_p3dx_rightMotor',
                                                 vrep.simx_opmode_oneshot_wait)
Esempio n. 46
0
	def __init__(self):

		print 'started'
		# Init vrep client
		vrep.simxFinish(-1)
		# list obstacles in vrep here for it to get detected and mapped into obstacle space
		objectsList = ['Wall1','Wall2','Wall3','Wall4','Wall5','Wall6', 'Right_Wall', 'Bottom_Wall', 'Top_Wall', 'Left_Wall']
		
		# resolution where 0.01 corresponds to 1cm in the simulator
		resolution = 0.02
		
		# the delay between consecutive movment of the quad
		self.moveDelay = 0.4
		
		# get clientID
		self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
		if self.clientID != -1:
			#The quad helper contains function implementations for various quadcopter functions
			self.quad_functions = quad_helper.quad_helper(self.clientID)
			print('Main Script Started')
			# self.quad_functions.init_sensors()
			self.quad_functions.start_sim()
			#Setting initial time
			init_time = time.time()
			d1=0

		#Initialize flags
		self.obstacleAvoidFlag = False

		#Getting map size from the top wall and left wall length
		err,topWallHandle = vrep.simxGetObjectHandle(self.clientID,'Top_Wall',vrep.simx_opmode_blocking)
		err,self.halfTopWallLen = vrep.simxGetObjectFloatParameter(self.clientID,topWallHandle,18,vrep.simx_opmode_blocking)
		err,leftWallHandle = vrep.simxGetObjectHandle(self.clientID,'Left_Wall',vrep.simx_opmode_blocking)
		err,self.halfLeftWallLen = vrep.simxGetObjectFloatParameter(self.clientID,leftWallHandle,19,vrep.simx_opmode_blocking)

		# used for simulation purpose only!!
		positions = [[0,0,3],[0,1,3],[-1,1,3],[-2,1,3],[-2,2,3],[-2,3,3],[-1,3,3],[0,3,3],[1,3,3]]

		# Rate init
		# self.rate = rospy.Rate(20.0)  # MUST be more then 20Hz

		#Initializing publishers
		# rospy.Subscriber("/quad_explore/target_position", Pose, self.posCb)
		
		#Code to shift the origin from the center to the bottom left
		obstOccMat = self.createObstacleOccupancyMat(objectsList,self.clientID,resolution)
		
		corners_cell_wise, image_with_cells = self.cellDecomposition(obstOccMat)
		nCells = corners_cell_wise.shape[0]
		self.cellOccupancyFlag = np.zeros(nCells).tolist()
		print self.cellOccupancyFlag
		
		startPos = [1,1]
		err,self.quadHandle = vrep.simxGetObjectHandle(self.clientID,'Quadricopter',vrep.simx_opmode_blocking)
		err = vrep.simxSetObjectPosition(self.clientID,self.quadHandle, -1, [startPos[0]-self.halfTopWallLen, startPos[1]-self.halfLeftWallLen, 1.0], vrep.simx_opmode_blocking)
		
		for _ in range(50):
			vrep.simxSynchronousTrigger(self.clientID)

		startPos = [int(startPos[1]/resolution), int(startPos[0]/resolution)]
		# endPos = [3,5]
		# endPos = [int(endPos[1]/resolution),int(endPos[0]/resolution)]
		# astarDist, path = self.astar(startPos,endPos, obstOccMat)
		# endPos = [3,5]

		# untul all cells are covered, run the algorithm!
		while 0 in self.cellOccupancyFlag:

			# get the closest cell to the current position
			currCell = self.moveToClosestCorner(corners_cell_wise,resolution,obstOccMat)
			# change coordinates from (x,y) to (y,x)
			currCellYX = self.convertCornerstoYX(currCell)
			# get current position of the quad
			err, obj_pos_origin = vrep.simxGetObjectPosition(self.clientID, self.quadHandle,self.originHandle,vrep.simx_opmode_blocking)
			start_pos_origin =  [int(obj_pos_origin[1]/resolution),int(obj_pos_origin[0]/resolution)]

			# create a new image of same size as obstcale map where everything other than the current cell is an obstacle space
			cell_image = np.ones_like(obstOccMat, dtype=np.uint8)*255
			cell_image[int(currCellYX[0, 0]): int(currCellYX[3, 0]), int(currCellYX[0, 1]):int(currCellYX[3, 1])] = 0

			# generate a path to traverse optimally inside the cell and process it
			path = InsideCellPlanning(start_pos_origin, currCellYX, cell_image, 10, 10)
			# path = np.hstack((path, 0.5*(1/resolution)*np.ones((path.shape[0], 1), dtype = np.uint8)))
			# path = (path*resolution).tolist()
			path = self.make3dPath(path,resolution)
			# move according to the path generated above
			self.simPath(path, self.moveDelay, resolution)
			print self.cellOccupancyFlag
			for _ in range(100):				# wait sometime for the quad to settle in
				vrep.simxSynchronousTrigger(self.clientID)
def VRep_close_and_connect():
    vrep.simxFinish(-1)
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
    return clientID
Esempio n. 48
0
def main():
    global clientID

    print('Program started')
    emptybuf = bytearray()

    vrep.simxFinish(-1)  # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)

    if clientID != -1:
        print('Connected to remote API server')

        # Start the simulation:
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)

        # start init wheels --------------------------------------------------------------------------------------------

        wheelJoints = np.empty(4, dtype=np.int)
        wheelJoints.fill(-1)  # front left, rear left, rear right, front right
        res, wheelJoints[0] = vrep.simxGetObjectHandle(
            clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait)
        res, wheelJoints[1] = vrep.simxGetObjectHandle(
            clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait)
        res, wheelJoints[2] = vrep.simxGetObjectHandle(
            clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait)
        res, wheelJoints[3] = vrep.simxGetObjectHandle(
            clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait)

        # end init wheels ----------------------------------------------------------------------------------------------

        # start init camera --------------------------------------------------------------------------------------------

        # change the angle of the camera view (default is pi/4)
        res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle',
                                      math.pi / 2,
                                      vrep.simx_opmode_oneshot_wait)

        # turn on camera
        res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2,
                                        vrep.simx_opmode_oneshot_wait)

        # get camera object-handle
        res, youBotCam = vrep.simxGetObjectHandle(
            clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait)

        # get first image
        err, resolution, image = vrep.simxGetVisionSensorImage(
            clientID, youBotCam, 0, vrep.simx_opmode_streaming)
        time.sleep(1)

        # end init camera ----------------------------------------------------------------------------------------------

        # start init range sensor ----------------------------------------------------------------------------------------------
        # initialize sensor and get sensor handles:
        rangeSen.initializeSensor(clientID)
        hokuyo = rangeSen.getSensorHandles(clientID)

        # end init range sensor ----------------------------------------------------------------------------------------------

        # programmable space -------------------------------------------------------------------------------------------

        # implement state machine
        # 1 - init | 2 - detect blob | 3 - move to blob | 4 - grab | 5 - follow next explore path | 6 - align to blob | 7 - follow next basket path
        # 8 - drop block | 0 - finish | -1 - finish with error

        state = 1

        # space to store data to share between states
        h_matrix = -1
        explorePaths = Queue()
        basketPaths = Queue()
        blobsList = []
        visitedBlobsList = []
        orientations = Queue()
        posBeforeMoveToBlob = move.getPos(clientID)[0][:-1]
        blockColors = []

        while state != 0:
            if state == 1:  # init
                # init path, H, and next state
                state, explorePaths, basketPaths, orientations, blockColors, h_matrix = ex.init_state(
                    youBotCam, clientID)

            elif state == 2:  # detect blob
                # find all blobs that are 360 degrees around youBot
                state, blobsList = ex.findBlobs(clientID, youBotCam, h_matrix,
                                                blobsList, visitedBlobsList)

            elif state == 3:  # move to blob
                # get to next blob state
                posBeforeMoveToBlob = move.getPos(clientID)[
                    0][:-1]  # store current position for moving back later
                state, blobsList, visitedBlobsList = ex.getToNextBlob(
                    clientID, blobsList, visitedBlobsList)

            elif state == 4:  # grab
                state = ex.grabBlob(clientID, h_matrix, youBotCam)

            elif state == 5:  # follow next explore path
                state = ex.followExplorePath(clientID, hokuyo, explorePaths,
                                             orientations)

            elif state == 6:  # align to blob
                state = ex.alignToBlob(clientID, youBotCam)

            elif state == 7:  # follow next basket path
                state = ex.followBasketPath(clientID, hokuyo, basketPaths,
                                            blockColors)

            elif state == 8:  # drop blob
                state = ex.dropBlob(clientID)
                move.sideway(c.maxDistToBlock, clientID, True)

            elif state == -1:  # finish with error
                print("Current state: fail state!")
                print("An error has occurred. Program finished with state -1.")
                state = 0
        print("End of blob grabing shit")
        '''
        state, explorePaths, basketPaths, orientations, blockColors, h_matrix = ex.init_state(youBotCam, clientID)
        move.forward(0.5, clientID)
        ex.alignToBlob(clientID, youBotCam)
        ex.grabBlob(clientID, h_matrix, youBotCam)
        time.sleep(5)
        #ex.moveArm(clientID, -90, 20,70,0,0)
        #ex.moveArm(clientID, -90, 90,0,0,0)
        #ex.getAngle(clientID)
        #ex.moveArm(clientID, 0, 0,0,0,0)
        # ex.moveArm(clientID, 180/math.pi*ex.getAngle(clientID), 95,40,35,0)
        '''
        # end of programmable space --------------------------------------------------------------------------------------------

        # Stop simulation
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)

        # Close connection to V-REP:
        vrep.simxFinish(clientID)
    else:
        print('Failed connecting to remote API server')
    print('Program ended')
Esempio n. 49
0
def main():

    print("Enter the start point x-coordinate(in cm) --> ")
    x_s = int(input())
    print("Enter the start point y-coordinate --> ")
    y_s = int(input())
    print("Enter the goal point x-coordinate --> ")
    x_g = int(input())
    print("Enter the goal point y-coordinate --> ")
    y_g = int(input())

    # close opened connections
    vrep.simxFinish(-1)

    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)

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

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

    # retrieve motor  handles
    errorCode, left_motor_handle = vrep.simxGetObjectHandle(
        clientID, 'wheel_left_joint', vrep.simx_opmode_oneshot_wait)
    errorCode, right_motor_handle = vrep.simxGetObjectHandle(
        clientID, 'wheel_right_joint', vrep.simx_opmode_oneshot_wait)

    errorCode, handle = vrep.simxGetObjectHandle(clientID, 'Turtlebot2',
                                                 vrep.simx_opmode_oneshot_wait)
    vrep.simxSetObjectPosition(clientID, handle, -1, [((x_s / 100) - 5.1870),
                                                      ((y_s / 100) - 4.8440),
                                                      (0.06)],
                               vrep.simx_opmode_oneshot_wait)

    # print(handle)
    emptyBuff = bytearray()

    simulation_points = a_star((x_s, y_s), (x_g, y_g))

    # start the simulation -->
    init = time.time()
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)

    for i in range(0, (len(simulation_points))):

        errorcode, position = vrep.simxGetObjectPosition(
            clientID, 189, -1, vrep.simx_opmode_streaming)

        # X_Point = (((simulation_points[i][0]) / (100)) - (5.1870))
        # Y_Point = (((simulation_points[i][1]) / (100)) - (4.8440))

        ul = simulation_points[i][2]
        ur = simulation_points[i][3]

        errorCode = vrep.simxSetJointTargetVelocity(clientID,
                                                    left_motor_handle, ul,
                                                    vrep.simx_opmode_streaming)
        errorCode = vrep.simxSetJointTargetVelocity(clientID,
                                                    right_motor_handle, ur,
                                                    vrep.simx_opmode_streaming)

        # setting the sampling time for simulation. It will be the same as sampling time used in create_neighbors function
        time.sleep(3.8)

        # print("Velocities --> ", ur, ul)

    ul = 0
    ur = 0
    errorCode = vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,
                                                ul, vrep.simx_opmode_streaming)
    errorCode = vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,
                                                ur, vrep.simx_opmode_streaming)

    final = time.time()
    print("time taken for simulation --> ", final - init)
Esempio n. 50
0
        if not done:
            target = (reward + theAgent.gamma *
                      np.amax(theAgent.model.predict(next_state)[0]))
            target_f = theAgent.model.predict(state)
            target_f[0][action] = target
            theAgent.model.fit(state, target_f, epochs=1, verbose=0)
        if reward > 2:
            theAgent.epsilon_update(1)
        else:
            theAgent.epsilon_update(0)
    return


if __name__ == "__main__":
    # Intialization of the connector to V-Rep simulator
    clientID = vrep.simxStart("127.0.0.1", 19997, 1, 1, 2000, 5)
    res, objs = vrep.simxGetObjects(clientID, vrep.sim_handle_all,
                                    vrep.simx_opmode_blocking)
    if clientID > -1:
        print("Connect to Remote API server!")
    else:
        print('Failed connecting to remote API server')
        sys.exit()
    #Initializing the Robot information
    #Initializing the Learning Agent for DQN
    env = environment(
        10,
        10)  #Block number of linear velocity and the grad of angular velocity
    action_num = env.vStateNum * env.aStateNum
    states_num = len(env.getState())
    print(action_num, ' --- ', states_num)
Esempio n. 51
0
    tmp = goaldb[0][0]
    tmp = tmp.replace("[", "").replace("]", "")
    goal[i] = list(np.fromstring(tmp, dtype='float', sep=' '))

    cursor.execute('SELECT map_name FROM scenarios WHERE scenario_id = ' +
                   str(i))
    VrepMap = cursor.fetchall()
    VrepMap = "{}{}.stl".format(base_name, VrepMap[0][0])
    Map[i] = VrepMap
connect.close()

# Connect to V-rep
print('Program started')
vrep.simxFinish(-1)  # just in case, close all opened connections
clientID = vrep.simxStart(
    '127.0.0.1', 19997, True, True, -500000,
    5)  # Connect to V-REP, set a very large time-out for blocking commands
if clientID != -1:
    print('Connected to remote API server')

    emptyBuff = bytearray()

    # Start the simulation:
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)

    # Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
    #    robotHandle=retInts[0]

    # Retrieve some handles:
    res, robotHandle = vrep.simxGetObjectHandle(clientID, 'UR5#',
                                                vrep.simx_opmode_oneshot_wait)
Esempio n. 52
0
 def __init__(self):
     # 建立通信
     vrep.simxFinish(-1)
     # 每隔0.2s检测一次,直到连接上V-rep
     while True:
         self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True,
                                        5000, 5)
         if self.clientID != -1:
             break
         else:
             time.sleep(0.1)
             print("Failed connecting to remote API server!")
     print("Connection success!")
     #设置机械臂仿真步长,为保持API端与VREP端相同步长
     vrep.simxSetFloatingParameter(self.clientID,
                                   vrep.sim_floatparam_simulation_time_step,
                                   self.dt, vrep.simx_opmode_oneshot)
     # 打开同步模式
     vrep.simxSynchronous(self.clientID, True)
     vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
     # 获取句柄joint
     # 机械臂1
     self.robot1_jointHandle = np.zeros((self.jointNum, ),
                                        dtype=np.int)  # joint 句柄
     for i in range(self.jointNum):
         _, returnHandle = vrep.simxGetObjectHandle(
             self.clientID, self.jointName + str(i + 1),
             vrep.simx_opmode_blocking)
         self.robot1_jointHandle[i] = returnHandle
     # 机械臂2
     self.robot2_jointHandle = np.zeros((self.jointNum, ),
                                        dtype=np.int)  # joint 句柄
     for i in range(self.jointNum):
         _, returnHandle = vrep.simxGetObjectHandle(
             self.clientID, self.jointName + str(i + 4),
             vrep.simx_opmode_blocking)
         self.robot2_jointHandle[i] = returnHandle
     # 获取末端frame
     _, self.end_handle = vrep.simxGetObjectHandle(
         self.clientID, 'end', vrep.simx_opmode_blocking)
     _, self.end0_handle = vrep.simxGetObjectHandle(
         self.clientID, 'end0', vrep.simx_opmode_blocking)
     _, self.goal1_handle = vrep.simxGetObjectHandle(
         self.clientID, 'goal_1', vrep.simx_opmode_blocking)
     _, self.goal2_handle = vrep.simxGetObjectHandle(
         self.clientID, 'goal_2', vrep.simx_opmode_blocking)
     # 俩机械臂间距
     _, self.dist_handle = vrep.simxGetDistanceHandle(
         self.clientID, 'robots_dist', vrep.simx_opmode_blocking)
     _, self.end_dis_handle = vrep.simxGetDistanceHandle(
         self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
     _, self.end0_dis_handle = vrep.simxGetDistanceHandle(
         self.clientID, 'robot2_goal', vrep.simx_opmode_blocking)
     # 获取link句柄
     self.link_handle1 = np.zeros((self.jointNum, ), dtype=np.int)  #link 句柄
     for i in range(self.jointNum):
         _, returnHandle = vrep.simxGetObjectHandle(
             self.clientID, self.linkName + str(i + 1),
             vrep.simx_opmode_blocking)
         self.link_handle1[i] = returnHandle
     self.link_handle2 = np.zeros((self.jointNum, ),
                                  dtype=np.int)  # link 句柄
     for i in range(self.jointNum):
         _, returnHandle = vrep.simxGetObjectHandle(
             self.clientID, self.linkName + str(i + 4),
             vrep.simx_opmode_blocking)
         self.link_handle2[i] = returnHandle
     print('Handles available!')
 def start_connection(self, ip, port):
     vrep.simxFinish(-1) 
     clientID = vrep.simxStart(ip, port, True, True, 5000, 5) 
     vrep.simxSynchronous(clientID, True)
     return clientID
Esempio n. 54
0
#downward_precision = 1e-4
#
## in degree
#ore_x_peg = 0
#ore_y_peg = 10
#ore_z_peg = 0

#######################

x_range = np.arange(x_start, x_end, x_delta)
y_range = np.arange(y_start, y_end, y_delta)

print('Program started. IP: {0}, Port:{1}'.format(remoteIP, remotePort))

vrep.simxFinish(-1)  # just in case, close all opened connections
clientID = vrep.simxStart(remoteIP, remotePort, True, True, 5000,
                          5)  # Connect to V-REP

if clientID == -1:
    print('Failed connecting to remote API server')
    sys.exit(-1)

print('Connected to remote API server, client ID: {0}'.format(clientID))

emptyBuff = bytearray()

# Delete all previous models
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
    clientID, 'remoteApiCommandServer',
    vrep.sim_scripttype_customizationscript, 'clearObjects_function', [], [],
    [], emptyBuff, vrep.simx_opmode_blocking)
def openPort():
    global clientID
    # close any open connections
    vrep.simxFinish(-1)
    # Connect to the V-REP continuous server
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
Esempio n. 56
0
import vrep

vrep.simxFinish(-1)  # 关掉之前的连接
clientId = vrep.simxStart("127.0.0.1", 19997, True, True, 5000, 5)  #建立和服务器的连接
if clientId != -1:  #连接成功
    print('connect successfully')
else:
    print('connect failed')
vrep.simxFinish(clientId)
print('program ended')
Esempio n. 57
0
    def __enter__(self):
        print('[ROBOTENV] Starting environment...')

        sync_mode_str = 'TRUE' if self.sync_mode else 'FALSE'
        portNb_str = str(self.portNb)

        # launch v-rep
        vrep_cmd = [
            self.vrepPath, '-gREMOTEAPISERVERSERVICE_' + portNb_str +
            '_FALSE_' + sync_mode_str
        ]
        if not self.showGUI:
            vrep_cmd.append('-h')  # headless mode
        vrep_cmd.append(self.scenePath)

        # headless mode via ssh
        #     vrep_cmd = "xvfb-run --auto-servernum --server-num=1 /homes/dam416/V-REP_PRO_EDU_V3_4_0_Linux/vrep.sh -h -s -q MicoRobot.ttt"
        # vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', '-s', '-q', self.scenePath]
        # vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', self.scenePath]
        print('[ROBOTENV] Launching V-REP...')
        # NOTE: do not use "stdout=subprocess.PIPE" below to buffer logs, causes deadlock at episode 464! (flushing the buffer may work... but buffering is not needed)
        self.vrepProcess = subprocess.Popen(vrep_cmd,
                                            shell=False,
                                            preexec_fn=os.setsid)
        # connect to V-Rep Remote Api Server
        vrep.simxFinish(-1)  # close all opened connections
        # Connect to V-REP
        print('[ROBOTENV] Connecting to V-REP...')
        counter = 0
        while True:
            self.clientID = vrep.simxStart('127.0.0.1', self.portNb, True,
                                           False, 5000, 0)
            if self.clientID != -1:
                break
            else:
                print("[ROBOTENV] Connection failed, retrying")
                counter += 1
                if counter == 10:
                    raise RuntimeError(
                        '[ROBOTENV] Connection to V-REP failed.')

        if self.clientID == -1:
            print('[ROBOTENV] Failed connecting to remote API server')
        else:
            print('[ROBOTENV] Connected to remote API server')

            # close model browser and hierarchy window
            vrep.simxSetBooleanParameter(self.clientID,
                                         vrep.sim_boolparam_browser_visible,
                                         False, vrep.simx_opmode_blocking)
            vrep.simxSetBooleanParameter(self.clientID,
                                         vrep.sim_boolparam_hierarchy_visible,
                                         False, vrep.simx_opmode_blocking)
            vrep.simxSetBooleanParameter(self.clientID,
                                         vrep.sim_boolparam_console_visible,
                                         False, vrep.simx_opmode_blocking)

            # load scene
            # time.sleep(5) # to avoid errors
            # returnCode = vrep.simxLoadScene(self.clientID, self.scenePath, 1, vrep.simx_opmode_oneshot_wait) # vrep.simx_opmode_blocking is recommended

            # # Start simulation
            # # vrep.simxSetIntegerSignal(self.clientID, 'dummy', 1, vrep.simx_opmode_blocking)
            # # time.sleep(5)  #to center window for recordings
            # if self.initial_joint_positions is not None:
            #     self.initializeJointPositions(self.initial_joint_positions)
            # self.startSimulation()
            # # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
            # # printlog('simxStartSimulation', returnCode)

            # get handles and start streaming distance to goal
            for i in range(0, self.nSJoints):
                returnCode, self.jointHandles[i] = vrep.simxGetObjectHandle(
                    self.clientID, 'Mico_joint' + str(i + 1),
                    vrep.simx_opmode_blocking)
            printlog('simxGetObjectHandle', returnCode)
            returnCode, self.fingersH1 = vrep.simxGetObjectHandle(
                self.clientID, 'MicoHand_fingers12_motor1',
                vrep.simx_opmode_blocking)
            returnCode, self.fingersH2 = vrep.simxGetObjectHandle(
                self.clientID, 'MicoHand_fingers12_motor2',
                vrep.simx_opmode_blocking)
            returnCode, self.goalCubeH = vrep.simxGetObjectHandle(
                self.clientID, 'GoalCube', vrep.simx_opmode_blocking)
            returnCode, self.targetCubePositionH = vrep.simxGetObjectHandle(
                self.clientID, 'GoalPosition', vrep.simx_opmode_blocking)
            returnCode, self.robotBaseH = vrep.simxGetObjectHandle(
                self.clientID, 'Mico_link1_visible', vrep.simx_opmode_blocking)
            returnCode, self.jointsCollectionHandle = vrep.simxGetCollectionHandle(
                self.clientID, "sixJoints#", vrep.simx_opmode_blocking)
            returnCode, self.distToGoalHandle = vrep.simxGetDistanceHandle(
                self.clientID, "distanceToGoal#", vrep.simx_opmode_blocking)
            returnCode, self.endEffectorH = vrep.simxGetObjectHandle(
                self.clientID, "DummyFinger#", vrep.simx_opmode_blocking)
            # returnCode, self.distanceToGoal = vrep.simxReadDistance(self.clientID, self.distToGoalHandle, vrep.simx_opmode_streaming) #start streaming
            # returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(self.clientID, self.jointsCollectionHandle, 15, vrep.simx_opmode_streaming) #start streaming

            if self.targetCubePosition is not None:
                # hide goal position plane
                visibility_layer_param_ID = 10
                visible_layer = 1
                returnCode = vrep.simxSetObjectIntParameter(
                    self.clientID, self.targetCubePositionH,
                    visibility_layer_param_ID, visible_layer,
                    vrep.simx_opmode_blocking)
                # print('simxSetObjectIntParameter return Code: ', returnCode)
                self.initializeGoalPosition()

            # Start simulation
            if self.initial_joint_positions is not None:
                self.initializeJointPositions()

            self.reset()

            # self.startSimulation()
            # # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
            # # printlog('simxStartSimulation', returnCode)

            # self.updateState()  # default initial state: 180 degrees (=pi radians) for all angles

            # check the state is valid
            # while True:
            #     self.updateState()
            #     print("\nstate: ", self.state)
            #     print("\nreward: ", self.reward)
            #     # wait for a state
            #     if (self.state.shape == (1,self.observation_space_size) and abs(self.reward) < 1E+5):
            #         print("sent reward3=", self.reward)
            #         break

            print(
                '[ROBOTENV] Environment succesfully initialised, ready for simulations'
            )
            # while vrep.simxGetConnectionId(self.clientID) != -1:
            #     ##########################EXECUTE ACTIONS AND UPDATE STATE HERE #################################
            #     time.sleep(0.1)
            #     #end of execution loop
            return self
#coding: utf-8
import time
import vrep
import keras
import math
import localization
import numpy as np
from keras.models import Model
from keras.models import load_model

serverIP = "127.0.0.1"
serverPort = 19999
#---------------------Conecta no servidor---------------------------------
clientID = vrep.simxStart(serverIP, serverPort, True, True, 2000, 5)
nomeSensor = []
sensorHandle = []
dist = []
leftMotorHandle = 0
rightMotorHandle = 0
global v_Left, v_Right, tacoDir, tacoEsq
v_Left = 1
v_Right = 1

if (clientID!=-1):
	print ("Servidor Conectado!")

#------------------------------Inicializa Sensores ----------------------------
	for i in range(0,8):
		nomeSensor.append("sensor" + str(i+1))

		res, handle = vrep.simxGetObjectHandle(clientID, nomeSensor[i], vrep.simx_opmode_oneshot_wait)
Esempio n. 59
0
try:
    import vrep
except:
    print ('--------------------------------------------------------------')
    print ('"vrep.py" could not be imported. This means very probably that')
    print ('either "vrep.py" or the remoteApi library could not be found.')
    print ('Make sure both are in the same folder as this file,')
    print ('or appropriately adjust the file "vrep.py"')
    print ('--------------------------------------------------------------')
    print ('')

import time

print ('Program started')
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')
    
    # Now try to retrieve data in a blocking fashion (i.e. a service call):
    res,objs=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_blocking)
    if res==vrep.simx_return_ok:
        print ('connect success')
        current_filename = os.path.realpath(__file__)
        vrep.simxAddStatusbarMessage(clientID,'connected from '+ current_filename,
                                     vrep.simx_opmode_oneshot)
    else:
        print ('Remote API function call returned with error code: ',res)
    
    res,quadrotor_base = vrep.simxGetObjectHandle(clientID,"Quadricopter_base", vrep.simx_opmode_oneshot_wait)
    _,quadrotor_target = vrep.simxGetObjectHandle(clientID, "Quadricopter_target", vrep.simx_opmode_oneshot_wait)
def quadcopter_fcn():
    try:
        import vrep
    except:
        print('--------------------------------------------------------------')
        print('"vrep.py" could not be imported. This means very probably that')
        print('either "vrep.py" or the remoteApi library could not be found.')
        print('Make sure both are in the same folder as this file,')
        print('or appropriately adjust the file "vrep.py"')
        print('--------------------------------------------------------------')
        print('')

    import time
    import numpy as np

    # output limits:
    #min_output=0
    #max_output=8.335
    # program parameters:
    #global variables:
    cumul = 0
    last_e = 0
    pAlphaE = 0
    pBetaE = 0
    psp2 = 0
    psp1 = 0
    prevEuler = 0

    cumulAlpha = 0
    cumulBeta = 0

    cumulAlphaPos = 0
    cumulBetaPos = 0

    particlesTargetVelocities = [0, 0, 0, 0]
    #speed weight:
    vParam = -2
    #parameters for vertical control
    Kpv = 2
    Kiv = 0
    Kdv = 2
    #parameters for horizontal control
    Kph = 0.4
    Kih = 0.1
    Kdh = 1.5
    Kph_pos1 = 0.4
    Kih_pos1 = 0.001
    Kdh_pos1 = 0.05
    Kph_pos0 = 0.4
    Kih_pos0 = 0.001
    Kdh_pos0 = 0.05
    #parameters for rotational control:
    Kpr = 0.05
    Kir = 0
    Kdr = 0.9

    print('Program started')
    vrep.simxFinish(-1)  # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
                              5)  # Connect to V-REP

    if clientID != -1:
        print('Connected to remote API server')

        # enable the synchronous mode on the client:
        vrep.simxSynchronous(clientID, True)

        # start the simulation:
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

        #functional/handle code:
        emptyBuff = bytearray()
        [returnCode,
         targetObj] = vrep.simxGetObjectHandle(clientID, 'Quadricopter_target',
                                               vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle] = vrep.simxGetObjectHandle(clientID,
                                                    'Quadricopter_base',
                                                    vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle] = vrep.simxGetObjectHandle(clientID, 'Quadricopter',
                                               vrep.simx_opmode_blocking)

        # main loop:
        while True:
            # vertical control:
            [returnCode,
             targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
                                                     vrep.simx_opmode_blocking)
            [returnCode,
             pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)
            [returnCode, l,
             w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
                                             vrep.simx_opmode_blocking)

            e = targetPos[2] - pos[2]
            cumul = cumul + e
            diff_e = e - last_e
            Pvert = Kpv * e
            Ivert = Kiv * cumul
            Dvert = Kdv * diff_e
            thrust = 5.335 + Pvert + Ivert + Dvert + l[2] * vParam  # get thrust
            last_e = e
            # horizontal control:
            [returnCode,
             sp] = vrep.simxGetObjectPosition(clientID, targetObj,
                                              qc_base_handle,
                                              vrep.simx_opmode_blocking)
            [rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
                clientID, 'Quadricopter', vrep.sim_scripttype_childscript,
                'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
                clientID, 'Quadricopter', vrep.sim_scripttype_childscript,
                'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, rc, rc,
             rc] = vrep.simxCallScriptFunction(clientID, 'Quadricopter',
                                               vrep.sim_scripttype_childscript,
                                               'qc_Get_Object_Matrix', [], [],
                                               [], emptyBuff,
                                               vrep.simx_opmode_blocking)
            [errorCode,
             M] = vrep.simxGetStringSignal(clientID, 'mtable',
                                           vrep.simx_opmode_oneshot_wait)
            if (errorCode == vrep.simx_return_ok):
                m = vrep.simxUnpackFloats(M)

            alphaE = vy[2] - m[11]
            cumulAlpha = cumulAlpha + alphaE
            diff_alphaE = alphaE - pAlphaE
            alphaCorr = Kph * alphaE + Kih * cumulAlpha + Kdh * diff_alphaE  #alpha correction

            betaE = vx[2] - m[11]
            cumulBeta = cumulBeta + betaE
            diff_betaE = betaE - pBetaE
            betaCorr = -Kph * betaE - Kih * cumulBeta - Kdh * diff_betaE  #beta correction

            pAlphaE = alphaE
            pBetaE = betaE

            cumulAlphaPos = cumulAlphaPos + sp[1]
            cumulBetaPos = cumulBetaPos + sp[0]
            alphaPos = Kph_pos1 * (
                sp[1]) + Kih_pos1 * cumulAlphaPos + Kdh_pos1 * (
                    sp[1] - psp2)  #alpha position correction
            betaPos = Kph_pos0 * (
                sp[0]) + Kih_pos0 * cumulBetaPos + Kdh_pos0 * (
                    sp[0] - psp1)  #beta position correction

            alphaCorr = alphaCorr + alphaPos
            betaCorr = betaCorr - betaPos

            psp2 = sp[1]
            psp1 = sp[0]
            # rotational control:
            [returnCode,
             euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
                                                    qc_base_handle,
                                                    vrep.simx_opmode_blocking)
            [returnCode, orientation
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)

            Prot = Kpr * euler[2]
            Drot = Kdr * (euler[2] - prevEuler)
            rotCorr = Prot + Drot
            prevEuler = euler[2]

            # set propeller velocities:
            propeller1_PTV = thrust * (1 - alphaCorr + betaCorr - rotCorr)
            propeller2_PTV = thrust * (1 - alphaCorr - betaCorr + rotCorr)
            propeller3_PTV = thrust * (1 + alphaCorr - betaCorr - rotCorr)
            propeller4_PTV = thrust * (1 + alphaCorr + betaCorr + rotCorr)
            particlesTargetVelocities = [
                propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV
            ]

            print '////////////'
            print '------------'
            print '-Controller parameters-'
            print 'Vertical control parameters=', [Kpv, Kiv, Kdv]
            print 'Horizontal control parameters=', [
                Kph, Kih, Kdh, Kph_pos0, Kdh_pos0, Kph_pos1, Kdh_pos1
            ]
            print 'Rotational control parameters=', [Kpr, Kir, Kdr]
            print '------------'
            print '-Vertical component-'
            print 'targetPos=', targetPos
            print 'pos=', pos
            print 'e=', e
            print 'thrust=', thrust
            print '------------'
            print '-Horizontal component-'
            print 'cumulAlpha=', cumulAlpha
            print 'cumulBeta=', cumulBeta
            print 'cumulAlphaPos=', cumulAlphaPos
            print 'cumulBetaPos=', cumulBetaPos
            print 'alphaE=', alphaE
            print 'betaE=', betaE
            print 'alphaCorr=', alphaCorr
            print 'betaCorr=', betaCorr
            print 'orientation=', orientation
            print 'sp_X=', sp[0]
            print 'sp_Y=', sp[1]
            print '------------'
            print '-Rotational component-'
            print 'vx=', vx
            print 'vy=', vy
            print 'gamma=', euler[2]
            print 'rotCorr=', rotCorr
            print '------------'
            print 'Velocity_propeller_1 = ', particlesTargetVelocities[0]
            print 'Velocity_propeller_2 = ', particlesTargetVelocities[1]
            print 'Velocity_propeller_3 = ', particlesTargetVelocities[2]
            print 'Velocity_propeller_4 = ', particlesTargetVelocities[3]
            print '------------'
            print '////////////'

            # send propeller velocities to output:
            [res, retInts, retFloats, retStrings,
             retBuffer] = vrep.simxCallScriptFunction(
                 clientID, 'Quadricopter', vrep.sim_scripttype_childscript,
                 'qc_propeller_v', [], particlesTargetVelocities, [],
                 emptyBuff, vrep.simx_opmode_blocking)
            vrep.simxSynchronousTrigger(clientID)

        # stop the simulation:
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

        # Now close the connection to V-REP:
        vrep.simxFinish(clientID)

    else:
        print('Failed connecting to remote API server')