コード例 #1
0
ファイル: Simulation.py プロジェクト: Etragas/GPSDrone
    def restart(self,earlyStop = False):
        if (self.cid != None):
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
        vrep.simxFinish(-1)  # just in case, close all opened connections
        time.sleep(1)
        self.connect()
        time.sleep(1)

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

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

        err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
        self.getTargetStart()
        for i in range(4):
            self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
                                                            'Quadricopter_propeller_respondable' + str(i) + str(1),
                                                            self.mode())
コード例 #2
0
    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))
コード例 #3
0
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)
    for move in startMoves:
        JointControl(clientID, 0, Body, move)
        time.sleep(0.1)
コード例 #4
0
ファイル: robot.py プロジェクト: anandsaha/stash
 def start_sim(self):
     time.sleep(self.sleep_sec_min)
     vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
     self.gripper_enabled = False
     self.__update_all_object_positions()
     self.cylinder_height = self.get_object_height(self.cylinder_handle)
     self.cylinder_z_locus = self.get_position(self.cylinder_handle)[2]
     self.bin_position = self.get_position(self.bin_handle)
コード例 #5
0
ファイル: Simulation.py プロジェクト: Etragas/GPSDrone
 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()
コード例 #6
0
ファイル: ePuckVRep.py プロジェクト: RL-LDV-TUM/epuck-vrep
    def startsim(self):
        """
        Start the simulation in synchronous mode to achieve exact simulation
        independent of the framerate. To be able to do this, you have to 
        connec to the port 19997 of vrep, edit the file
        `remoteApiConnections.txt` to contain something like

            portIndex1_port           = 19997
            portIndex1_debug          = true
            portIndex1_syncSimTrigger = false

        """
        self.set_led(8,0)
        vrep.simxStartSimulation(self._clientID, vrep.simx_opmode_oneshot)
        vrep.simxSynchronous(self._clientID, True)
        self.stepsim(1)
コード例 #7
0
def wait_until_simulator_started(clientID):
    while True:
        try:
            if vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) == vrep.simx_return_ok:
                break
        except KeyboardInterrupt:
            sys.exit('Program Ended')
コード例 #8
0
ファイル: robots.py プロジェクト: bjkomer/nengo_vrep
    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
コード例 #9
0
ファイル: Simulator.py プロジェクト: PatriciaPolero/lucy
 def startSim(self, clientID, screen=True):
     vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_dynamic_engine,bulletEngine ,vrep.simx_opmode_oneshot_wait)
     vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, simulatioTimeStepDT, vrep.simx_opmode_oneshot_wait)
     error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
     if not screen:
         vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait)
         #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
     return error
コード例 #10
0
ファイル: SimulatorBackend.py プロジェクト: dtbinh/Homeo
 def reset(self):
     """In VREP we reset a simulation by stopping and restarting it"""
     eCode = vrep.simxStopSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)  
     if eCode != 0:
         raise Exception("Could not stop VREP simulation")
     eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)   
     if eCode != 0:
         raise Exception("Could not start VREP simulation")
     vrep.simxSynchronousTrigger(self._VREP_clientId)
コード例 #11
0
    def run(self):

        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)

            t0 = time.time()
            while time.time() - t0 < 30:
                for bot in self.bots:
                    bot.robotCode()
コード例 #12
0
ファイル: vRepSimulator.py プロジェクト: bchappet/dnfpy
 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 __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
コード例 #14
0
ファイル: quadcopter.py プロジェクト: Etragas/GPSDrone
 def reset( self ):
     err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     time.sleep(1)
     self.pos_err = [0,0,0]
     self.ori_err = [0,0,0]
     self.lin = [0,0,0]
     self.ang = [0,0,0]
     err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     if SYNC:
       vrep.simxSynchronous( self.cid, True )
コード例 #15
0
    def run(self):
        if self.clientID!=-1:
            if not self.multirobots:
                # Start simulation if MultiRobotRunner not already starting it
                _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
            self.robotCode()

        if not self.multirobots:
            # Stop VREP simulation cleanly if MultiRobotRunner not already stopping it
            self.clean_exit()
