def restart(self,earlyStop = False):
if (self.cid != None):
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
self.connect()
time.sleep(1)
vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
if earlyStop:
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
return
vrep.simxStartSimulation(self.cid, self.mode())
self.runtime = 0
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait)
err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
self.getTargetStart()
for i in range(4):
self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
'Quadricopter_propeller_respondable' + str(i) + str(1),
self.mode())
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './simu.ttt'
self.gain = 2
self.initial_position = [3,3,to_rad(45)]
self.r = 0.096 # wheel radius
self.R = 0.267 # demi-distance entre les r
print('New pioneer simulation started')
vrep.simxFinish(-1)
self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)
if self.client_id!=-1:
print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
self.set_position(self.initial_position)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
def close(self, kill=False):
if self.connected:
remote_api.simxStopSimulation(self.api_id, remote_api.simx_opmode_oneshot_wait)
remote_api.simxFinish(self.api_id)
self.connected = False
if kill and self.vrep_proc is not None:
os.killpg(self.vrep_proc.pid, signal.SIGTERM)
def cleanUp(self):
print "About to stop simulation connected to self.simulID: ", self.simulID
vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
vrep.simxFinish(self.simulID)
vrep.simxFinish(-1)
print "Disconnected from V-REP"
def 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 close_connection(self):
if self.clientID != -1:
# BeforeQSize closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(self.clientID)
# Now close the connection to V-REP:
vrep.simxFinish(self.clientID)
print ('Program ended')
def __init__(self, ip='127.0.0.1', port=19997):
# Close eventual old connections
vrep.simxFinish(-1)
self.clientID = -1
self.opmode = vrep.simx_opmode_oneshot_wait
# Connect to V-REP remote server
self.connect(ip, port)
def disconnection(self):
"""
Make disconnection with v-rep simulator
"""
# stop the simulation:
vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
# Now close the connection to V-REP:
vrep.simxFinish(self.clientID)
def __init__(self, **kwargs):
vrep.simxFinish(-1)
print "Start"
print ('Connected to remote API server')
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
self.opmode = vrep.simx_opmode_oneshot_wait
self.model = vrep.simxGetObjectHandle(self.clientID, '2W1A', self.opmode)
self.nGeneration = kwargs.get("nGeneration", 1)
if self.clientID == -1:
sys.exit('Failed connecting to remote API server')
def _init_client_id(self):
vrep.simxFinish(-1)
self.client_id = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if self.client_id != -1:
print 'Connected to remote API server'
else:
print 'Connection not successful'
sys.exit('Could not connect')
def start(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 __init__(self, address, port):
print ('Program started')
vrep.simxFinish(-1)
self.clientID=vrep.simxStart(address,port,True,True,5000,5)
if self.clientID!=-1:
print "Conexion establecida, todo en orden"
else:
print "Conexion con el simulador fallida, reinicia el simulador"
error,self.motorFL=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
error,self.motorFR=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
error,self.camera=vrep.simxGetObjectHandle(self.clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait)
error,self.robot=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx',vrep.simx_opmode_oneshot_wait)
def start_simulator():
print 'Program started'
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000, 5)
if clientID != -1:
print 'Connected to remote API server'
else:
print 'Failed connecting to remote API server'
sys.exit('Program Ended')
return clientID
def env_init(self):
print ('VREP Environmental Program Started')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if self.clientID!=-1:
print ('Connected to remote API server')
self.fmu = FMU()
self.start_simulation()
else:
print('Connection Failure')
sys.exit('Abort Connection')
return self.makeTaskSpec()
def run360():
print "connecting to vrep with address", IPADDRESS, "on port", PORT
clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
print "hello vrep! ", clientID
if clientID == -1:
print "failed to connect to vrep server"
return
# get the motor joint handle
error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
print "starting the motor..."
# set the velocity of the joint
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait);
print "spinning 360 degrees..."
# set up to stream joint positions
vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming);
passed90Degrees = False
# The control loop:
while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server..
(error,position) = vrep.simxGetJointPosition(
clientID,
jointHandle,
vrep.simx_opmode_buffer
)
# Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
if error==vrep.simx_error_noerror :
# here we have the newest joint position in variable jointPosition!
# break when we've done a 360
if passed90Degrees and position >= 0 and position < math.pi/2.0:
break
elif not passed90Degrees and position > math.pi/2.0:
passed90Degrees = True
print "stoppping..."
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait);
if clientID != -1:
vrep.simxFinish(clientID)
print "done!"
def VREPConnect(self):
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def close(self):
"""
Close the connection with the robot. Same as 'disconnect()'
:return: 0 if all ok
:rtype: int
"""
if self.is_connected():
# Stop the robot
self.stop()
# Close the socket
vrep.simxFinish(self._clientID)
return 0
else:
return -1
def Initialize():
# just in case, close all opened connections
vrep.simxFinish(-1)
# connect to local host port 19999
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)
if clientID!=-1: #check if client connection successful
print ('Connected to remote API server')
else:
print ('Connection not successful')
sys.exit('Could not connect')
return clientID
def __init__(self, sim_dt=0.05, nengo_dt=0.001, sync=True):
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
self.sync = sync
if self.cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % self.cid)
vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
if self.sync:
vrep.simxSynchronous( self.cid, True )
else:
print ('Failed connecting to V-REP remote API server')
exit(1)
self.count = 0
self.sim_dt = sim_dt
self.nengo_dt = nengo_dt
def finishSimulation(clientID):
errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
error=errorStop or errorClose
errorFinish=vrep.simxFinish(clientID)
error=error or errorFinish
return error
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 finishSimulation(self, clientID):
self.getObjectPositionFirstTime = True
errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
error=errorStop or errorClose
errorFinish=vrep.simxFinish(clientID)
error=error or errorFinish
return error
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 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 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 __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 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 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 vrep_exit(clientID):
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(clientID)
exit(0)
def close_connection(self):
#clientID = vrep.simxStart(ip, port, True, True, 5000, 5)
vrep.simxFinish(self.clientID)
return 0
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 cv2
import numpy as np
import matplotlib.pyplot as plt
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, leftMotor = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
errCode, rightMotor = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
errCode, cam = vrep.simxGetObjectHandle(clientID, 'camera',
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 ('Number of objects in the scene: ',len(objs))
else:
print ('Remote API function call returned with error code: ',res)
time.sleep(2)
# Now retrieve streaming data (i.e. in a non-blocking fashion):
startTime=time.time()
def disconnectVREP(clientID):
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
print('Connection finished')
def end_communication(self):
vrep.simxStopSimulation(self.client_id, vrep.simx_opmode_blocking)
vrep.simxFinish(self.client_id)
pgrp = os.getpgid(self.process.pid)
os.killpg(pgrp, signal.SIGKILL)
def train(load_model_path, save_model_path, number_training_steps,
data_path, image_shape, ratio=1, batch_size=2,
save_online_batch=False, save_online_batch_path=None):
# Initialize connection
connection = VrepConnection()
connection.synchronous_mode()
connection.start()
# Load data
data_file = h5py.File(data_path,"r")
x = h5py_to_array(data_file['images'][:1000], image_shape)
y = data_file['joint_vel'][:1000]
datapoints = x.shape[0]
# Initialize ratios
online_batchsize = int(np.floor(1.0 * batch_size/(ratio + 1)))
data_images_batchsize = int(batch_size - online_batchsize)
current_online_batch = 0
x_batch = np.empty((batch_size,) + image_shape)
y_batch = np.empty((batch_size, 6))
jointpos_array = np.empty((batch_size, 6))
nextpos_array = np.empty((batch_size, 6))
print "Batch size: ", batch_size
print "Online batch size: ", online_batchsize
print "Dataset batch size: ", data_images_batchsize
# Load keras model
model = load_model(load_model_path)
# Use client id from connection
clientID = connection.clientID
# Get joint handles
jhList = [-1, -1, -1, -1, -1, -1]
for i in range(6):
err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking)
jhList[i] = jh
# Initialize joint position
jointpos = np.zeros(6)
for i in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming)
jointpos[i] = jp
# Initialize vision sensor
res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming)
vrep.simxGetPingTime(clientID)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
# Initialize distance handle
err, distanceHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_streaming)
# Initialize online batch hdf5 file
if save_online_batch:
f = h5py.File(save_online_batch_path, "w")
dset1 = f.create_dataset("images", (batch_size,) + image_shape, dtype=np.float32)
dset2 = f.create_dataset("joint_pos", (batch_size, 6), dtype=np.float32)
dset3 = f.create_dataset("next_pos", (batch_size, 6), dtype=np.float32)
dset4 = f.create_dataset("distance", (batch_size, 1), dtype=np.float32)
dset5 = f.create_dataset("joint_vel", (batch_size, 6), dtype=np.float32)
# Step while IK movement has not begun
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
while (signalValue == 0):
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
# Iterate over number of steps to train model online
path_empty_counter = 0
step_counter = 0
while step_counter < number_training_steps:
# 1. Obtain image from vision sensor and next path position from Lua script
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
img = np.resize(image, (1,) + image_shape) # resize into proper shape for input to neural network
img = img.astype(np.uint8)
img = img.astype(np.float32)
img /= 255
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
err, _, next_pos, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'getNextPathPos', inputInts,
inputFloats, inputStrings,
inputBuffer,
vrep.simx_opmode_blocking)
# 2. Pass into neural network to get joint velocities
jointvel = model.predict(img, batch_size=1)[0] # output is a 2D array of 1X6, access the first variable to get vector
stepsize = 1
jointvel *= stepsize
# 3. Apply joint velocities to arm in V-REP
for j in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer)
jointpos[j] = jp
err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_buffer)
# Check if next pos is valid before fitting
if len(next_pos) != 6:
path_empty_counter += 1
continue
ik_jointvel = joint_difference(jointpos, next_pos)
ik_jointvel = ik_jointvel / np.sum(np.absolute(ik_jointvel)) * 0.5 * distance_to_target
ik_jointvel = np.resize(ik_jointvel, (1, 6))
x_batch[current_online_batch] = img[0]
y_batch[current_online_batch] = ik_jointvel[0]
jointpos_array[current_online_batch] = jointpos
nextpos_array[current_online_batch] = next_pos
dset4[current_online_batch] = distance_to_target
current_online_batch += 1
if current_online_batch == online_batchsize:
# Step counter
print "Training step: ", step_counter
step_counter += 1
# Random sample from dataset
if ratio > 0:
indices = random.sample(range(int(datapoints)), int(data_images_batchsize))
indices = sorted(indices)
x_batch[online_batchsize:] = x[indices]
y_batch[online_batchsize:] = y[indices]
dset4[online_batchsize:] = data_file["distance"][indices]
dset2[online_batchsize:] = data_file["joint_pos"][indices]
# 4. Fit model
model.fit(x_batch, y_batch,
batch_size=batch_size,
epochs=1)
# Reset counter
current_online_batch = 0
# Save to online batch dataset
dset1[:] = x_batch
dset5[:] = y_batch
dset2[:online_batchsize] = jointpos_array[:online_batchsize]
dset3[:] = nextpos_array
# Print statements
# print "Predicted joint velocities: ", jointvel, "Abs sum: ", np.sum(np.absolute(jointvel))
# print "IK Joint velocity: ", ik_jointvel, "Abs sum: ", np.sum(np.absolute(ik_jointvel))
# print "Distance to cube: ", distanceToCube
# trigger next step and wait for communication time
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
# save updated model and delete model
model.save(save_model_path)
del model
# Close file
if save_online_batch:
f.close()
# Error check
print "No of times path is empty:", path_empty_counter
def followpath(path, objectHandle):
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
pos_on_path = 1
dis = 0
path_pos = {}
_, robotHandle = vrep.simxGetObjectHandle(
clientID, 'Start', vrep.simx_opmode_oneshot_wait)
_, targetHandle = vrep.simxGetObjectHandle(
clientID, 'End', vrep.simx_opmode_oneshot_wait)
_, lm = vrep.simxGetObjectHandle(clientID, 'LeftJoint',
vrep.simx_opmode_oneshot_wait)
_, rm = vrep.simxGetObjectHandle(clientID, 'RightJoint',
vrep.simx_opmode_oneshot_wait)
_, ebot = vrep.simxGetObjectHandle(clientID, 'eBot',
vrep.simx_opmode_oneshot_wait)
print(
vrep.simxGetObjectPosition(clientID, robotHandle, -1,
vrep.simx_opmode_oneshot_wait))
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'Dummy', vrep.sim_scripttype_childscript,
'threadFunction', [], path, [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
print(len(retFloats))
while (1):
_, _, dis, _, retBuffer = vrep.simxCallScriptFunction(
clientID, 'Dummy', vrep.sim_scripttype_childscript, 'follow',
[pos_on_path], retFloats, [], emptyBuff,
vrep.simx_opmode_blocking)
v_des = -0.05
om_des = -0.8 * dis[1]
d = 0.06
v_r = (v_des + d * om_des)
v_l = (v_des - d * om_des)
r_w = 0.0275
omega_right = (v_r / r_w)
omega_left = (v_l / r_w)
print(omega_right)
print(omega_left)
_ = vrep.simxSetJointTargetVelocity(clientID, lm,
(-1 * omega_left),
vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(clientID, rm,
(-1 * omega_right),
vrep.simx_opmode_oneshot_wait)
if (dis[0] < 0.1):
pos_on_path += 3
print(pos_on_path)
if (pos_on_path - 1 == len(retFloats)):
_ = vrep.simxSetJointTargetVelocity(
clientID, lm, 0, vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
clientID, rm, 0, vrep.simx_opmode_oneshot_wait)
break
if (objectHandle):
_ = vrep.simxSetObjectPosition(clientID, objectHandle, ebot,
[0, 0, 0.052],
vrep.simx_opmode_oneshot_wait)
def mainLoop(mode):
nombreArchivo = ""
if mode == 'incr':
print("El programa se ejecutará de la manera tradicional")
nombreArchivo = "nuevo.txt"
else:
print("El programa se ejecutará para visualizar las mejores corridas")
nombreArchivo = "archivo.txt"
vrep.simxFinish(-1)
portNumb = 19997
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)
#Read Instructions from file
instructions = readInstructions(nombreArchivo)
#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)
'''
TODO Ahora tengo que graficar la posición x de la cabez en las mejores 10 corridas y también el punto móvil
'''
#Visualization of the info collected
sortedScore = sorted(runInfo, key=lambda x: x[1], reverse=True)
filteredBestSec = []
for i in range(0, 10):
filteredBestSec.append(instructions[sortedScore[i][0]])
sg.recordSecuences(filteredBestSec, "mejores.txt")
# print(runInfo)
sg.recordRunOutput(runInfo, "salida.txt")
if mode == 'incr':
newLot = []
for x in filteredBestSec:
newLot = newLot + sg.generateNewSec(x, 10)
sg.recordSecuences(filteredBestSec + newLot, "nuevo.txt")
else:
# TODO graficar solo las mejores 10 y el punto móvil.
for h in range(0, 10):
# print ("esto debería ser el tiempo: ", sortedScore[h][2][0])
# print ("Que es esto?: ", sortedScore[h][0], " ---- ", sortedScore[h][1])
# # print ("esto debería ser el valor de x: ", sortedScore[h][2][0][1])
# print (list(map(lambda x:x[2][1],sortedScore[h])))
# print (list(map(lambda x:x[2][0][1][0],sortedScore[h])))
plt.plot(list(map(lambda x: x[1], sortedScore[h][2])),
list(map(lambda x: x[0][1][0], sortedScore[h][2])))
#plt.plot(list(map(lambda x:x[1],h)),list(map(lambda x:x[0][1][1],sortedScore[h])))
#plt.plot(list(map(lambda x:x[1],h)),list(map(lambda x:x[0][1][2],sortedScore[h])))
timeList = list(map(lambda x: x[1], sortedScore[0][2]))
plt.plot(timeList, list(map(lambda x: puntoMovil(x)[0], timeList)))
plt.show()
else:
print("No se pudo establecer conexión con la api del simulador")
print("Verificar que el simulador está abierto")
def disconnect(self):
if hasattr(self, 'clientID'):
vrep.simxFinish(self.clientID)
else:
vrep.simxFinish(-1)
def move(dtheta1, dtheta2, dtheta3, dtheta4, dtheta5, dtheta6, dtheta7):
# Close all open connections (just in case)
vrep.simxFinish(-1)
# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
# Get "handle" to the joints of robot
result, joint_one_handle = vrep.simxGetObjectHandle(
clientID, 'Sawyer_joint1', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, joint_two_handle = vrep.simxGetObjectHandle(
clientID, 'Sawyer_joint2', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for second joint')
result, joint_three_handle = vrep.simxGetObjectHandle(
clientID, 'Sawyer_joint3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for third joint')
result, joint_four_handle = vrep.simxGetObjectHandle(
clientID, 'Sawyer_joint4', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for fourth joint')
result, joint_five_handle = vrep.simxGetObjectHandle(
clientID, 'Sawyer_joint5', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for fifth joint')
result, joint_six_handle = vrep.simxGetObjectHandle(
clientID, 'Sawyer_joint6', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for sixth joint')
result, joint_seven_handle = vrep.simxGetObjectHandle(
clientID, 'Sawyer_joint7', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for seventh joint')
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Wait two seconds
time.sleep(2)
# Get the current value of the joint variables
result, theta1 = vrep.simxGetJointPosition(clientID, joint_one_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print(
'current value of first joint variable: theta1 = {:f}'.format(theta1))
result, theta2 = vrep.simxGetJointPosition(clientID, joint_two_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print(
'current value of second joint variable: theta2 = {:f}'.format(theta2))
result, theta3 = vrep.simxGetJointPosition(clientID, joint_three_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print(
'current value of third joint variable: theta3 = {:f}'.format(theta3))
result, theta4 = vrep.simxGetJointPosition(clientID, joint_four_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print(
'current value of fourth joint variable: theta4 = {:f}'.format(theta4))
result, theta5 = vrep.simxGetJointPosition(clientID, joint_five_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print(
'current value of fifth joint variable: theta5 = {:f}'.format(theta5))
result, theta6 = vrep.simxGetJointPosition(clientID, joint_six_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print(
'current value of sixth joint variable: theta6 = {:f}'.format(theta6))
result, theta7 = vrep.simxGetJointPosition(clientID, joint_seven_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print('current value of seventh joint variable: theta7 = {:f}'.format(
theta7))
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_one_handle,
theta1 + dtheta1, vrep.simx_opmode_oneshot)
# Wait one seconds
time.sleep(1)
# Set the desired value of the second joint variable
vrep.simxSetJointTargetPosition(clientID, joint_two_handle,
theta2 + dtheta2, vrep.simx_opmode_oneshot)
# Wait one seconds
time.sleep(1)
# Set the desired value of the third joint variable
vrep.simxSetJointTargetPosition(clientID, joint_three_handle,
theta3 + dtheta3, vrep.simx_opmode_oneshot)
# Wait one seconds
time.sleep(1)
# Set the desired value of the fourth joint variable
vrep.simxSetJointTargetPosition(clientID, joint_four_handle,
theta4 + dtheta4, vrep.simx_opmode_oneshot)
# Wait one seconds
time.sleep(1)
# Set the desired value of the fifth joint variable
vrep.simxSetJointTargetPosition(clientID, joint_five_handle,
theta5 + dtheta5, vrep.simx_opmode_oneshot)
# Wait one seconds
time.sleep(1)
# Set the desired value of the sixth joint variable
vrep.simxSetJointTargetPosition(clientID, joint_six_handle,
theta6 + dtheta6, vrep.simx_opmode_oneshot)
# Wait one seconds
time.sleep(1)
# Set the desired value of the seventh joint variable
vrep.simxSetJointTargetPosition(clientID, joint_seven_handle,
theta7 + dtheta7, vrep.simx_opmode_oneshot)
# Wait 10 seconds to see the final pose
time.sleep(5)
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Close the connection to V-REP
vrep.simxFinish(clientID)
if Temp.fitness < Best.fitness:
Best = copy.deepcopy(Temp)
num = 0
if num > sLearn:
break
return Best
def convertsensor(a): #Function which is going to convert sensors values
if a > 0.95: #When value is bigger than 0.95 function returns 0
return 0.0 #When it is seeing line then it return 1.
else:
return 1.0
vrep.simxFinish(-1) #It is closing all open connections with VREP
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1: #It is checking if connection is successful
print('Connected to remote API server')
else:
print('Connection not successful')
sys.exit('Could not connect')
#Getting motor handles
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, "left_joint", vrep.simx_opmode_oneshot_wait)
errorCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, "right_joint", vrep.simx_opmode_oneshot_wait)
sensor_h = [] #handles list
sensor_val = [] #Sensor value list
#Getting sensor handles list
def shutDown_vrep():
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
# File created by Thibaut Royer, Epitech school # [email protected] # It intends to be an example program for the "Two wheels, one arm" educative project. import vrep import math import random import time print('Start') # 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) if clientID != -1: print('Connected to remote API server') # Communication operating mode with the remote API : wait for its answer before continuing (blocking mode) # http://www.coppeliarobotics.com/helpFiles/en/remoteApiConstants.htm opmode = vrep.simx_opmode_oneshot_wait # Try to retrieve motors and robot handlers # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle ret1, wristHandle = vrep.simxGetObjectHandle(clientID, "WristMotor", opmode) ret2, elbowHandle = vrep.simxGetObjectHandle(clientID, "ElbowMotor", opmode) ret3, shoulderHandle = vrep.simxGetObjectHandle(clientID, "ShoulderMotor", opmode)
def disconnect(self):
if not self.connected:
raise RuntimeError('Client is not connected.')
vrep.simxFinish(self.client_id)
self.connected = False
x_l = x1 - P_x / 2
x_r = x0 - P_x / 2
y_p = P_y / 2 - (y0)
alpha_rad = toRadians(alpha)
beta_rad = toRadians(beta)
if x_l == x_r:
x_l += 1
x = (B * x_l) / (x_l - x_r)
y = (B * P_x * math.tan(beta_rad / 2) * y_p) / (
(x_l - x_r) * P_y * math.tan(alpha_rad / 2))
z = (B * P_x / 2) / ((x_l - x_r) * math.tan(alpha_rad / 2))
return trivial_transformation(clientID, -x, -y, z)
if __name__ == '__main__':
img0 = cv2.imread('5zed0.jpg')
img1 = cv2.imread('5zed1.jpg')
vrep.simxFinish(-1) #close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to Remote API Server...')
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
zedDistance(clientID, img1, img0)
#res, retInt, retFloat, retString, retBuffer = vrep.simxCallScriptFunction(clientID, 'misson_landing', )
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
zedDistance(0, img1, img0)
num_agents = 4 # Number of agents
em_capacity = 1000000 # Capacity of the experience memory
em_reset_frequency = 100 # Frequency for resetting the experience memory (in episodes) NOT APPLIED
batch_size = 500 # Batch size for training
training_frequency = 1 # Frequency of training (steps)
boltzmann_temperature = 6 # Boltzmann temperature
copy_weights_frequency = 100 # Frequency for copying weights from policy to target network
steps_limit = 300 # Limit of steps per episode
discount_factor = 0.9 # Discount factor
task = 1 # 1 stands for square formation, 0 stands for dispersion
num_actions = 9 # Number of actions
num_neurons = [32, 32] # Number of neurons in each of the three layers
num_lstm_units = 32 # Number of LSTM units
if __name__ == "__main__":
vrep.simxFinish(-1) # Close all open connections
client_ID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000,
5) # Connect to V-REP
assert client_ID != -1, "Could not connect to remote API server."
print(
'Connected to remote API server. Agents might move chaotically until the simulation is fully initialized.'
)
# Start simulation
vrep.simxStartSimulation(client_ID, vrep.simx_opmode_blocking)
# Initialize all agents
agent_list = [] # List containing agents
for i in range(0, num_agents):
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './simu_fleurs.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)
res, self.visible = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_visible',
vrep.simx_opmode_oneshot_wait)
#Les capteurs d'obstacles 1 2 3 4 5 6 7 8 (capteurs de l'avant du Pioneer)
res, self.sensor_1 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor1',
vrep.simx_opmode_oneshot_wait
) #il s'agit du capteur , vérifier le op_mode vrep.simx_opmode_blocking
res, self.sensor_2 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor2',
vrep.simx_opmode_oneshot_wait) #il s'agit du capteur
res, self.sensor_3 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor3',
vrep.simx_opmode_oneshot_wait) #il s'agit du capteur
res, self.sensor_4 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor4',
vrep.simx_opmode_oneshot_wait) #il s'agit du capteur
res, self.sensor_5 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor5',
vrep.simx_opmode_oneshot_wait) #il s'agit du capteur
res, self.sensor_6 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor6',
vrep.simx_opmode_oneshot_wait) #il s'agit du capteur
res, self.sensor_7 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor7',
vrep.simx_opmode_oneshot_wait) #il s'agit du capteur
res, self.sensor_8 = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor8',
vrep.simx_opmode_oneshot_wait) #il s'agit du capteur
self.set_position(self.initial_position)
vrep.simxStartSimulation(self.client_id,
vrep.simx_opmode_oneshot_wait)
# print('sensor 1',self.sensor1)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
@author: ryuhei
"""
import time
import numpy as np
import matplotlib.pyplot as plt
import vrep
import contexttimer
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', 19998, True, True, 5000, 5)
assert client_id != -1, 'Failed connecting to remote API server'
e = vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)
# print ping time
sec, msec = vrep.simxGetPingTime(client_id)
print "Ping time: %f" % (sec + msec / 1000.0)
# Handles of body and wheels
e, body = vrep.simxGetObjectHandle(client_id, 'body',
vrep.simx_opmode_oneshot_wait)
e, joint_0 = vrep.simxGetObjectHandle(client_id, 'joint_0',
vrep.simx_opmode_oneshot_wait)
def start():
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) #start my Connection
error_code =vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
return clientID, error_code
def main():
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 ----------------------------------------------------------------------------------------------
# programmable space -------------------------------------------------------------------------------------------
print("Begin calculation of H-matrix, please wait ...")
err, res, image = vrep.simxGetVisionSensorImage(
clientID, youBotCam, 0, vrep.simx_opmode_buffer)
image = colorDet.convertToCv2Format(image, res)
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
found, prime_corners = cv2.findChessboardCorners(image, (3, 4))
prime_corners = addOne(prime_corners)
# gCX are the world coordinates for the points on the chessboards which are later used to construct the h-matrix
gCX = [[0.075, 0.55, 1.0], [0.075, 0.5, 1.0], [0.075, 0.45, 1.0],
[0.025, 0.55, 1.0], [0.025, 0.5, 1.0], [0.025, 0.45, 1.0],
[-0.025, 0.55, 1.0], [-0.025, 0.5, 1.0], [-0.025, 0.45, 1.0],
[-0.075, 0.55, 1.0], [-0.075, 0.5, 1.0], [-0.075, 0.45, 1.0]]
# convert all global corners of the chessboard in egocentric world space
ego_corners = []
for gc in gCX:
newCorner = colorDet.globalToEgocentric(gc, clientID)
ego_corners.append(newCorner)
# add a 1 in every row (globalToEgocentric only returns x,y coordinates
ego_corners = addOne(ego_corners)
# convert ego_corners in numpy array
ego_corners = np.asarray(ego_corners)
# calculate H-matrix
A = getA(prime_corners, ego_corners)
H = getH(A)
newH = cv2.findHomography(prime_corners, ego_corners)
print("H-matrix cv2:", newH)
print("H-matrix own:", H / H[2][2])
blobs = colorDet.findAllBlobs(clientID, youBotCam, H)
obstacleList = []
for b in blobs:
obstacleList.append(b[0])
print("Found blobs:")
# sort blob list and print it
blobs = sortBlobsByRad(blobs)
for blob in blobs:
print("Coordinate: ", blob[0],
" Upper and lower Bound of the color: ", blob[1])
# get position of youBot and goal
roboPos, ori = move.getPos(clientID)
res, objHandle = vrep.simxGetObjectHandle(
clientID, "goal", vrep.simx_opmode_oneshot_wait)
targetPosition = vrep.simxGetObjectPosition(
clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait)
targetPosition = targetPosition[1][:2]
# drive to the goal with obstacles ahead
driveThroughPath(obstacleList, roboPos[:2], targetPosition, clientID)
# 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 clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
def __init__(self, *args, **kwargs):
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientId = vrep.simxStart(
'127.0.0.1', 19997, True, True, 5000, 5
) # Connect to V-REP, set a very large time-out for blocking commands
def main(args):
# Close all open connections (just in case)
vrep.simxFinish(-1)
# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
MLeft = np.array([[-1, 0, 0, -0.1278], [0, 0, 1, 1.3363],
[0, 1, 0, 1.2445], [0, 0, 0, 1]])
SLeft = np.zeros((6, 7))
SLeft[:, 0] = screw(0, 0, 1, -0.1278, 0.2630, 0)
SLeft[:, 1] = screw(-1, 0, 0, -0.1278, 0.310, 1.3244)
SLeft[:, 2] = screw(0, 1, 0, -0.1278, 0.4140, 1.3244)
SLeft[:, 3] = screw(-1, 0, 0, -0.1278, 0.6765, 1.2554)
SLeft[:, 4] = screw(0, 1, 0, -0.1278, 0.7801, 1.2554)
SLeft[:, 5] = screw(-1, 0, 0, -0.1278, 1.0508, 1.2454)
SLeft[:, 6] = screw(0, 1, 0, -0.1278, 1.1667, 1.2454)
MRight = np.array([[0, 0, 1, 1.332], [1, 0, 0, -0.12287],
[0, 1, 0, 1.2445], [0, 0, 0, 1]])
SRight = np.zeros((6, 7))
SRight[:, 0] = screw(0, 0, 1, 0.2387, -0.1230, 0)
SRight[:, 1] = screw(0, 1, 0, 0.3077, -0.1230, 1.3244)
SRight[:, 2] = screw(1, 0, 0, 0.4097, -0.1230, 1.3244)
SRight[:, 3] = screw(0, 1, 0, 0.6722, -0.1230, 1.2554)
SRight[:, 4] = screw(1, 0, 0, 0.7758, -0.1230, 1.2554)
SRight[:, 5] = screw(0, 1, 0, 1.0465, -0.1230, 1.2454)
SRight[:, 6] = screw(1, 0, 0, 1.1624, -0.1230, 1.2454)
# To mirror the arms, negate the thetas of joints 1, 3, and 5
setOne = [-20, 10, -30, 20, -40, -30, 45]
moveArmsAndFrames(clientID, MLeft, SLeft, MRight, SRight, setOne)
time.sleep(2)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(2)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
setTwo = [60, 40, 0, 10, -70, -30, 45]
moveArmsAndFrames(clientID, MLeft, SLeft, MRight, SRight, setTwo)
time.sleep(2)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(2)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# setThree = setTwo
setThree = [-10, 30, -10, 20, -20, -20, 5]
moveArmsAndFrames(clientID, MLeft, SLeft, MRight, SRight, setThree)
# Let all animations finish
time.sleep(2)
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Close the connection to V-REP
vrep.simxFinish(clientID)
def end(clientID):
#end and cleanup
error_code =vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(clientID)
return error_code
def closeConnection(clientID):
vrep.simxGetPingTime(clientID)
vrep.simxFinish(clientID)
# dados += str(vLeft/2)
# dados += '\t'
# dados += '\t'
# if(vRight < 0):
# dados += str(0)
# dados += '\t'
# dados += '\t'
# else:
# dados += str(vRight/2)
contador += 1
# print(dados)
# atualiza velocidades dos motores
erro = vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle,
vLeft / 1.4,
vrep.simx_opmode_streaming)
erro = vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle,
vRight / 1.4,
vrep.simx_opmode_streaming)
vrep.simxFinish(clientID) # fechando conexao com o servidor
# print 'Conexao fechada!'
arquivo = open(
'/home/jgfilho/Documentos/UFRN/2017.2/Inteligência Artificial/vrep_robo/Python/data_set.txt',
'w')
arquivo.write(dados)
arquivo.close()
else:
print 'Problemas para conectar o servidor!'
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 main():
# finish first
vrep.simxFinish(-1)
# connect to server
clientID = vrep.simxStart("127.0.0.1", 19997, True, True, 5000, 5)
if clientID != -1:
print("Connect Succesfully.")
else:
print("Connect failed.")
assert 0
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# get handles
_, vision_handle_0 = vrep.simxGetObjectHandle(clientID, "zed_vision0",
vrep.simx_opmode_blocking)
_, vision_handle_1 = vrep.simxGetObjectHandle(clientID, "zed_vision1",
vrep.simx_opmode_blocking)
_, controller_handle = vrep.simxGetObjectHandle(clientID,
"Quadricopter_target",
vrep.simx_opmode_blocking)
_, base_handle = vrep.simxGetObjectHandle(clientID, "Quadricopter",
vrep.simx_opmode_blocking)
# set Controller
synchronous_flag = True
time_interval = 0.05
flight_controller = Controller(
clientID=clientID,
base_handle=base_handle,
controller_handle=controller_handle,
vision_handle_0=vision_handle_0,
vision_handle_1=vision_handle_1,
synchronous=synchronous_flag,
time_interval=time_interval,
v_max=0.0305,
v_add=0.0005,
v_sub=0.0005,
v_min=0.03,
v_constant=0.02,
use_constant_v=False,
)
# set controller position
base_position = flight_controller.getPosition('base')
flight_controller.setPosition('controller', base_position)
# start simulation
vrep.simxSynchronous(clientID, synchronous_flag)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
target_name = "Bill#2"
photo_num = 0
while True:
# 巡航搜索
search_points = [[-4.175, -0.2]]
photo_interval = 50
photo_count = photo_interval
find_flag = False
pos_now = np.array(flight_controller.getPosition('controller')[:2])
start_num = find_nearest_point_num(pos_now, search_points)
last_position = [4.65, -7.4]
while True:
for target_point_0 in search_points[start_num:]:
target_point = copy.deepcopy(target_point_0)
target_point.append(base_position[2])
target_point = np.array(target_point)
flight_controller.moveTo(target_point, 1, 1, True)
while flight_controller.left_time > 0:
if photo_count >= photo_interval:
flight_controller.to_take_photos()
photo_count = 0
else:
photo_count += 1
result = flight_controller.step_forward_move()
if 'photos' in result:
cv2.imwrite("task_3/" + str(photo_num) + "zed0.jpg",
result['photos'][0])
cv2.imwrite("task_3/" + str(photo_num) + "zed1.jpg",
result['photos'][1])
pos = resNet(result['photos'][1], result['photos'][0],
4, clientID)
print(pos)
if pos is not None:
target_position = pos
find_flag = True
break
photo_num += 1
if find_flag:
break
if find_flag:
break
start_num = 0
if find_flag:
break
print("Find target", target_position)
flight_controller.moving_queue = Queue()
# _, target_handle = vrep.simxGetObjectHandle(clientID, target_name, vrep.simx_opmode_blocking)
# _, target_position = vrep.simxGetObjectPosition(clientID, target_handle, -1, vrep.simx_opmode_blocking)
target_position[2] = base_position[2]
target_position = np.array(target_position)
time_interval = 5
people_chooser = PeopleChoose(pos_threshold=1,
ori_threshold=0,
color_threshold=None)
people_chooser.last_position = target_position[:2]
print("Target:", target_position)
while True:
print(photo_num)
move_to_position = copy.deepcopy(target_position)
move_to_position[1] += 3
print(target_position, move_to_position)
flight_controller.moveTo(np.array(move_to_position), 1, 1, True)
flight_controller.to_take_photos()
for i in range(time_interval):
result = flight_controller.step_forward_move()
if 'photos' in result:
cv2.imwrite("task_3_2/" + str(photo_num) + "zed0.jpg",
result['photos'][0])
cv2.imwrite("task_3_2/" + str(photo_num) + "zed1.jpg",
result['photos'][1])
photo_0 = cv2.imread("task_3_2/" + str(photo_num) + "zed0.jpg")
photo_1 = cv2.imread("task_3_2/" + str(photo_num) + "zed1.jpg")
pos_list = get_people_pos(clientID, photo_1, photo_0)
photo_num += 1
next_position = people_chooser.find_next_position(pos_list)
next_position.append(target_position[2])
target_position = np.array(next_position)
print("Target:", target_position)
def disconnect():
# vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
