def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './simu.ttt'
self.gain = 2
self.initial_position = [3,3,to_rad(45)]
self.r = 0.096 # wheel radius
self.R = 0.267 # demi-distance entre les r
print('New pioneer simulation started')
vrep.simxFinish(-1)
self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)
if self.client_id!=-1:
print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
self.set_position(self.initial_position)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
def readDummyPath(self):
fp = open(self.dummyPath, 'r')
lines = fp.readlines() # 1行毎にファイル終端まで全て読む(改行文字も含まれる)
fp.close()
for line in lines:
buf = line.split(",")
for x in buf:
params = x.split(":")
if len(params) >= 2:
name = params[0]
try:
portNb = 19998 # int(params[1])
self.dummyID = vrep.simxStart("127.0.0.1", portNb, True, True, 2000, 5)
if self.dummyID == -1:
print >> sys.stderr, "Fatal: No client ID while creating Dummy Communicator."
else:
self.setClientID(self.dummyID)
except ValueError:
print >> sys.stderr, "Fatal: non integer value while creating Dummy Communicator."
time.sleep(1)
exit()
else:
name = params[0]
returnCode, handle = vrep.simxGetObjectHandle(self.dummyID, name, vrep.simx_opmode_oneshot_wait)
if returnCode != vrep.simx_return_ok:
print >> sys.stderr, "Fatal: Error obtaining a handle for " + name + "!"
else:
print name, handle
item = VRepItem(name, self.dummyID, handle)
self.addItem(item)
def connectVREP(self, ipAddr=LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep IP"), port=int(LoadSystemConfiguration.getProperty(LoadSystemConfiguration(),"Vrep port"))):
self.getObjectPositionFirstTime = True
#vrep.simxFinish(-1) # just in case, close all opened connections #TODO this could be a problem when the connection pool is implemented
time1 = time()
clientID = vrep.simxStart(ipAddr,port,True,True,5000,5)
time2 = time()
print "LOGGING time for connecting VREP in senconds: ", time2 - time1
return clientID
def vrepConnect(clientID, port):
clientID=vrep.simxStart('127.0.0.1',port,True,True,1000,5)
if clientID!=-1:
print("Open Connection on port:"+str(port)+' with clientID: '+str(clientID))
else:
print("Failed connecting through port:"+str(port))
vrep.simxFinish(clientID)
return clientID
def __init__(self, port):
super(VREP, self).__init__()
# Add simExtRemoteApiStart(<port>) to an init script of VREP
# subscribe our client
self.id = vrep.simxStart('127.0.0.1', port, True, False, 5000, 5)
self._handle = {}
self._p = []
self._v = []
self._s = []
def __init__(self, **kwargs):
vrep.simxFinish(-1)
print "Start"
print ('Connected to remote API server')
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
self.opmode = vrep.simx_opmode_oneshot_wait
self.model = vrep.simxGetObjectHandle(self.clientID, '2W1A', self.opmode)
self.nGeneration = kwargs.get("nGeneration", 1)
if self.clientID == -1:
sys.exit('Failed connecting to remote API server')
def _init_client_id(self):
vrep.simxFinish(-1)
self.client_id = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if self.client_id != -1:
print 'Connected to remote API server'
else:
print 'Connection not successful'
sys.exit('Could not connect')
def start_simulator():
print 'Program started'
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000, 5)
if clientID != -1:
print 'Connected to remote API server'
else:
print 'Failed connecting to remote API server'
sys.exit('Program Ended')
return clientID
def connect(self):
global SYNC
self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP
if self.cid != -1:
print ('Connected to V-REP remote API serv'
'\er, client id: %s' % self.cid)
vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot)
if SYNC:
vrep.simxSynchronous(self.cid, True)
else:
print ('Failed connecting to V-REP remote API server')
exit()
def __init__(self, address, port):
print ('Program started')
vrep.simxFinish(-1)
self.clientID=vrep.simxStart(address,port,True,True,5000,5)
if self.clientID!=-1:
print "Conexion establecida, todo en orden"
else:
print "Conexion con el simulador fallida, reinicia el simulador"
error,self.motorFL=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
error,self.motorFR=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
error,self.camera=vrep.simxGetObjectHandle(self.clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait)
error,self.robot=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx',vrep.simx_opmode_oneshot_wait)
def start(self):
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if self.clientID != -1:
print ('Connected to remote API server')
self.LWMotor_hdl = vrep.simxGetObjectHandle(self.clientID,'LeftWheel_Motor', vrep.simx_opmode_oneshot_wait) # LeftWheel Motor handle
self.RWMotor_hdl = vrep .simxGetObjectHandle(self.clientID,'RightWheel_Motor', vrep.simx_opmode_oneshot_wait) # RightWheel Motor handle
self.Robot_hdl = vrep.simxGetObjectHandle(self.clientID, 'Cubot', vrep.simx_opmode_oneshot_wait)
print ('Handle acquired !!')
else:
print ('Error : connection to vrep failed')
def env_init(self):
print ('VREP Environmental Program Started')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if self.clientID!=-1:
print ('Connected to remote API server')
self.fmu = FMU()
self.start_simulation()
else:
print('Connection Failure')
sys.exit('Abort Connection')
return self.makeTaskSpec()
def run360():
print "connecting to vrep with address", IPADDRESS, "on port", PORT
clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
print "hello vrep! ", clientID
if clientID == -1:
print "failed to connect to vrep server"
return
# get the motor joint handle
error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
print "starting the motor..."
# set the velocity of the joint
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait);
print "spinning 360 degrees..."
# set up to stream joint positions
vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming);
passed90Degrees = False
# The control loop:
while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server..
(error,position) = vrep.simxGetJointPosition(
clientID,
jointHandle,
vrep.simx_opmode_buffer
)
# Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
if error==vrep.simx_error_noerror :
# here we have the newest joint position in variable jointPosition!
# break when we've done a 360
if passed90Degrees and position >= 0 and position < math.pi/2.0:
break
elif not passed90Degrees and position > math.pi/2.0:
passed90Degrees = True
print "stoppping..."
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait);
if clientID != -1:
vrep.simxFinish(clientID)
print "done!"
def VREPConnect(self):
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def Initialize():
# just in case, close all opened connections
vrep.simxFinish(-1)
# connect to local host port 19999
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)
if clientID!=-1: #check if client connection successful
print ('Connected to remote API server')
else:
print ('Connection not successful')
sys.exit('Could not connect')
return clientID
def __init__(self, sim_dt=0.05, nengo_dt=0.001, sync=True):
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
self.sync = sync
if self.cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % self.cid)
vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
if self.sync:
vrep.simxSynchronous( self.cid, True )
else:
print ('Failed connecting to V-REP remote API server')
exit(1)
self.count = 0
self.sim_dt = sim_dt
self.nengo_dt = nengo_dt
def connectToVREP(self, VREP_World=None):
"Connect to VREP and load the correct world if needed"
"FIXME: SHOULD LAUNCH VREP IF NOT RUNNING"
VREP_exec = 'vrep'
if self.VREPSimulation == None:
self.VREPSimulation = VREP_World
'1. check that V-Rep is running and see whether we are already connected to it. Otherwise connect'
if VREP_exec not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
raise Exception(("V-REP is not running! Please start V-REP with scene: %s" % self.VREPSimulation))
else:
"Check if we are connected with the passed clientId already"
if self._VREP_clientId is not None:
print "ClientId = " ,self._VREP_clientId
connId = vrep.simxGetConnectionId(self._VREP_clientId)
print "My connId is " , connId
if connId == -1: # we are not: set client Id to none and re-connect
print "Disconnecting all existing connections to V-REP"
vrep.simxFinish(-1)
self._VREP_clientId = None
while self._VREP_clientId is None:
self._VREP_clientId = vrep.simxStart(self._host, self._kheperaPort,True,True, 5000,5)
if not self._VREP_clientId == -1:
eCode = vrep.simxSynchronous(self._VREP_clientId, True)
if eCode != 0:
raise Exception("Failed to connect to VREP simulation. Bailing out")
# print " we are connected with clientId ", self._VREP_clientId
"2. Check the correct world is running"
if self.VREPSimulation is not None:
VREP_Scene = split(self.VREPSimulation)[1]
if VREP_Scene not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
eCode = vrep.simxLoadScene(self._VREP_clientId, self.VREPSimulation, 0, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception(("Could not load into V-REP the world", self.VREPSimulation))
"3. Make sure VREP has bees set to the correct directory for saving data"
self.setDataDir(self.dataDir)
'4. Start simulation'
eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("VREP simulation cannot get started")
else:
print "V-REP simulation is running with clientId: ", self._VREP_clientId
return self._VREP_clientId
def __init__(self, ip, port):
"""Initializes connectivity to V-REP environment, initializes environmental variables.
"""
vrep.simxFinish(-1)
self.clientID = vrep.simxStart(ip, port, True, True, 5000, 5)
if self.clientID != -1:
print('Successfully connected to V-REP')
else:
raise RuntimeError('Could not connect to V-REP')
# Some artificial delays to let V-REP stabilize after an action
self.sleep_sec = config.SLEEP_VAL
self.sleep_sec_min = config.SLEEP_VAL_MIN
# id of the Robot Arm in V-REP environment
self.main_object = 'uarm'
# Initial state of the Gripper
self.gripper_enabled = False
# Get hands to various objects in the scene
err, self.cylinder_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_pickupCylinder2',
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("Could not get handle to the cylinder object. Halting program.")
err, self.bin_handle = vrep.simxGetObjectHandle(self.clientID, 'uarm_bin',
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("Could not get handle to the Bin object. Halting program.")
err, self.gripper_handle = vrep.simxGetObjectHandle(self.clientID, 'uarmGripper_motor1Method2',
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("Could not get handle to the Gripper object. Halting program.")
# Distance between cylinder and bin when former is inside later
# This was measured after putting the cylinder in the bin
self.cylinder_bin_distance = config.CYLINDER_BIN_DISTANCE
# Various values, to be set in the start_sim() function, because their values can be obtained
# only after the simulation starts
self.cylinder_height = None
self.cylinder_z_locus = None
self.bin_position = None
self.objects = None
self.object_positions = None
def __init__(self, n_robots, base_name):
self.name = "line follower tester"
self.n_robots = n_robots
self.base_name = base_name
self.robot_names = [self.base_name]
for i in range(self.n_robots-1):
self.robot_names.append(self.base_name+'#'+str(i))
# Establish connection to V-REP
vrep.simxFinish(-1) # just in case, close all opened connections
# Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
# (requires the simulation to be running)
if self.clientID != -1:
print ('Connected to remote API server')
else:
print ('Failed connecting to remote API server')
sys.exit('Could not connect')
# Reset running simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.2)
# Get initial robots' positions
self.robot_handles = []
self.robot_pos0 = []
for rbt_name in self.robot_names:
res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
self.robot_handles.append(rbt_tmp)
# Initialize data stream
vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)
time.sleep(0.2)
for rbt in self.robot_handles:
res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
self.robot_pos0.append(pos)
def reset_vrep():
print 'Start to connect vrep'
# Close eventual old connections
vrep.simxFinish(-1)
# Connect to V-REP remote server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
opmode = vrep.simx_opmode_oneshot_wait
# Try to retrieve motors and robot handlers
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
ret_l, LMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", opmode)
ret_r, RMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", opmode)
ret_a, RobotHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx", opmode)
vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(clientID)
print 'Connection to vrep reset-ed!'
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")
def initialize_vrep_client(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID!=-1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def connection(self):
"""
Make connection with v-rep simulator
"""
print ('Waiting for connection...')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',self.port,True,True,5000,5) # Connect to V-REP
if self.clientID!=-1:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
if self.synchronous:
vrep.simxSynchronous(self.clientID,True)
# start the simulation:
vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
else:
raise IOError('Failed connecting to remote API server')
def vrepSim(clientID, action):
vrep.simxFinish(-1)
localhost = "127.0.0.1"
clientID=vrep.simxStart(localhost,19997,True,True,1000,5)
if clientID!=-1:
print('Connected to remote API server')
if action=="start":
err = vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
if (not err):
print('Sim Started')
elif action=="stop":
err = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
if (not err):
print('Sim Stopped')
print("Disconnected from remote API server")
else:
print('Failed connecting to remote API server')
vrep.simxFinish(clientID)
return clientID
def __init__(self):
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(0.5)
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # tenta conectar no simulador, se salva o clientID
# paramos a simulacao, se estiver em andamento, e comecamos denovo
# vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
#time.sleep(0.5)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# modo da API, como eh False, esta no modo assincrono(os ticks da simulacao rodam em velocidade independente)
vrep.simxSynchronous(self.clientID, False)
print("connected with id ", self.clientID)
self.oldtarget = None
self.camera = None
self.setup()
self.lastimageAcquisitionTime = 0
def __init__(self,port_number):
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', port_number, True, True, 5000, 5)
# start simulation
rc = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
# object handles
res, self.quad_obj = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait)
res, self.camera = vrep.simxGetObjectHandle(self.clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)
res, self.target = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait)
# Initialise data streaming from V-REP
err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.camera, 0, vrep.simx_opmode_streaming)
_,pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_streaming)
_,target_pos = vrep.simxGetObjectPosition(self.clientID, self.target, -1, vrep.simx_opmode_streaming)
time.sleep(2)
# Variables
_,self.last_pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_buffer)
def initializeVrepClient(self):
""" Initialization for Python to connect to VREP.
We obtain a clientID after connecting to VREP, which is saved to
`self.clientID`
This method is only called if we controlling one robot with the remote API
"""
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart(self.ip,self.port,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID!=-1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def connect(self):
"""
Connect to the V-REP simulator
:return: If the connection was successful
:rtype: boolean
"""
if self.is_connected():
self._debug('Already connected')
return False
self._clientID = vrep.simxStart(self._hostname, self._port, True, True, 5000, 5)
if self._clientID == -1:
self._debug("Failed to connect to remote V-REP server")
return False
self._debug("Connected to V-REP server")
self.reset()
return True
def __init__(self, cfg, verbose=False, calcheck=True, setup=True):
self.cfg = cfg
self.verbose = verbose
self.calcheck = calcheck
self.setup = setup
self.connected = False
self.scene_loaded = False
self.tracked_objects = []
self.tracked_handles = []
self.handles = {}
self.info = {}
if not calcheck:
assert not cfg.execute.prefilter, 'Can\'t skip the calibration check and prefilter collisions. Choose.'
self.vrep_proc = None
self.mac_folder = os.path.expanduser(cfg.execute.simu.mac_folder)
self.launch_sim()
self.objects_pos = {}
remote_api.simxFinish(-1)
trycount = 0
while trycount < 5:
self.api_id = remote_api.simxStart('127.0.0.1', self.port, True, False, 5000, 5)
if self.api_id != -1:
break
trycount += 1
assert self.api_id != -1
time.sleep(1)
self.scene = None
if setup and cfg.execute.simu.load:
if self.cfg.execute.is_simulation:
self.setup_scene('robot')
else:
self.setup_scene('solomarker')
def init_vrep_connection(self):
vrep.simxFinish(-1) # just in case, close all opened connections
self.client_id=vrep.simxStart(self.ip,self.port,True,True,5000,5) # Connect to V-REP
if self.client_id!=-1:
print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
#get motor handler
self.motor_handlers = []
for motor in self.robot.motors:
res, motor_handler = vrep.simxGetObjectHandle(
self.client_id,
motor.name,
vrep.simx_opmode_oneshot_wait
)
self.motor_handlers.append(motor_handler)
self.motor_handlers.sort()
print(self.motor_handlers)
#get effector handler
res, self.effector_handler = vrep.simxGetObjectHandle(
self.client_id,
'effector',
vrep.simx_opmode_oneshot_wait
)
#get collision handler
self.collision_handlers = []
for collision in self.collision_list:
res, collision_handler = vrep.simxGetObjectHandle(
self.client_id,
collision,
vrep.simx_opmode_oneshot_wait
)
self.collision_handlers.append(collision_handler)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
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')
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
def __init__(self):
rospy.init_node('Dummy', anonymous=True)
rospy.Subscriber("/restart", Int8, self.receiveStatus, queue_size=1)
fin = rospy.Publisher('/finished', Int8, queue_size=1)
report_sim = rospy.Publisher('/simulation', String, queue_size=1)
starting = rospy.Publisher('/starting', Int16, queue_size=1)
vrep.simxFinish(-1) #clean up the previous stuff
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print("Could not connect to server")
sys.exit()
first = True
counter = 0
while (counter < episodes):
print("Episode Number ", counter + 1)
r = 1
if True: #not first:
if (counter) % 50 == 0 and counter != 0:
print('Sleep for 60')
#time.sleep(60)
else:
time.sleep(3)
simulation_index = np.random.randint(len(VREP_SCENES))
sim_name, sim_path = VREP_SCENES[simulation_index]
msg = String()
msg.data = sim_name
report_sim.publish(msg)
vrep.simxLoadScene(clientID, sim_path, 0,
vrep.simx_opmode_blocking)
time.sleep(2)
while r != 0:
r = vrep.simxStartSimulation(clientID,
vrep.simx_opmode_oneshot)
msg = Int16()
msg.data = counter + 1
starting.publish(msg)
start = time.time()
self.restart = False
elapsed = 0
while (not self.restart and elapsed < maxTime):
curr = time.time()
elapsed = curr - start
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
if not self.restart: #timed out!
msg = Int8()
msg.data = 2
fin.publish(msg)
print(' #### Timed out!')
is_running = True
while is_running:
error_code, ping_time = vrep.simxGetPingTime(clientID)
error_code, server_state = vrep.simxGetInMessageInfo(
clientID, vrep.simx_headeroffset_server_state)
is_running = server_state & 1
counter += 1
first = False
time.sleep(2)
msg = Int8()
msg.data = 1
fin.publish(msg)
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()
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")
def __init__(self):
# Closes all opened connections
vrep.simxFinish(-1)
# Connects to V-REP
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
def truncated_Fourier(coeficients, time):
value = coeficients[0] / 2.0
for i in xrange(N_COEF):
value += coeficients[2 * i + 2] * math.cos(
(i + 1) * coeficients[1] * time)
value += coeficients[2 * i + 3] * math.sin(
(i + 1) * coeficients[1] * time)
return value
#Conexao com o vrep
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart(
'127.0.0.1', 19997, True, True, 5000,
5) # Conecta com o VREP. Por padrao ele ja abre essa porta.
if clientID == -1:
exit(10)
#Reset da simulacao
def reset_simulation(clientID):
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(1) # um pequeno sleep entre o stop e o start
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
#Juntas do NAO
Head_Yaw = []
Head_Pitch = []
def setMotorSpeeds(clientID, handles, speeds):
vrep.simxSetJointTargetVelocity(clientID, handles[0], speeds[0],
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handles[1], speeds[1],
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handles[2], speeds[2],
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, handles[3], speeds[3],
vrep.simx_opmode_streaming)
if __name__ == "__main__":
vrep.simxFinish(-1) #clean up the previous stuff
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
counter = 0
with vrepInterface.VRepInterface.open() as vr:
vr.simxStartSimulation(vrep.simx_opmode_oneshot_wait)
try:
while (1):
sensorHandle, sensorHandle_left, sensorHandle_right = getSensorHandles(
clientID)
follower_frontMotorHandles = getFollowerMotorHandles(clientID)
leader_frontMotorHandles = getLeaderMotorHandles(clientID)
error, blarg, middle_image = vrep.simxReadVisionSensor(
clientID, sensorHandle, vrep.simx_opmode_oneshot_wait)
error, blarg, left_image = vrep.simxReadVisionSensor(
clientID, sensorHandle_left, vrep.simx_opmode_oneshot_wait)
error, blarg, right_image = vrep.simxReadVisionSensor(
def start(self,port):
vrep.simxFinish(-1)
self.clientID=vrep.simxStart('127.0.0.1',port,True,True,5000,5)
import numpy as np
import time
import math as m
import sys
import vrep # access all the VREP elements
from q2R import q2R
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # start a connection
if clientID!=-1:
print ('Connected to remote API server')
else:
print('Not connected to remote API server')
sys.exit("No connection")
# Getting handles for the motors
err, motorL = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_blocking)
err, motorR = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_blocking)
err, robot = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_blocking)
# Assigning handles to the ultrasonic sensors
usensor = []
for i in range(1,17):
err, s = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor'+str(i), vrep.simx_opmode_blocking)
usensor.append(s)
# Sensor initialization
for i in range(16):
err, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(clientID, usensor[i], vrep.simx_opmode_streaming)
err, psr = vrep.simxGetObjectPosition(clientID, usensor[2], robot, vrep.simx_opmode_streaming)
print('')
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt
import math
PI = math.pi
print("Simulação iniciada!!")
vrep.simxFinish(-1) #fecha todas as conexões existentes
port = 25100
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5)
if clientID != -1:
print("Conectado ao servidor remote API na porta ", port)
errCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
if errCode != vrep.simx_return_ok:
print("Erro em obter o handle")
errCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
if errCode != vrep.simx_return_ok:
print("Erro em obter o handle")
def run_benchmark_training(sim_number, sim_time):
print("start running benchmark model")
prefs.codegen.target = "numpy"
start_scope()
# number of sensor in robot
N_sensor = 8
N_vision = 2
# robot radius
r = 0.0586
# CD neurons number
N_CD = 36
# vision detection_neurons number
N_VD = 10
# HD neuron number
N_HD = 72
# coordinate neuron
N_x_axis, N_y_axis = 32, 32
N_PI = N_x_axis * N_y_axis
# Simulation time
# Speed cell number
N_speed = 6
m = 5 # 60
iter = 0
# width and length of the arena x=width/ y=lenght
# 1 square in vrep = 0.5*0.5
x_scale = 2
y_scale = 2
# distance unit per neuron
N = 200 / (N_x_axis * N_y_axis)
#define the initial mismatch encounter
last_mismatch = 0
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Collision detection Neural Architecture--------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark, wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex, Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson, spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse, Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron, Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred, color_notred_synapses, color_red_synapses = CD_net(
N_CD, N_vision, N_VD)
# gaussian input for 8 collison sensors
stimuli = np.array([gaussian_spike(N_CD, j * N_CD / N_sensor, 10, 0.2) for j in range(N_sensor)])
# gaussion distribution for 2 vision input(wall or landmark)
stimuli_vision = np.array([gaussian_spike(N_VD, j * N_VD / N_vision, 10, 0.2) for j in range(N_vision)])
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Head Direction and position Neural Architecture----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse, str_IHD_synapse, sti_PI, sti, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron, IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons, West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse, PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons, WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse, IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse = HD_PI_integrated_net(
N_HD, N_speed, N_x_axis, N_y_axis, sim_time)
##-----------------head direction correcting matrix network-------------#
# Poisson_IMU,IMU_Neuron,HD_errormatrix_neurons,HD_positive_error,HD_negative_error,IMU_poi_synapses,IMU_errors_connecting_synapses,HD_error_connect_synapses,Error_negative_synapses,Error_positive_synapses,Ex_speed_pool_Neurons,Inh_speed_pool_Neurons,Positive_ex_pool_synapses,Negative_inh_pool_synapses,statemon_positiveHD_error,statemon_negativeHD_error,spikemon_positiveHD_error,spikemon_negativeHD_error,spiketrain_pos=HD_error_correcting(N_HD,HD_Neuron)
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Fusi Synapse between Position - Landmark Neuron----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''measure collision weight by fusi synapse landmark'''
landmark_PI_plastic = fusi_landmark(PI_Neurons, Landmark)
w_landmark_plas_shape = np.shape(landmark_PI_plastic.w_fusi_landmark)[0]
w_plastic_landmark = landmark_PI_plastic.w_fusi_landmark
all_fusi_weights_landmark = landmark_PI_plastic.w_fusi_landmark
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Fusi Synapse between Position - Wall Neuron----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''measure collision weight by fusi synapse landmark'''
wall_PI_plastic = fusi_wall(PI_Neurons, Wall)
w_plas_shape_wall = np.shape(wall_PI_plastic.w_fusi_wall)[0]
w_plastic_wall = wall_PI_plastic.w_fusi_wall
all_fusi_weights_wall = wall_PI_plastic.w_fusi_wall
##--------------------------------------------------------------------------------------------------------------------##
##-------------------------------------------Mismatch Network--------------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''store previously estimated landmark position by fusi synapse estimatedlandmark'''
preeq = '''
v_post += 1*(w_piest >= 0.5)
'''
PI_Neurons_est_synapse = Synapses(PI_Neurons, Estimated_landmark_neuron, 'w_piest : 1', on_pre=preeq)
PI_Neurons_est_synapse.connect()
Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron, Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse, NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse, Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch = mismatch_net(
Non_landmark_neuron, Estimated_landmark_neuron, Landmark)
##--------------------------------------------------------------------------------------------------------------------##
##----------------------------------------------Network--------------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
print("adding network")
# Network
PINet = Network()
# add collision network
PINet.add(Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark,
wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron,
Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse,
Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex,
Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson,
spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse,
Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron,
Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex,
Red, Notred, color_notred_synapses, color_red_synapses
)
# add position network
PINet.add(IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse,
str_IHD_synapse, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron,
IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron,
HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse,
Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD,
Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse,
PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons,
West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse,
PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse,
PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons,
WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse,
IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse,
IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse)
# add fusi plastic synapses for landmark,wall neurons with PI neurons
PINet.add(landmark_PI_plastic)
PINet.add(wall_PI_plastic)
# add mismatch network
PINet.add(PI_Neurons_est_synapse, Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse,
spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron,
Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse,
NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse,
Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch)
##-----------------------------------------------------------------------------------------##
##---------------------------------Robot controller----------------------------------------##
##-----------------------------------------------------------------------------------------##
# Connect to the server
print('finished adding network')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print("Connected to remote API server")
else:
print("Not connected to remote API server")
sys.exit("Could not connect")
vrep.simxSynchronous(clientID, 1)
##----------------------------------Controller initialized------------------------------------------##
# set motor
err_code, l_motor_handle = vrep.simxGetObjectHandle(clientID, "KJunior_motorLeft", vrep.simx_opmode_blocking)
err_code, r_motor_handle = vrep.simxGetObjectHandle(clientID, "KJunior_motorRight", vrep.simx_opmode_blocking)
# Compass output=orientation
# define robot
err_code, robot = vrep.simxGetObjectHandle(clientID, 'KJunior', vrep.simx_opmode_blocking)
# define Angles
err_code, Angles = vrep.simxGetObjectOrientation(clientID, robot, -1, vrep.simx_opmode_streaming)
# define object position
err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1, vrep.simx_opmode_streaming)
# get sensor
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8 = getSensor(clientID)
# read point
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8, clientID)
# Distance from sensor to obstacle
sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7,
detectedPoint8)
# get sensor for mismatch landmark detection(get sensor point and distance)
err_code, sensorMismatch = vrep.simxGetObjectHandle(clientID, "Proximity_sensor", vrep.simx_opmode_blocking)
err_code,detectionStateMismatch,detectedPointMismatch,detectedObjectHandleMismatch,detectedSurfaceNormalVectorMismatch=vrep.simxReadProximitySensor(clientID,sensorMismatch,vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
# get vision sensor
err_code, camera = vrep.simxGetObjectHandle(clientID, "Vision_sensor", vrep.simx_opmode_blocking)
err_code, resolution, image = vrep.simxGetVisionSensorImage(clientID, camera, 1, vrep.simx_opmode_streaming)
##---------------------------------Sensor initial condition (for CD)------------------------------------##
# sensor value of every sensor for every neuron
# Use inv_filter to filter out noise
# sensor_val = np.array([np.repeat(inv_filter(sensor_val1),N_CD), np.repeat(inv_filter(sensor_val2),N_CD),np.repeat(inv_filter(sensor_val3),N_CD),np.repeat(inv_filter(sensor_val7),N_CD),np.repeat(inv_filter(sensor_val8),N_CD)])
sensor_val = np.array([np.repeat(inv_filter(sensor_val1), N_CD), np.repeat(inv_filter(sensor_val2), N_CD),
np.repeat(inv_filter(sensor_val3), N_CD), np.repeat(inv_filter(sensor_val4), N_CD),
np.repeat(inv_filter(sensor_val5), N_CD), np.repeat(inv_filter(sensor_val6), N_CD),
np.repeat(inv_filter(sensor_val7), N_CD), np.repeat(inv_filter(sensor_val8), N_CD)])
sensor_val_vision = np.zeros([N_vision, N_VD])
# sum of each sensor value * its gaussian distribution --> sum to find all activity for each neurons --> WTA
All_stimuli = np.sum(sensor_val * stimuli, axis=0)
All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
# find the winner
winner = WTA(All_stimuli)
for w in winner:
Poisson_synapse.w_poi[w] = All_stimuli[w]
winner_vision = WTA(All_stimuli_vision)
for w in winner_vision:
PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w]
##------------------------------------------------------------------------------------------------##
# initial speed of wheel
l_steer = 0.24
r_steer = 0.24
# record the initial time
t_int = time.time()
# record compass angle
compass_angle = np.array([])
# record x and y axis at each time
x_axis = np.array([])
y_axis = np.array([])
# path that is passing
r = np.array([])
# all_time
all_time = np.array([])
# collision index that the robot collided
collision_index = np.array([])
collision_index_during_run = []
# record weight parameter
# record every m sec
weight_record_time = 1
weight_during_run_landmark = np.zeros([np.shape(w_plastic_landmark)[0], 3 * int(sim_time / 5)])
weight_during_run_wall = np.zeros([np.shape(w_plastic_wall)[0], 3 * int(sim_time / 5)])
time_step_list = np.array([])
# start simulation
while (time.time() - t_int) < sim_time:
t1 = time.time() - t_int
# record proximity sensor at each time step
sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6,
detectedPoint7, detectedPoint8)
all_sensor = np.array([sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5])
all_sensor[all_sensor < 4.1e-20] = np.infty
activated_sensor = np.argmin(all_sensor)
# obtain vision sensor values
pixelimage = set(image)
if all_sensor[activated_sensor] < 0.1:
if activated_sensor == 3:
r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
elif activated_sensor == 4:
r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
elif activated_sensor == 0:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
elif activated_sensor == 1:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
elif activated_sensor == 2:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
else:
l_steer = 0.24
r_steer = 0.24
zeta = 0
else:
l_steer = 0.24
r_steer = 0.24
zeta = 0
####-------------------- Record weight------------------------####
weight_during_run_landmark[:, iter] = w_plastic_landmark
weight_during_run_wall[:, iter] = w_plastic_wall
collision_index_during_run.append(collision_index)
####-------------------- Start recording spike (CD) ------------------------####
# cleaning noises for the collision distance sensor value
sensor_val = np.array([np.repeat(inv_filter(sensor_val1), N_CD), np.repeat(inv_filter(sensor_val2), N_CD),
np.repeat(inv_filter(sensor_val3), N_CD), np.repeat(inv_filter(sensor_val4), N_CD),
np.repeat(inv_filter(sensor_val5), N_CD), np.repeat(inv_filter(sensor_val6), N_CD),
np.repeat(inv_filter(sensor_val7), N_CD), np.repeat(inv_filter(sensor_val8), N_CD)])
# get the vision sensor value
sensor_val_vision = generate_vision_val(pixelimage, N_VD)
# reset weight to 0 again
Poisson_synapse.w_poi = 0
PoissonVision_synapse.w_vision_poi = 0
err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming)
# All stimuli and WTA
All_stimuli = (np.sum(sensor_val * stimuli, axis=0))
winner = WTA(All_stimuli)
for w in winner:
Poisson_synapse.w_poi[w] = All_stimuli[w]
All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
winner_vision = WTA(All_stimuli_vision)
for w in winner_vision:
PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w] / 10
# --------mismatching detecting---------##
PI_Neurons_est_synapse.w_piest = landmark_PI_plastic.w_fusi_landmark
####-------------------- End recording spike ----------------------------####
####-------------------- Start recording Head direction ------------------------####
# Choose neuron that is the nearest to the turning speed/direction
# if turn left
if r_steer == 0:
neuron_index = nearest_neuron_speed(zeta, N_speed)
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Left_drive_synapse[neuron_index].w_left_drive = 5
Right_drive_synapse[neuron_index].w_right_drive = 0
Go_straight.w_str = -3
directional_PI_synapse.w_dir_PI = 0
Stay_stay_layer.w_stay_stay = 2
# if turn right
elif l_steer == 0:
neuron_index = nearest_neuron_speed(zeta, N_speed)
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Left_drive_synapse[neuron_index].w_left_drive = 0
Right_drive_synapse[neuron_index].w_right_drive = 5
Go_straight.w_str = -3
directional_PI_synapse.w_dir_PI = 0 # if turn position PI stay
Stay_stay_layer.w_stay_stay = 2
# if go straight
else:
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Go_straight.w_str = 10
directional_PI_synapse.w_dir_PI = 4 # if move position PI run
Stay_stay_layer.w_stay_stay = -4
##-----------------reset IHD (Compass) -----------------##
# Get actual heading direction
err_code, Angles = vrep.simxGetObjectOrientation(clientID, robot, -1, vrep.simx_opmode_streaming)
heading_dir = getHeadingdirection(Angles)
# print("IMU head direction is", heading_dir, "angle from IMU is", Angles)
compass_angle = np.append(compass_angle, heading_dir)
# recalibrate head direction to nearest neuron
recal = nearest_neuron_head(heading_dir, N_HD)
# set reset by compass weight upon angle atm
Reset_synapse.w_reset = np.array(gaussian_spike(N_HD, recal, 30, 0.03))
# print("reset synapse",Reset_synapse.w_reset)
##----------------Active searching for landmark when encountering mismatch -----------------##
if len(spikemon_Mismatch) > last_mismatch:
print('encounter mismatch')
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
while mismatch_sensordistance <= 0.01:
l_steer = 0
r_steer = 0.5
err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer,vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer,vrep.simx_opmode_streaming)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
current_error_distance = mismatch_sensordistance
l_steer = 0.24
r_steer = 0.24
err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming)
last_mismatch = len(spikemon_Mismatch)
##----------------- Read position -----------------##
err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1, vrep.simx_opmode_streaming)
# print("position value is",Position)
x_pos = Position[0]
y_pos = Position[1]
# recalibrate head direction to nearest neuron
x_axis = np.append(x_axis, x_pos)
y_axis = np.append(y_axis, y_pos)
# recalibrate
recal_x_axis = nearest_neuron_x_axis(x_pos, N_x_axis, x_scale)
recal_y_axis = nearest_neuron_y_axis(y_pos, N_y_axis, y_scale)
recal_index = N_x_axis * recal_y_axis + recal_x_axis
r = np.append(r, recal_index) # is an array keeping all index that neuron fire during the run
##----------------- reset position neuron with IMU recalibration ---------------------##
'''coment this line to disable IMU'''
#PI_Reset_synapse.w_poi_PI = np.array(gaussian_spike(N_PI, recal_index, 20, 0.01))
##----------------- Recording Position Index when collision -----------------##
if l_steer == 0 or r_steer == 0:
collision_index = np.append(collision_index, recal_index)
##----------------- Collect time-----------------##
all_time = np.append(all_time, time.time() - t_int)
# run
PINet.run(15 * ms)
print("current plas landmark synapese index", np.where(w_plastic_landmark >= 0.5))
print('wall synpases values index bigger', np.where(w_plastic_wall >= 0.5))
print('count mismatch', len(spikemon_Mismatch))
print('count landmark', len(spikemon_landmark))
print('####-------------------- Read sensor (new round) ----------------------------####')
# Start new measurement in the next time step
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8, clientID)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
err_code, resolution, image = vrep.simxGetVisionSensorImage(clientID, camera, 1, vrep.simx_opmode_streaming)
# record time step and final time
t2 = time.time() - t_int
delta_t = t2 - t1
final_time = time.time()
time_step_list = np.append(time_step_list, delta_t)
print("delta-t", delta_t,iter)
print("final time t2", int(t2))
iter += 1
##---------------end of simulation ---------------##
print("Done")
# saving data
# setting pathway
pathway = "/Users/jieyab/SNN/for_plotting/"
exp_number = str(sim_number)
np.save(pathway + "r" + exp_number, r)
np.save(pathway + "spikemon_CD_i" + exp_number, spikemon_CD.i)
np.save(pathway + "spikemon_CD_t" + exp_number, spikemon_CD.t / ms)
np.save(pathway + "spikemon_HD_i" + exp_number, spikemon_HD.i)
np.save(pathway + "spikemon_HD_t" + exp_number, spikemon_HD.t / ms)
np.save(pathway + "spikemon_PI_i" + exp_number, spikemon_PI.i)
np.save(pathway + "spikemon_PI_t" + exp_number, spikemon_PI.t / ms)
np.save(pathway + "spikemon_noncollision_i" + exp_number, spikemon_noncollison.i)
np.save(pathway + "spikemon_noncollision_t" + exp_number, spikemon_noncollison.t / ms)
np.save(pathway + "spikemon_wall_i" + exp_number, spikemon_wall.i)
np.save(pathway + "spikemon_wall_t" + exp_number, spikemon_wall.t / ms)
np.save(pathway + "spikemon_nonlandmark_i" + exp_number, spikemon_nonlandmark.i)
np.save(pathway + "spikemon_nonlandmark_t" + exp_number, spikemon_nonlandmark.t / ms)
np.save(pathway + "spikemon_landmark_i" + exp_number, spikemon_landmark.i)
np.save(pathway + "spikemon_landmark_t" + exp_number, spikemon_landmark.t / ms)
np.save(pathway + "weight_during_run_landmark" + exp_number, weight_during_run_landmark)
np.save(pathway + "weight_during_run_wall" + exp_number, weight_during_run_wall)
np.save(pathway + "weight_landmark" + exp_number, w_plastic_landmark)
np.save(pathway + "weight__wall" + exp_number, w_plastic_wall)
np.save(pathway + "spikemon_mismatch_t" + exp_number, spikemon_Mismatch.t / ms)
np.save(pathway + "spikemon_mismatch_v" + exp_number, spikemon_Mismatch.v)
np.save(pathway + "collision_index" + exp_number, collision_index)
np.save(pathway + "collision_index_during_run" + exp_number, collision_index_during_run)
np.save(pathway + "compass_angle" + exp_number, compass_angle)
np.save(pathway + "all_time" + exp_number, all_time)
np.save(pathway + "spikemon_estimatedlandmark_i" + exp_number, spikemon_estimatedlandmark.i)
np.save(pathway + "spikemon_estimatedlandmark_t" + exp_number, spikemon_estimatedlandmark.t / ms)
np.save(pathway + "spikemon_nonestimatedlandmark_i" + exp_number, spikemon_nonestimatedlandmark.i)
np.save(pathway + "spikemon_nonestimatedlandmark_t" + exp_number, spikemon_nonestimatedlandmark.t / ms)
np.save(pathway + "spikemon_red_i" + exp_number, spikemon_red.i)
np.save(pathway + "spikemon_red_t" + exp_number, spikemon_red.t / ms)
np.save(pathway + "spikemon_notred_i" + exp_number, spikemon_notred.i)
np.save(pathway + "spikemon_notred_t" + exp_number, spikemon_notred.t / ms)
np.save(pathway + "step_time" + exp_number, time_step_list)
return clientID
ax4.xaxis.set_ticks_position('bottom')
ax4.set_xlabel('state')
ax4.set_ylabel('action')
plt.tight_layout()
plt.show()
plt.draw()
if __name__ == '__main__':
try:
client_id
except NameError:
client_id = -1
e = vrep.simxStopSimulation(client_id, vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(-1)
client_id = vrep.simxStart('127.0.0.1', 25000, True, True, 5000, 5)
assert client_id != -1, 'Failed connecting to remote API server'
# print ping time
sec, msec = vrep.simxGetPingTime(client_id)
print "Ping time: %f" % (sec + msec / 1000.0)
robot = Robot(client_id)
agent = Agent(robot, alpha=0.5, gamma=0.9, epsilon=0.9, q_init=0)
num_episodes = 10
len_episode = 200
return_history = []
try:
for episode in range(num_episodes):
print "start simulation # %d" % episode
def init(port=19997):
vrep.simxFinish(-1) # 关闭所有连接
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5) # 开启连接
if clientID == -1:
raise NameError("Could not connect Vrep")
return clientID
"""
"""
Vrep y OpenCV en Python
@author: Jose Antonio Ruiz Millan
"""
import vrep
import sys
import cv2
import numpy as np
import time
import matplotlib.pyplot as plt
vrep.simxFinish(-1) #Terminar todas las conexiones
clientID = vrep.simxStart(
'127.0.0.1', 19999, True, True, 5000,
5) #Iniciar una nueva conexion en el puerto 19999 (direccion por defecto)
if clientID != -1:
print('Conexion establecida')
else:
sys.exit("Error: no se puede conectar") #Terminar este script
#Guardar la referencia de los motores
_, left_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_oneshot_wait)
_, right_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_oneshot_wait)
def __init__(self):
print 'started'
# Init vrep client
vrep.simxFinish(-1)
# list obstacles in vrep here for it to get detected and mapped into obstacle space
objectsList = ['Wall1','Wall2','Wall3','Wall4','Wall5','Wall6', 'Right_Wall', 'Bottom_Wall', 'Top_Wall', 'Left_Wall']
# resolution where 0.01 corresponds to 1cm in the simulator
resolution = 0.02
# the delay between consecutive movment of the quad
self.moveDelay = 0.4
# get clientID
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if self.clientID != -1:
#The quad helper contains function implementations for various quadcopter functions
self.quad_functions = quad_helper.quad_helper(self.clientID)
print('Main Script Started')
# self.quad_functions.init_sensors()
self.quad_functions.start_sim()
#Setting initial time
init_time = time.time()
d1=0
#Initialize flags
self.obstacleAvoidFlag = False
#Getting map size from the top wall and left wall length
err,topWallHandle = vrep.simxGetObjectHandle(self.clientID,'Top_Wall',vrep.simx_opmode_blocking)
err,self.halfTopWallLen = vrep.simxGetObjectFloatParameter(self.clientID,topWallHandle,18,vrep.simx_opmode_blocking)
err,leftWallHandle = vrep.simxGetObjectHandle(self.clientID,'Left_Wall',vrep.simx_opmode_blocking)
err,self.halfLeftWallLen = vrep.simxGetObjectFloatParameter(self.clientID,leftWallHandle,19,vrep.simx_opmode_blocking)
# used for simulation purpose only!!
positions = [[0,0,3],[0,1,3],[-1,1,3],[-2,1,3],[-2,2,3],[-2,3,3],[-1,3,3],[0,3,3],[1,3,3]]
# Rate init
# self.rate = rospy.Rate(20.0) # MUST be more then 20Hz
#Initializing publishers
# rospy.Subscriber("/quad_explore/target_position", Pose, self.posCb)
#Code to shift the origin from the center to the bottom left
obstOccMat = self.createObstacleOccupancyMat(objectsList,self.clientID,resolution)
corners_cell_wise, image_with_cells = self.cellDecomposition(obstOccMat)
nCells = corners_cell_wise.shape[0]
self.cellOccupancyFlag = np.zeros(nCells).tolist()
print self.cellOccupancyFlag
startPos = [1,1]
err,self.quadHandle = vrep.simxGetObjectHandle(self.clientID,'Quadricopter',vrep.simx_opmode_blocking)
err = vrep.simxSetObjectPosition(self.clientID,self.quadHandle, -1, [startPos[0]-self.halfTopWallLen, startPos[1]-self.halfLeftWallLen, 1.0], vrep.simx_opmode_blocking)
for _ in range(50):
vrep.simxSynchronousTrigger(self.clientID)
startPos = [int(startPos[1]/resolution), int(startPos[0]/resolution)]
# endPos = [3,5]
# endPos = [int(endPos[1]/resolution),int(endPos[0]/resolution)]
# astarDist, path = self.astar(startPos,endPos, obstOccMat)
# endPos = [3,5]
# untul all cells are covered, run the algorithm!
while 0 in self.cellOccupancyFlag:
# get the closest cell to the current position
currCell = self.moveToClosestCorner(corners_cell_wise,resolution,obstOccMat)
# change coordinates from (x,y) to (y,x)
currCellYX = self.convertCornerstoYX(currCell)
# get current position of the quad
err, obj_pos_origin = vrep.simxGetObjectPosition(self.clientID, self.quadHandle,self.originHandle,vrep.simx_opmode_blocking)
start_pos_origin = [int(obj_pos_origin[1]/resolution),int(obj_pos_origin[0]/resolution)]
# create a new image of same size as obstcale map where everything other than the current cell is an obstacle space
cell_image = np.ones_like(obstOccMat, dtype=np.uint8)*255
cell_image[int(currCellYX[0, 0]): int(currCellYX[3, 0]), int(currCellYX[0, 1]):int(currCellYX[3, 1])] = 0
# generate a path to traverse optimally inside the cell and process it
path = InsideCellPlanning(start_pos_origin, currCellYX, cell_image, 10, 10)
# path = np.hstack((path, 0.5*(1/resolution)*np.ones((path.shape[0], 1), dtype = np.uint8)))
# path = (path*resolution).tolist()
path = self.make3dPath(path,resolution)
# move according to the path generated above
self.simPath(path, self.moveDelay, resolution)
print self.cellOccupancyFlag
for _ in range(100): # wait sometime for the quad to settle in
vrep.simxSynchronousTrigger(self.clientID)
def VRep_close_and_connect():
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
return clientID
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')
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)
if not done:
target = (reward + theAgent.gamma *
np.amax(theAgent.model.predict(next_state)[0]))
target_f = theAgent.model.predict(state)
target_f[0][action] = target
theAgent.model.fit(state, target_f, epochs=1, verbose=0)
if reward > 2:
theAgent.epsilon_update(1)
else:
theAgent.epsilon_update(0)
return
if __name__ == "__main__":
# Intialization of the connector to V-Rep simulator
clientID = vrep.simxStart("127.0.0.1", 19997, 1, 1, 2000, 5)
res, objs = vrep.simxGetObjects(clientID, vrep.sim_handle_all,
vrep.simx_opmode_blocking)
if clientID > -1:
print("Connect to Remote API server!")
else:
print('Failed connecting to remote API server')
sys.exit()
#Initializing the Robot information
#Initializing the Learning Agent for DQN
env = environment(
10,
10) #Block number of linear velocity and the grad of angular velocity
action_num = env.vStateNum * env.aStateNum
states_num = len(env.getState())
print(action_num, ' --- ', states_num)
tmp = goaldb[0][0]
tmp = tmp.replace("[", "").replace("]", "")
goal[i] = list(np.fromstring(tmp, dtype='float', sep=' '))
cursor.execute('SELECT map_name FROM scenarios WHERE scenario_id = ' +
str(i))
VrepMap = cursor.fetchall()
VrepMap = "{}{}.stl".format(base_name, VrepMap[0][0])
Map[i] = VrepMap
connect.close()
# Connect to V-rep
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart(
'127.0.0.1', 19997, True, True, -500000,
5) # Connect to V-REP, set a very large time-out for blocking commands
if clientID != -1:
print('Connected to remote API server')
emptyBuff = bytearray()
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
# robotHandle=retInts[0]
# Retrieve some handles:
res, robotHandle = vrep.simxGetObjectHandle(clientID, 'UR5#',
vrep.simx_opmode_oneshot_wait)
def __init__(self):
# 建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True,
5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
#设置机械臂仿真步长，为保持API端与VREP端相同步长
vrep.simxSetFloatingParameter(self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, vrep.simx_opmode_oneshot)
# 打开同步模式
vrep.simxSynchronous(self.clientID, True)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# 获取句柄joint
# 机械臂1
self.robot1_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
# 机械臂2
self.robot2_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_jointHandle[i] = returnHandle
# 获取末端frame
_, self.end_handle = vrep.simxGetObjectHandle(
self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.end0_handle = vrep.simxGetObjectHandle(
self.clientID, 'end0', vrep.simx_opmode_blocking)
_, self.goal1_handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_1', vrep.simx_opmode_blocking)
_, self.goal2_handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_2', vrep.simx_opmode_blocking)
# 俩机械臂间距
_, self.dist_handle = vrep.simxGetDistanceHandle(
self.clientID, 'robots_dist', vrep.simx_opmode_blocking)
_, self.end_dis_handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
_, self.end0_dis_handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot2_goal', vrep.simx_opmode_blocking)
# 获取link句柄
self.link_handle1 = np.zeros((self.jointNum, ), dtype=np.int) #link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 1),
vrep.simx_opmode_blocking)
self.link_handle1[i] = returnHandle
self.link_handle2 = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 4),
vrep.simx_opmode_blocking)
self.link_handle2[i] = returnHandle
print('Handles available!')
def start_connection(self, ip, port):
vrep.simxFinish(-1)
clientID = vrep.simxStart(ip, port, True, True, 5000, 5)
vrep.simxSynchronous(clientID, True)
return clientID
#downward_precision = 1e-4
#
## in degree
#ore_x_peg = 0
#ore_y_peg = 10
#ore_z_peg = 0
#######################
x_range = np.arange(x_start, x_end, x_delta)
y_range = np.arange(y_start, y_end, y_delta)
print('Program started. IP: {0}, Port:{1}'.format(remoteIP, remotePort))
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart(remoteIP, remotePort, True, True, 5000,
5) # Connect to V-REP
if clientID == -1:
print('Failed connecting to remote API server')
sys.exit(-1)
print('Connected to remote API server, client ID: {0}'.format(clientID))
emptyBuff = bytearray()
# Delete all previous models
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'remoteApiCommandServer',
vrep.sim_scripttype_customizationscript, 'clearObjects_function', [], [],
[], emptyBuff, vrep.simx_opmode_blocking)
def openPort():
global clientID
# close any open connections
vrep.simxFinish(-1)
# Connect to the V-REP continuous server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
import vrep
vrep.simxFinish(-1) # 关掉之前的连接
clientId = vrep.simxStart("127.0.0.1", 19997, True, True, 5000, 5) #建立和服务器的连接
if clientId != -1: #连接成功
print('connect successfully')
else:
print('connect failed')
vrep.simxFinish(clientId)
print('program ended')
def __enter__(self):
print('[ROBOTENV] Starting environment...')
sync_mode_str = 'TRUE' if self.sync_mode else 'FALSE'
portNb_str = str(self.portNb)
# launch v-rep
vrep_cmd = [
self.vrepPath, '-gREMOTEAPISERVERSERVICE_' + portNb_str +
'_FALSE_' + sync_mode_str
]
if not self.showGUI:
vrep_cmd.append('-h') # headless mode
vrep_cmd.append(self.scenePath)
# headless mode via ssh
# vrep_cmd = "xvfb-run --auto-servernum --server-num=1 /homes/dam416/V-REP_PRO_EDU_V3_4_0_Linux/vrep.sh -h -s -q MicoRobot.ttt"
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', '-s', '-q', self.scenePath]
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', self.scenePath]
print('[ROBOTENV] Launching V-REP...')
# NOTE: do not use "stdout=subprocess.PIPE" below to buffer logs, causes deadlock at episode 464! (flushing the buffer may work... but buffering is not needed)
self.vrepProcess = subprocess.Popen(vrep_cmd,
shell=False,
preexec_fn=os.setsid)
# connect to V-Rep Remote Api Server
vrep.simxFinish(-1) # close all opened connections
# Connect to V-REP
print('[ROBOTENV] Connecting to V-REP...')
counter = 0
while True:
self.clientID = vrep.simxStart('127.0.0.1', self.portNb, True,
False, 5000, 0)
if self.clientID != -1:
break
else:
print("[ROBOTENV] Connection failed, retrying")
counter += 1
if counter == 10:
raise RuntimeError(
'[ROBOTENV] Connection to V-REP failed.')
if self.clientID == -1:
print('[ROBOTENV] Failed connecting to remote API server')
else:
print('[ROBOTENV] Connected to remote API server')
# close model browser and hierarchy window
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_browser_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_hierarchy_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_console_visible,
False, vrep.simx_opmode_blocking)
# load scene
# time.sleep(5) # to avoid errors
# returnCode = vrep.simxLoadScene(self.clientID, self.scenePath, 1, vrep.simx_opmode_oneshot_wait) # vrep.simx_opmode_blocking is recommended
# # Start simulation
# # vrep.simxSetIntegerSignal(self.clientID, 'dummy', 1, vrep.simx_opmode_blocking)
# # time.sleep(5) #to center window for recordings
# if self.initial_joint_positions is not None:
# self.initializeJointPositions(self.initial_joint_positions)
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# get handles and start streaming distance to goal
for i in range(0, self.nSJoints):
returnCode, self.jointHandles[i] = vrep.simxGetObjectHandle(
self.clientID, 'Mico_joint' + str(i + 1),
vrep.simx_opmode_blocking)
printlog('simxGetObjectHandle', returnCode)
returnCode, self.fingersH1 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor1',
vrep.simx_opmode_blocking)
returnCode, self.fingersH2 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor2',
vrep.simx_opmode_blocking)
returnCode, self.goalCubeH = vrep.simxGetObjectHandle(
self.clientID, 'GoalCube', vrep.simx_opmode_blocking)
returnCode, self.targetCubePositionH = vrep.simxGetObjectHandle(
self.clientID, 'GoalPosition', vrep.simx_opmode_blocking)
returnCode, self.robotBaseH = vrep.simxGetObjectHandle(
self.clientID, 'Mico_link1_visible', vrep.simx_opmode_blocking)
returnCode, self.jointsCollectionHandle = vrep.simxGetCollectionHandle(
self.clientID, "sixJoints#", vrep.simx_opmode_blocking)
returnCode, self.distToGoalHandle = vrep.simxGetDistanceHandle(
self.clientID, "distanceToGoal#", vrep.simx_opmode_blocking)
returnCode, self.endEffectorH = vrep.simxGetObjectHandle(
self.clientID, "DummyFinger#", vrep.simx_opmode_blocking)
# returnCode, self.distanceToGoal = vrep.simxReadDistance(self.clientID, self.distToGoalHandle, vrep.simx_opmode_streaming) #start streaming
# returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(self.clientID, self.jointsCollectionHandle, 15, vrep.simx_opmode_streaming) #start streaming
if self.targetCubePosition is not None:
# hide goal position plane
visibility_layer_param_ID = 10
visible_layer = 1
returnCode = vrep.simxSetObjectIntParameter(
self.clientID, self.targetCubePositionH,
visibility_layer_param_ID, visible_layer,
vrep.simx_opmode_blocking)
# print('simxSetObjectIntParameter return Code: ', returnCode)
self.initializeGoalPosition()
# Start simulation
if self.initial_joint_positions is not None:
self.initializeJointPositions()
self.reset()
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# self.updateState() # default initial state: 180 degrees (=pi radians) for all angles
# check the state is valid
# while True:
# self.updateState()
# print("\nstate: ", self.state)
# print("\nreward: ", self.reward)
# # wait for a state
# if (self.state.shape == (1,self.observation_space_size) and abs(self.reward) < 1E+5):
# print("sent reward3=", self.reward)
# break
print(
'[ROBOTENV] Environment succesfully initialised, ready for simulations'
)
# while vrep.simxGetConnectionId(self.clientID) != -1:
# ##########################EXECUTE ACTIONS AND UPDATE STATE HERE #################################
# time.sleep(0.1)
# #end of execution loop
return self
#coding: utf-8
import time
import vrep
import keras
import math
import localization
import numpy as np
from keras.models import Model
from keras.models import load_model
serverIP = "127.0.0.1"
serverPort = 19999
#---------------------Conecta no servidor---------------------------------
clientID = vrep.simxStart(serverIP, serverPort, True, True, 2000, 5)
nomeSensor = []
sensorHandle = []
dist = []
leftMotorHandle = 0
rightMotorHandle = 0
global v_Left, v_Right, tacoDir, tacoEsq
v_Left = 1
v_Right = 1
if (clientID!=-1):
print ("Servidor Conectado!")
#------------------------------Inicializa Sensores ----------------------------
for i in range(0,8):
nomeSensor.append("sensor" + str(i+1))
res, handle = vrep.simxGetObjectHandle(clientID, nomeSensor[i], vrep.simx_opmode_oneshot_wait)
try:
import vrep
except:
print ('--------------------------------------------------------------')
print ('"vrep.py" could not be imported. This means very probably that')
print ('either "vrep.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
import time
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print('Connected to remote API server')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res,objs=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_blocking)
if res==vrep.simx_return_ok:
print ('connect success')
current_filename = os.path.realpath(__file__)
vrep.simxAddStatusbarMessage(clientID,'connected from '+ current_filename,
vrep.simx_opmode_oneshot)
else:
print ('Remote API function call returned with error code: ',res)
res,quadrotor_base = vrep.simxGetObjectHandle(clientID,"Quadricopter_base", vrep.simx_opmode_oneshot_wait)
_,quadrotor_target = vrep.simxGetObjectHandle(clientID, "Quadricopter_target", vrep.simx_opmode_oneshot_wait)
def quadcopter_fcn():
try:
import vrep
except:
print('--------------------------------------------------------------')
print('"vrep.py" could not be imported. This means very probably that')
print('either "vrep.py" or the remoteApi library could not be found.')
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "vrep.py"')
print('--------------------------------------------------------------')
print('')
import time
import numpy as np
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
#global variables:
cumul = 0
last_e = 0
pAlphaE = 0
pBetaE = 0
psp2 = 0
psp1 = 0
prevEuler = 0
cumulAlpha = 0
cumulBeta = 0
cumulAlphaPos = 0
cumulBetaPos = 0
particlesTargetVelocities = [0, 0, 0, 0]
#speed weight:
vParam = -2
#parameters for vertical control
Kpv = 2
Kiv = 0
Kdv = 2
#parameters for horizontal control
Kph = 0.4
Kih = 0.1
Kdh = 1.5
Kph_pos1 = 0.4
Kih_pos1 = 0.001
Kdh_pos1 = 0.05
Kph_pos0 = 0.4
Kih_pos0 = 0.001
Kdh_pos0 = 0.05
#parameters for rotational control:
Kpr = 0.05
Kir = 0
Kdr = 0.9
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff = bytearray()
[returnCode,
targetObj] = vrep.simxGetObjectHandle(clientID, 'Quadricopter_target',
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle] = vrep.simxGetObjectHandle(clientID,
'Quadricopter_base',
vrep.simx_opmode_blocking)
[returnCode,
qc_handle] = vrep.simxGetObjectHandle(clientID, 'Quadricopter',
vrep.simx_opmode_blocking)
# main loop:
while True:
# vertical control:
[returnCode,
targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode, l,
w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
vrep.simx_opmode_blocking)
e = targetPos[2] - pos[2]
cumul = cumul + e
diff_e = e - last_e
Pvert = Kpv * e
Ivert = Kiv * cumul
Dvert = Kdv * diff_e
thrust = 5.335 + Pvert + Ivert + Dvert + l[2] * vParam # get thrust
last_e = e
# horizontal control:
[returnCode,
sp] = vrep.simxGetObjectPosition(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
clientID, 'Quadricopter', vrep.sim_scripttype_childscript,
'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
clientID, 'Quadricopter', vrep.sim_scripttype_childscript,
'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, rc, rc,
rc] = vrep.simxCallScriptFunction(clientID, 'Quadricopter',
vrep.sim_scripttype_childscript,
'qc_Get_Object_Matrix', [], [],
[], emptyBuff,
vrep.simx_opmode_blocking)
[errorCode,
M] = vrep.simxGetStringSignal(clientID, 'mtable',
vrep.simx_opmode_oneshot_wait)
if (errorCode == vrep.simx_return_ok):
m = vrep.simxUnpackFloats(M)
alphaE = vy[2] - m[11]
cumulAlpha = cumulAlpha + alphaE
diff_alphaE = alphaE - pAlphaE
alphaCorr = Kph * alphaE + Kih * cumulAlpha + Kdh * diff_alphaE #alpha correction
betaE = vx[2] - m[11]
cumulBeta = cumulBeta + betaE
diff_betaE = betaE - pBetaE
betaCorr = -Kph * betaE - Kih * cumulBeta - Kdh * diff_betaE #beta correction
pAlphaE = alphaE
pBetaE = betaE
cumulAlphaPos = cumulAlphaPos + sp[1]
cumulBetaPos = cumulBetaPos + sp[0]
alphaPos = Kph_pos1 * (
sp[1]) + Kih_pos1 * cumulAlphaPos + Kdh_pos1 * (
sp[1] - psp2) #alpha position correction
betaPos = Kph_pos0 * (
sp[0]) + Kih_pos0 * cumulBetaPos + Kdh_pos0 * (
sp[0] - psp1) #beta position correction
alphaCorr = alphaCorr + alphaPos
betaCorr = betaCorr - betaPos
psp2 = sp[1]
psp1 = sp[0]
# rotational control:
[returnCode,
euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, orientation
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
Prot = Kpr * euler[2]
Drot = Kdr * (euler[2] - prevEuler)
rotCorr = Prot + Drot
prevEuler = euler[2]
# set propeller velocities:
propeller1_PTV = thrust * (1 - alphaCorr + betaCorr - rotCorr)
propeller2_PTV = thrust * (1 - alphaCorr - betaCorr + rotCorr)
propeller3_PTV = thrust * (1 + alphaCorr - betaCorr - rotCorr)
propeller4_PTV = thrust * (1 + alphaCorr + betaCorr + rotCorr)
particlesTargetVelocities = [
propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV
]
print '////////////'
print '------------'
print '-Controller parameters-'
print 'Vertical control parameters=', [Kpv, Kiv, Kdv]
print 'Horizontal control parameters=', [
Kph, Kih, Kdh, Kph_pos0, Kdh_pos0, Kph_pos1, Kdh_pos1
]
print 'Rotational control parameters=', [Kpr, Kir, Kdr]
print '------------'
print '-Vertical component-'
print 'targetPos=', targetPos
print 'pos=', pos
print 'e=', e
print 'thrust=', thrust
print '------------'
print '-Horizontal component-'
print 'cumulAlpha=', cumulAlpha
print 'cumulBeta=', cumulBeta
print 'cumulAlphaPos=', cumulAlphaPos
print 'cumulBetaPos=', cumulBetaPos
print 'alphaE=', alphaE
print 'betaE=', betaE
print 'alphaCorr=', alphaCorr
print 'betaCorr=', betaCorr
print 'orientation=', orientation
print 'sp_X=', sp[0]
print 'sp_Y=', sp[1]
print '------------'
print '-Rotational component-'
print 'vx=', vx
print 'vy=', vy
print 'gamma=', euler[2]
print 'rotCorr=', rotCorr
print '------------'
print 'Velocity_propeller_1 = ', particlesTargetVelocities[0]
print 'Velocity_propeller_2 = ', particlesTargetVelocities[1]
print 'Velocity_propeller_3 = ', particlesTargetVelocities[2]
print 'Velocity_propeller_4 = ', particlesTargetVelocities[3]
print '------------'
print '////////////'
# send propeller velocities to output:
[res, retInts, retFloats, retStrings,
retBuffer] = vrep.simxCallScriptFunction(
clientID, 'Quadricopter', vrep.sim_scripttype_childscript,
'qc_propeller_v', [], particlesTargetVelocities, [],
emptyBuff, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')