コード例 #16
0
ファイル: VRepBRSimulator.py プロジェクト: dtbinh/Lingadrome
 def resetSimulation(self):
     returnCode = vrep.simx_return_novalue_flag
     while returnCode!=vrep.simx_return_ok:
         returnCode=vrep.simxStopSimulation(self.__clientID, vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(0.5)
     returnCode = vrep.simx_return_novalue_flag
     while returnCode!=vrep.simx_return_ok:
         returnCode=vrep.simxStartSimulation(self.__clientID, vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(1.0)
コード例 #17
0
	def reset_rob(self):
		"""
			Sets the bubbleRob to his starting position; mind the hack, simulation has to be stopped 
		"""

		##### Set absolute position

		#stop simulation
		vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) 

		#100ms delay, this is a hack because server isn't ready either
		time.sleep(0.3)

		#set on absolute position
		err = vrep.simxSetObjectPosition(self.clientID, self.bubbleRobHandle, -1, self.bubbleRobStartPosition, vrep.simx_opmode_oneshot_wait)

		#start simulation again
		vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)

		time.sleep(0.3)
コード例 #18
0
ファイル: environment.py プロジェクト: dtbinh/RL-on-VREP
 def start(self):
     returnCode = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
     if (returnCode>1):
         print "returnCode: ", returnCode
         raise Exception('Could not start')
     returnCode=vrep.simxSynchronous(self.clientID,True)
     
     for k in range(10): #Run to steps to step through initial "drop"
         returncode = vrep.simxSynchronousTrigger(self.clientID)
         
     if (returnCode!=0):
         raise Exception('Could not set synchronous mode')
コード例 #19
0
ファイル: robots.py プロジェクト: bjkomer/nengo_vrep
 def reset( self ):
     err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     time.sleep(1)
     self.pos_err = [0,0,0]
     self.ori_err = [0,0,0]
     self.lin = [0,0,0]
     self.ang = [0,0,0]
     self.vert_prox = 0
     self.left_prox = 0
     self.right_prox = 0
     err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     if self.sync:
       vrep.simxSynchronous( self.cid, True )
コード例 #20
0
    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)
コード例 #21
0
ファイル: ProxSensorTest.py プロジェクト: dtbinh/Homeo
 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")
コード例 #22
0
    def run(self):
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)

            # in zone passer, it was
            # get to the 3 starting positions
            # while game is still going...
            # do things in a while loop

            while True:
                t=time.time()
                for bot in self.bots:
                    bot.robotCode()
                print 'loop time'
                print time.time()-t
コード例 #23
0
ファイル: Simulator.py プロジェクト: aguirrea/lucy
    def startSim(self, clientID, screen=True):
        #I need the simulator stopped in order to be started
        retSimStop = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
        if retSimStop != simx_return_ok :
            print "simulation couldnt be stopped!"
        else:
            print "simulation stopped!"

        #setting the physics engine
        retSetPhyEngine = vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_dynamic_engine, bulletEngine, vrep.simx_opmode_oneshot_wait)
        if retSetPhyEngine != simx_return_ok:
            print "unable to set the physical engine"
        else:
            print "physical engine correctly setted"

        if int(self.sysConf.getProperty("physics enable?"))==1:
            vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot)
        else:
            vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot)

        #settig simulation speed
        if self.speedmodifier > 0:
            vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait)

        #settig simulation step
        retSetTimeStep = vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, self.simulationTimeStepDT, vrep.simx_opmode_oneshot_wait)
        if retSetTimeStep != simx_return_ok :
            print "problems setting time step"
        else:
            print "time step setted!"

        #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_realtime_simulation,1,vrep.simx_opmode_oneshot_wait)

        #sync mode configuration
        if self.synchronous:
            vrep.simxSynchronous(clientID,True)

        #light mode configuration
        if not screen:
            vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait)
            #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)


        #start simulation
        error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
        if int(self.sysConf.getProperty("blank screen?"))==1:
            vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
        return error
コード例 #24
0
ファイル: SimulatorBackend.py プロジェクト: dtbinh/Homeo
 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 
コード例 #25
0
    def run(self):
        """ Run method is a Cyclic Exectuor.
        robots that are added with `addRobot` should have method
        `robotCode` which returns in a small amount of time

        If you desire high level planner control, use the structure
        as a template.
        """
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
            t0 = time.time()
            while True:
                for bot in self.bots:
                    bot.robotCode()
                self.ballEngine.update()

        self.clean_exit()
コード例 #26
0
ファイル: VREPDetermTest.py プロジェクト: dtbinh/Homeo
    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")
コード例 #27
0
    def run(self):
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
            # Create threads for each
            self.threads = []
            for bot in self.bots:
                # Robot Thread
                t = threading.Thread(
                      name="%sThread" % bot.bot_name
                    , target=bot.robotCode)
                t.setDaemon(True) # Require it to join!
                self.threads.append(t)
                t.start()
                print("Started robotCode for %s" % bot.bot_name)

        # Require threads to join before exiting
        for t in self.threads:
            t.join()
        self.clean_exit()
コード例 #28
0
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,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)
コード例 #30
0
def callback(msg):

    global count
    global dt
    global _

    if clientID != -1 and count == 0:  # if we connected successfully
        print('Connected to remote API server')

        # --------------------- Setup the simulation

        vrep.simxSynchronous(clientID, True)

        global joint_names

        # joint target velocities discussed below
        global joint_target_velocities
        joint_target_velocities = np.ones(len(joint_names)) * 10000.0

        global joint_handles

        # get the handles for each joint and set up streaming
        joint_handles = [
            vrep.simxGetObjectHandle(clientID, name,
                                     vrep.simx_opmode_blocking)[1]
            for name in joint_names
        ]

        # get handle for target and set up streaming
        global target_handle
        _, target_handle = vrep.simxGetObjectHandle(clientID, 'target',
                                                    vrep.simx_opmode_blocking)

        vrep.simxSetFloatingParameter(
            clientID,
            vrep.sim_floatparam_simulation_time_step,
            dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        # --------------------- Start the simulation

        # start our simulation in lockstep with our code
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
        print("Starting Simulation")

    if clientID != -1 and count >= 0 and count < 6:  # run for 1 simulated second

        target_xyz = [msg.x, msg.y, msg.z]
        # store for plotting
        global track_target

        track_target.append(np.copy(target_xyz))

        # get the (x,y,z) position of the target
        vrep.simxSetObjectPosition(clientID, target_handle, -1, target_xyz,
                                   vrep.simx_opmode_blocking)
        if _ != 0: raise Exception("Deu ruim parceiro")

        target_xyz = np.asarray(target_xyz)

        q = np.zeros(len(joint_handles))
        dq = np.zeros(len(joint_handles))
        for ii, joint_handle in enumerate(joint_handles):
            # get the joint angles
            _, q[ii] = vrep.simxGetJointPosition(clientID, joint_handle,
                                                 vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()
            # get the joint velocity
            _, dq[ii] = vrep.simxGetObjectFloatParameter(
                clientID,
                joint_handle,
                2012,  # parameter ID for angular velocity of the joint
                vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()

        L = np.array([0, .42, .225])  # arm segment lengths

        xyz = np.array([L[0]*np.cos(q[0])+L[1]*np.cos(q[0])*np.cos(q[1])+L[2]*np.cos(q[0])*np.cos(q[1])*np.cos(q[2])- \
L[2]*np.cos(q[0])*np.sin(q[1])*np.sin(q[2]),
            L[0]*np.cos(q[0])+L[1]*np.cos(q[1])*np.sin(q[0])+L[2]*np.sin(q[0])*np.cos(q[1])*np.cos(q[2])- \
L[2]*np.sin(q[0])*np.sin(q[1])*np.sin(q[2]),
                       # have to add .1 offset to z position
                   L[1]*np.sin(q[1])+L[2]*(np.cos(q[1])*np.sin(q[2])+np.cos(q[2])*np.sin(q[1]))+.14])

        global track_hand

        track_hand.append(np.copy(xyz))  #store for plotting

        # calculate the Jacobian for the hand
        JEE = np.zeros((3, 3))
        JEE[0, 2] = -L[2] * np.sin(q[1] + q[2]) * np.cos(q[0])
        JEE[1, 2] = -L[2] * np.sin(q[1] + q[2]) * np.sin(q[0])
        JEE[2, 2] = L[2] * np.cos(q[1] + q[2])
        JEE[0, 1] = -np.cos(q[0]) * (L[2] * np.sin(q[1] + q[2]) +
                                     L[1] * np.sin(q[1]))
        JEE[1, 1] = -np.sin(q[0]) * (L[2] * np.sin(q[1] + q[2]) +
                                     L[1] * np.sin(q[1]))
        JEE[2, 1] = L[2] * np.cos(q[1] + q[2]) + L[1] * np.cos(q[1])
        JEE[0, 0] = -np.sin(q[0]) * (L[0] + JEE[2][1])
        JEE[1, 0] = np.cos(q[0]) * (L[0] + JEE[2][1])

        # get the Jacobians for the centres-of-mass for the arm segments
        JCOM1 = np.zeros((6, 3))

        JCOM1[0, 0] = L[0] * -np.sin(q[0]) / 2
        JCOM1[1, 0] = L[0] * np.cos(q[0]) / 2
        JCOM1[5, 0] = 1.0

        JCOM2 = np.zeros((6, 3))

        JCOM2[0, 1] = L[1] * (np.cos(q[1]) * np.sin(q[0]) -
                              np.cos(q[0]) * np.sin(q[1])) / 2
        JCOM2[1, 1] = L[1] * (-np.cos(q[1]) * np.cos(q[0]) -
                              np.sin(q[1]) * np.sin(q[0])) / 2
        JCOM2[4, 1] = 1.0
        JCOM2[0, 0] = L[1] * (np.cos(q[0]) * np.sin(q[1]) - np.cos(q[1]) *
                              np.sin(q[0])) / 2 - L[0] * np.sin(q[0])
        JCOM2[1,
              0] = L[1] * (np.sin(q[0]) * np.sin(q[1]) +
                           np.cos(q[0]) * np.cos(q[1])) / 2 + 2 * JCOM1[1][0]
        JCOM2[4, 0] = 1.0

        JCOM3 = np.zeros((6, 3))

        JCOM3[0, 2] = L[2] * (np.cos(q[2]) * np.sin(q[0]) -
                              np.cos(q[0]) * np.cos(q[1]) * np.sin(q[2])) / 2
        JCOM3[1, 2] = L[2] * (-np.cos(q[2]) * np.cos(q[0]) -
                              np.cos(q[1]) * np.sin(q[0]) * np.sin(q[2])) / 2
        JCOM3[2, 2] = -L[2] * np.sin(q[1]) * np.sin(q[2]) / 2
        JCOM3[4, 2] = 1.0
        JCOM3[0, 1] = L[2] * -np.cos(q[0]) * np.cos(q[2]) * np.sin(
            q[1]) / 2 - L[1] * np.cos(q[0]) * np.sin(q[1])
        JCOM3[1, 1] = L[2] * -np.cos(q[2]) * np.sin(q[0]) * np.sin(
            q[1]) / 2 - L[1] * np.sin(q[0]) * np.sin(q[1])
        JCOM3[2, 1] = (L[2] * np.cos(q[1]) * np.cos(q[2]) /
                       2) + L[1] * np.cos(q[1])
        JCOM3[4, 1] = 1.0
        JCOM3[0,0] = L[2]*(np.cos(q[0])*np.sin(q[2]) - np.cos(q[1])*np.cos(q[2])*np.sin(q[0]))/2 -L[1]*np.cos(q[1])*np.sin(q[0])- \
                        L[0]*np.sin(q[0])
        JCOM3[1,0] = L[2]*(np.sin(q[0])*np.sin(q[2]) + np.cos(q[0])*np.cos(q[1])*np.cos(q[2]))/2 + \
L[1]*np.cos(q[0])*np.cos(q[1])+2*JCOM1[1][0]
        JCOM3[4, 0] = 1.0

        m1 = 0.15  #from VREP
        M1 = np.diag([m1, m1, m1, .001, .001, .001])
        m2 = 1.171  # from VREP
        M2 = np.diag([m1, m1, m1, .008, .008, .001])
        m3 = .329  # from VREP
        M3 = np.diag([m2, m2, m2, 5e-4, 5e-4, 1e-4])

        # generate the mass matrix in joint space
        Mq = np.dot(JCOM1.T, np.dot(M1, JCOM1)) + \
             np.dot(JCOM2.T, np.dot(M2, JCOM2)) + \
np.dot(JCOM3.T, np.dot(M3, JCOM3))

        # compensate for gravity
        gravity = np.array([
            0,
            0,
            -9.81,
            0,
            0,
            0,
        ])
        Mq_g = np.dot(JCOM1.T, np.dot(M1, gravity)) + \
                np.dot(JCOM2.T, np.dot(M2, gravity)) + \
  np.dot(JCOM2.T, np.dot(M2, gravity))

        Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T))
        Mu, Ms, Mv = np.linalg.svd(Mx_inv)
        # cut off any np.singular values that could cause control problems
        for i in range(len(Ms)):
            Ms[i] = 0 if Ms[i] < 1e-5 else 1. / float(Ms[i])
    # numpy returns U,S,V.T, so have to transpose both here
        Mx = np.dot(Mv.T, np.dot(np.diag(Ms), Mu.T))

        # calculate desired movement in operational (hand) space
        kp = 100
        kv = np.sqrt(kp)
        u_xyz = np.dot(Mx, kp * (target_xyz - xyz))

        u = np.dot(JEE.T, u_xyz) - np.dot(Mq, kv * dq) - Mq_g
        u *= -1  # because the joints on the arm are backwards

        for ii, joint_handle in enumerate(joint_handles):
            # the way we're going to do force control is by setting
            # the target velocities of each joint super high and then
            # controlling the max torque allowed (yeah, i know)

            # get the current joint torque
            _, torque = \
                vrep.simxGetJointForce(clientID,
                        joint_handle,
                        vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()

            # if force has changed signs,
            # we need to change the target velocity sign
            if np.sign(torque) * np.sign(u[ii]) < 0:
                joint_target_velocities[ii] = \
                        joint_target_velocities[ii] * -1
                vrep.simxSetJointTargetVelocity(
                    clientID,
                    joint_handle,
                    joint_target_velocities[ii],  # target velocity
                    vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()

            # and now modulate the force
            vrep.simxSetJointForce(
                clientID,
                joint_handle,
                abs(u[ii]),  # force to apply
                vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()

    # move simulation ahead one time step
        vrep.simxSynchronousTrigger(clientID)
        count += dt
        print("Cheguei aqui uma vez")
    elif clientID != -1 and count >= 6:
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

        # Before closing the connection to V-REP,
        #make sure that the last command sent out had time to arrive.
        vrep.simxGetPingTime(clientID)

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

    else:
        raise Exception('Failed connecting to remote API server')

    return
コード例 #31
0
ファイル: robotenv.py プロジェクト: dkanou/curriculumlearning
 def startSimulation(self):
     # make sure simulation is stopped, stop if needed
     self.stop_if_needed()
     vrep.simxSynchronous(self.clientID, self.sync_mode)
     vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
コード例 #32
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)
コード例 #33
0
ファイル: hexapod.py プロジェクト: suhasjs/hops-framework
    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()
コード例 #34
0
ファイル: main.py プロジェクト: umd-agrc/QuadRL
def reset(clientID):
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
    time.sleep(0.1)
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
コード例 #35
0
ファイル: simulator.py プロジェクト: Ye-Hanyu/dex-net-silvia
 def start(self):
     vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
コード例 #36
0
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')
コード例 #37
0
ファイル: robot.py プロジェクト: pbustos/vrep-simulator
 def start_simulation(self):
     #start the simulation
     errorCode = vrep.simxStartSimulation(self.clientID,
                                          vrep.simx_opmode_blocking)
     assert errorCode == 0, "Simulation could not be started"
     return
コード例 #38
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")
コード例 #39
0
    def evoluer(self,clientID, mot1, mot2, cores):
        """Cette fonction evoluera la population à la génération suivante
            Les étapes suivantes sont faites:
                1-On commence par supprimer tous les individus des éspèces sauf le meilleur
                2-On sépare la population en espèces
                3-On vide le contenu et on garde la dérnière génération
                4-On reproduit les espèces"""

        if self.generationCount>0:
            indivAl = self.tournamentSelection(self.contenu)
            self.contenu = []
        else:
            indivAl = ut.randomPick(self.contenu)

        for e in self.especes:
            tailleProgeniture = 1+int(floor(self.length*e.averageFitness()/self.averageFitness))

            for i in range(tailleProgeniture):
                if len(self.contenu) < self.length:
                    if i == 0:
                        self.contenu.append(e.leader)
                    else:
                        if len(e.best) > 1:
                            par1, par2 = e.parents()
                            enfant = self.crossover(par1, par2)
                        else:
                            enfant = deepcopy(e.individu())
                            enfant.id = self.lastIndId
                            self.lastIndId += 1
                            
                        enfant.mutationPoids()
                        if rand.random() < prob.mutation.connexion:
                            self.mutationConnexion(enfant)
                        
                        if rand.random() < prob.mutation.noeud:
                            self.mutationNoeud(enfant)
    
                        self.contenu.append(enfant)
            e.lastBestFitness = e.leader.fitness
                    
        assert self.length - len(self.contenu) <= 1, "La population est morte"
        if self.length - len(self.contenu) == 1:
            self.contenu.append(deepcopy(indivAl))
        
#        for i in range(len(self.especes)):
#            if e.age > 1:
#                e = self.especes[i]
#                if e.stagnated():
#                    e.stagnationAge += 1
#                else:
#                    e.stagnationAge = 0
#                if e.stagnationAge >= constants.speciation.stagnationAgeThresh:
#                    del self.especes[i]
        
        for e in self.especes:
            e.flush()
        vrep.simxSynchronous(clientID, True)
        err, simState = vrep.simxGetIntegerSignal(clientID, "simulationState", vrep.simx_opmode_streaming)
#        while simState == 17:
#            err, simState = vrep.simxGetIntegerSignal(clientID, "simulationState", vrep.simx_opmode_streaming)
        err = vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
        assert err == 0
        while vrep.simxGetConnectionId != -1 and vrep.simxGetLastCmdTime(clientID) < 1000:     
            for i in range(len(self.contenu)):
                error, angle1 = vrep.simxGetJointPosition(clientID, mot1[i], vrep.simx_opmode_blocking)
                error, angle2 = vrep.simxGetJointPosition(clientID, mot2[i], vrep.simx_opmode_blocking)
                ind = self.contenu[i]
                ind.phenotype.evaluate(ut.entree('1;'+str((angle1 % pi)/pi)+';'+str((angle2 % pi)/pi)))
                out = ind.output()
                mot1V = out[0]
                mot2V = out[1]
                vrep.simxSetJointTargetPosition(clientID, mot1[i], angle1 + 10*mot1V*1e-2, vrep.simx_opmode_streaming)
                vrep.simxSetJointTargetPosition(clientID, mot2[i], angle2 + 10*mot2V*1e-2, vrep.simx_opmode_streaming)
            vrep.simxSynchronousTrigger(clientID)
            
        for i in range(len(self.contenu)):
            error, pos = vrep.simxGetObjectPosition(clientID, cores[i],-1, vrep.simx_opmode_blocking)
            print(pos)
            self.contenu[i].fitness = pos[0]  
            vrep.simxSetJointTargetPosition(clientID, mot1[i], 0, vrep.simx_opmode_oneshot)
            vrep.simxSetJointTargetPosition(clientID, mot2[i], 0, vrep.simx_opmode_oneshot)
        print()
        vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
        
        self.updateEspeces()
#        if len(self.especes) > constants.speciation.nbSpeciesTarget:
#            constants.speciation.distThreshold += constants.speciation.distanceThresholdMod
#        elif len(self.especes) < constants.speciation.nbSpeciesTarget:
#            constants.speciation.distThreshold -= constants.speciation.distanceThresholdMod
        

        
        for i in self.contenu:
            i.phenotype.reinit()
        for e in self.especes:
            e.ajusterFitness()
            e.calculateBest()
        
#        self.updateBest()
        self.updateAverageFitness()
        self.generationCount += 1
コード例 #40
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')
コード例 #41
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!')
コード例 #42
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')
コード例 #43
0
    # Try to retrieve motors and robot handlers
    # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
    ret1, wristHandle = vrep.simxGetObjectHandle(clientID, "WristMotor", opmode)
    ret2, elbowHandle = vrep.simxGetObjectHandle(clientID, "ElbowMotor", opmode)
    ret3, shoulderHandle = vrep.simxGetObjectHandle(clientID, "ShoulderMotor", opmode)
    ret4, leftWheelHandle = vrep.simxGetObjectHandle(clientID, "LeftWheelJoint", opmode)
    ret5, rightWheelHandle = vrep.simxGetObjectHandle(clientID, "RightWheelJoint", opmode)
    ret6, robotHandle = vrep.simxGetObjectHandle(clientID, "2W1A", opmode)

    # If handlers are OK, execute three random simulations
    if ret1 == 0 and ret2 == 0 and ret3 == 0 and ret4 == 0 and ret5 == 0 and ret6 == 0:
        random.seed()
        for i in range(0, 3):
            # Start the simulation
            # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStartSimulation
            vrep.simxStartSimulation(clientID, opmode)
            time.sleep(3)
            msg = "----- Simulation started -----"
            print msg
            logging.info(msg)

            # Start getting the robot position
            # Unlike other commands, we will use a streaming operating mode
            # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectPosition
            pret, robotPos = vrep.simxGetObjectPosition(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
            msg = "2w1a position: (x = " + str(robotPos[0]) +\
                  ", y = " + str(robotPos[1]) + ") res = " + str(pret)
            print msg
            logging.info(msg)

            # Start getting the robot orientation
コード例 #44
0
ファイル: main.py プロジェクト: umd-agrc/QuadRL
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
コード例 #45
0
    def run_trial(self, genomes):
        # Set the signals for each robot
        for genome, robot in zip(genomes, self.robot_names):
            par = [genome[:4], genome[4:]]
            for j in range(len(par)):
                # For each motor
                for k in range(len(par[j])):
                    # For each sensor (+ base value)
                    signal_name = robot + "_" + str(j + 1) + "_" + str(k + 1)
                    res = vrep.simxSetFloatSignal(self.clientID, signal_name,
                                                  par[j][k],
                                                  vrep.simx_opmode_oneshot)
                    if res > 1:
                        print 'Error setting signal ' + signal_name + ': ' + str(
                            res)

        # Start running simulation
        # print 'Running trial'
        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)

        # Initialize output arrays
        sim_time = [[] for i in range(self.n_robots)]
        robot_x = [[] for i in range(self.n_robots)]
        robot_y = [[] for i in range(self.n_robots)]
        init_pos = [[] for i in range(self.n_robots)]
        fit_y = [0 for i in range(self.n_robots)]

        t_step = 0.01  # how often to check the simulation's signals
        t_flag = time.time() + t_step
        once = True
        new_data = [0 for i in range(self.n_robots)]
        go_loop = True

        while go_loop:
            now = time.time()
            if now > t_flag:
                t_flag = now + t_step

                # Get info from robot
                for i in range(self.n_robots):
                    res1, in1 = vrep.simxGetFloatSignal(
                        self.clientID, self.robot_names[i] + '_1',
                        vrep.simx_opmode_buffer)
                    res2, in2 = vrep.simxGetFloatSignal(
                        self.clientID, self.robot_names[i] + '_2',
                        vrep.simx_opmode_buffer)
                    res3, in3 = vrep.simxGetFloatSignal(
                        self.clientID, self.robot_names[i] + '_3',
                        vrep.simx_opmode_buffer)
                    if res1 == 0 and res2 == 0 and res3 == 0:
                        sim_time[i].append(in1)
                        robot_x[i].append(in2)
                        robot_y[i].append(in3)
                        new_data[i] = 1  # new data arrived

                if once:
                    if sum(new_data) == self.n_robots:
                        for i in range(self.n_robots):
                            init_pos[i] = [robot_x[i][-1], robot_y[i][-1]]
                        once = False
                else:
                    for i in range(self.n_robots):
                        if new_data[i] == 1:
                            fit_y[i] += abs(robot_y[i][-1]) * (
                                sim_time[i][-1] - sim_time[i][-2])
                            new_data[i] = 0
                            if sim_time[i][
                                    -1] > 3:  # time limit for the simulation
                                go_loop = False
                                break

        # # Pause the simulation
        # vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot)
        # time.sleep(0.15)
        #
        # trial_fitness = []
        # # Get the trial results for each robot
        # for i in range(len(self.robot_handles)):
        #     res, pos = vrep.simxGetObjectPosition(self.clientID, self.robot_handles[i], -1, vrep.simx_opmode_buffer)
        #     # print pos_tmp
        #
        #     delta_x = pos[0] - self.robot_pos0[i][0]
        #     delta_y = pos[1] - self.robot_pos0[i][1]
        #
        #     # trial_fitness.append(delta_x)
        #     trial_fitness.append([delta_x, 1./(1.+20*abs(delta_y))])
        trial_fitness = []
        for i in range(self.n_robots):
            x_fit = robot_x[i][-1] - init_pos[i][0]
            y_fit = 1 / (1 + 20 * fit_y[i])
            trial_fitness.append([x_fit, y_fit])

        # Stop and reset the simulation
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
        time.sleep(0.35)

        return trial_fitness
コード例 #46
0
ファイル: vrep_env.py プロジェクト: jlabhishek/cnn_bot
 def start(self):
     error_code = vrep.simxStartSimulation(self.clientID,
                                           vrep.simx_opmode_oneshot_wait)