def vrepPrint(client_id, message):
"""Print a message in both the python command line and on the V-REP Statusbar.
The Statusbar is the white command line output on the bottom of the V-REP GUI window.
"""
vrep.simxAddStatusbarMessage(client_id, message, vrep.simx_opmode_oneshot)
print(message)
def dualPrint(msg):
'''
Print the following message to the current console window and V-Rep Pro console.
'''
if (validConnection()):
vrep.simxAddStatusbarMessage(clientID, msg,
vrep.simx_opmode_oneshot_wait)
print(msg)
def main():
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print('Failed connecting to remote API server')
return
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)
sleep(2)
# Now retrieve streaming data (i.e. in a non-blocking fashion):
vrep.simxGetIntegerParameter(
clientID, vrep.sim_intparam_mouse_x,
vrep.simx_opmode_streaming) # Initialize streaming
err, Cub1_handle = vrep.simxGetObjectHandle(clientID, "Cub1",
vrep.simx_opmode_oneshot_wait)
err, nozzle_handle = vrep.simxGetObjectHandle(
clientID, "nozzle", vrep.simx_opmode_oneshot_wait)
# M path
# m_path(30, clientID, Cub1_handle)
#walk_to(0, 0, 0.5, clientID, Cub1_handle)
# walk_with(1, 0, 0, clientID, Cub1_handle)
#walk_with(-0.3, -0.5, 0, clientID, Cub1_handle)
walk_with(-0.3, -0.3, 0, clientID, Cub1_handle)
walk_with(0.3, 0.3, 0, clientID, Cub1_handle)
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(clientID, 'Hello V-REP!',
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)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
def eval_genomes(robot, genomes, config):
for genome_id, genome in genomes:
# Enable the synchronous mode
vrep.simxSynchronous(settings.CLIENT_ID, True)
if (vrep.simxStartSimulation(settings.CLIENT_ID,
vrep.simx_opmode_oneshot) == -1):
print('Failed to start the simulation\n')
print('Program ended\n')
return
robot.chromosome = genome
robot.wheel_speeds = np.array([])
robot.sensor_activation = np.array([])
robot.norm_wheel_speeds = np.array([])
individual = robot
start_position = None
# collistion detection initialization
errorCode, collision_handle = vrep.simxGetCollisionHandle(
settings.CLIENT_ID, 'robot_collision', vrep.simx_opmode_blocking)
collision = False
first_collision_check = True
now = datetime.now()
fitness_agg = np.array([])
scaled_output = np.array([])
net = neat.nn.FeedForwardNetwork.create(genome, config)
id = uuid.uuid1()
if start_position is None:
start_position = individual.position
distance_acc = 0.0
previous = np.array(start_position)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_streaming)
while not collision and datetime.now() - now < timedelta(
seconds=settings.RUNTIME):
# The first simulation step waits for a trigger before being executed
vrep.simxSynchronousTrigger(settings.CLIENT_ID)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_buffer)
individual.neuro_loop()
# # Traveled distance calculation
# current = np.array(individual.position)
# distance = math.sqrt(((current[0] - previous[0])**2) + ((current[1] - previous[1])**2))
# distance_acc += distance
# previous = current
output = net.activate(individual.sensor_activation)
# normalize motor wheel wheel_speeds [0.0, 2.0] - robot
scaled_output = np.array([scale(xi, 0.0, 2.0) for xi in output])
if settings.DEBUG:
individual.logger.info('Wheels {}'.format(scaled_output))
individual.set_motors(*list(scaled_output))
# After this call, the first simulation step is finished
vrep.simxGetPingTime(settings.CLIENT_ID)
# Fitness function; each feature;
# V - wheel center
V = f_wheel_center(output[0], output[1])
if settings.DEBUG:
individual.logger.info('f_wheel_center {}'.format(V))
# pleasure - straight movements
pleasure = f_straight_movements(output[0], output[1])
if settings.DEBUG:
individual.logger.info(
'f_straight_movements {}'.format(pleasure))
# pain - closer to an obstacle more pain
pain = f_pain(individual.sensor_activation)
if settings.DEBUG: individual.logger.info('f_pain {}'.format(pain))
# fitness_t at time stamp
fitness_t = V * pleasure * pain
fitness_agg = np.append(fitness_agg, fitness_t)
# dump individuals data
if settings.SAVE_DATA:
with open(settings.PATH_NE + str(id) + '_fitness.txt',
'a') as f:
f.write(
'{0!s},{1},{2},{3},{4},{5},{6},{7},{8}, {9}\n'.format(
id, scaled_output[0], scaled_output[1], output[0],
output[1], V, pleasure, pain, fitness_t,
distance_acc))
# errorCode, distance = vrep.simxGetFloatSignal(CLIENT_ID, 'distance', vrep.simx_opmode_blocking)
# aggregate fitness function - traveled distance
# fitness_aff = [distance_acc]
# behavarioral fitness function
fitness_bff = [np.sum(fitness_agg)]
# tailored fitness function
fitness = fitness_bff[0] # * fitness_aff[0]
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(settings.CLIENT_ID,
'fitness: {}'.format(fitness),
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(settings.CLIENT_ID)
print('%s fitness: %f | fitness_bff %f | fitness_aff %f' %
(str(genome_id), fitness, fitness_bff[0],
0.0)) # , fitness_aff[0]))
if (vrep.simxStopSimulation(settings.CLIENT_ID,
settings.OP_MODE) == -1):
print('Failed to stop the simulation\n')
print('Program ended\n')
return
time.sleep(1)
genome.fitness = fitness
import datetime
import time
import numpy as np
from scipy.spatial.transform import Rotation as R
if __name__ == '__main__':
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')
vrep.simxAddStatusbarMessage(
clientID, 'Hello V-REP! ' +
datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'),
vrep.simx_opmode_blocking)
cmd = cmdWrapper(clientID, 'remoteApiServer',
vrep.sim_scripttype_customizationscript)
for d in ["lf", "lh", "rf", "rh"]:
_, h1 = cmd.getObjectHandle(d + '_wire_joint')
_, h2 = cmd.getObjectHandle(d + '_wire_dummy')
_, h3 = cmd.getObjectHandle(d + '_wire_dummy2')
_, pos1 = cmd.getObjectPosition(h1, -1)
pos1 = np.array(pos1).reshape((3, 1))
_, pos2 = cmd.getObjectPosition(h2, -1)
pos2 = np.array(pos2).reshape((3, 1))
vec2 = pos2 - pos1
vec2 = vec2 / np.linalg.norm(vec2)
vec1 = np.array([0., 0., 1.]).reshape((3, 1))
import numpy as np
import vrep
import sys
import time
import funcoes
#Encerra conexoes previas
vrep.simxFinish(-1)
#Faz a conexao com o Vrep
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
#Verifica se a conexao foi efetiva
if clientID != -1:
print("Conectado ao VRep!! Obaaaaa!!!")
vrep.simxAddStatusbarMessage(clientID, "Conexao estabelecida!", operationMode=vrep.simx_opmode_oneshot)
else:
print("Nao conectado ao VRep!!!")
sys.exit("Xau!!")
#Instancia manipuladores para os atuadores(rodas) e os sensores
codErro, motorE = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_leftMotor",
vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_rightMotor",
vrep.simx_opmode_blocking)
codErro, sensorFrontal1 = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_ultrasonicSensor5", vrep.simx_opmode_blocking)
codErro, sensorFrontal2 = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_ultrasonicSensor4", vrep.simx_opmode_blocking)
codErro, sensorEsq = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_ultrasonicSensor1", vrep.simx_opmode_blocking)
codErro, sensorDir = vrep.simxGetObjectHandle(clientID,"Pioneer_p3dx_ultrasonicSensor8", vrep.simx_opmode_blocking)
if clientID!=-1:
print('Connected to remote API server via portID {}'.format(portID))
else:
print('Failed connecting to remote API server')
vrep.simxFinish(clientID)
exit(1)
#(re)start the simulation
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# get handle/pointer to all objects in the V-rep scene
err,objs=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait)
printErr(err, 'Number of objects in the scene: {}'.format(len(objs)), 'Remote API function call returned with error code: {}'.format(err))
# show message in status bar below the V-REP main window
vrep.simxAddStatusbarMessage (clientID,"Connect from Python Client",vrep.simx_opmode_oneshot_wait)
# get handles for the 4 wheel motors
err,frontleftMotorHandle=vrep.simxGetObjectHandle(clientID,"rollingJoint_fl",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for front left motor: {}'.format(frontleftMotorHandle), 'Error by getting handle for front left motor: {}'.format(err))
err,rearleftMotorHandle=vrep.simxGetObjectHandle(clientID,"rollingJoint_rl",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for rear left motor: {}'.format(rearleftMotorHandle), 'Error by getting handle for rear left motor: {}'.format(err))
err,rearrightMotorHandle=vrep.simxGetObjectHandle(clientID,"rollingJoint_rr",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for rear right motor: {}'.format(rearrightMotorHandle), 'Error by getting handle for rear right motor: {}'.format(err))
err,frontrightMotorHandle=vrep.simxGetObjectHandle(clientID,"rollingJoint_fr",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for front right motor: {}'.format(frontrightMotorHandle), 'Error by getting handle for front right motor: {}'.format(err))
# get handles for the 5 arm joints
def add_statusbar_message(self, message): self.RAPI_rc(vrep.simxAddStatusbarMessage(self.cID, message, vrep.simx_opmode_blocking))
clientID, LeftJoint_handle, 2, vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(
clientID, RightJoint_handle, -2, vrep.simx_opmode_streaming)
LASTSENSOR = 2
elif LASTSENSOR == 0:
errorCode = vrep.simxSetJointTargetVelocity(
clientID, LeftJoint_handle, -2, vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(
clientID, RightJoint_handle, 2, vrep.simx_opmode_streaming)
elif LASTSENSOR == 2:
errorCode = vrep.simxSetJointTargetVelocity(
clientID, LeftJoint_handle, 2, vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(
clientID, RightJoint_handle, -2, vrep.simx_opmode_streaming)
time.sleep(0.005)
print('FINISHED')
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(clientID, 'Hello V-REP!',
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)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
def show_msg(message):
""" send a message for printing in V-REP """
vrep.simxAddStatusbarMessage(clientID, message, WAIT)
return
import numpy as np
import vrep
import sys
import time
# Encerra conexoes previas
vrep.simxFinish(-1)
# Faz a conexao com o Vrep
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
# Verifica se a conexao foi efetiva
if clientID != -1:
print("Conectado ao VRep!! Obaaaaa!!!")
vrep.simxAddStatusbarMessage(clientID, "Conexao estabelecida!",
vrep.simx_opmode_buffer)
else:
print("Nao conectado ao VRep!!!")
sys.exit("Xau!!")
#Instancia objetos no Python para os handlers
codErro, ref = vrep.simxGetObjectHandle(clientID, 'ref',
vrep.simx_opmode_oneshot)
codErro, dummy = vrep.simxGetObjectHandle(clientID, 'dummy',
vrep.simx_opmode_oneshot)
codErro, pioneerColDetect = vrep.simxGetCollisionHandle(
clientID, 'pioneerColDetect', vrep.simx_opmode_oneshot)
def printMessage(self, message):
message = '### ' + str(datetime.datetime.now().time()) + ' | ' + message
returnCode = vrep.simxAddStatusbarMessage(self.clientID, message, vrep.simx_opmode_oneshot_wait)
return returnCode
import time
import obstaculosVRep as oVrep
import robotFunctions as RF
# Encerra conexoes previas
vrep.simxFinish(-1)
# Faz a conexao com o Vrep
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
# Verifica se a conexao foi efetiva
if clientID != -1:
print("Conectado ao VRep!! Obaaaaa!!!")
vrep.simxAddStatusbarMessage(clientID,
"Conexao estabelecida!",
operationMode=vrep.simx_opmode_oneshot)
else:
print("Nao conectado ao VRep!!!")
sys.exit("Xau!!")
#Instancia objetos no Python para os handlers
codErro, ref = vrep.simxGetObjectHandle(clientID,
'ref',
operationMode=vrep.simx_opmode_oneshot)
#Destino
codErro, target = vrep.simxGetObjectHandle(
clientID, 'target', operationMode=vrep.simx_opmode_oneshot)
# rule12 = ctrl.Rule(s1_left['jauh' ] & s2_front['sedang'] & (s7_frontr['dekat'] | s8_right['dekat']) , (outLeft['max_cepat'] , outRight['stop']))
pioneer_ctrl = ctrl.ControlSystem(
[rule1, rule2, rule3, rule4, rule5, rule6, rule7, rule8,
rule9]) #,rule10,rule11,rule12
pioneer = ctrl.ControlSystemSimulation(pioneer_ctrl)
return pioneer
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~My Program Start From Here~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ '''
#Remote API Configuration
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print("Connected to remote API server")
vrep.simxAddStatusbarMessage(clientID, "Program Loaded!",
vrep.simx_opmode_oneshot)
else:
print("Connection not successful")
sys.exit("Could not connect")
val_s = [
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1
]
#braitenbegr (belok kiri = -1*braitenberL) (X) braitenbegr (belok kanan = -1*braitenberR)
#braitenber basic
braitenbergR = [-6, -4, -3, -2]
braitenbergL = [-3, -1.5, -1, -5]
#braitenbergR=[ -0.8, -7 , -7, -0.8]
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)
print("Start synchronous simulation")
vrep.simxSynchronous(clientID, True)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
_, quadrotor_base = vrep.simxGetObjectHandle(clientID, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait)
_, quadrotor_target = vrep.simxGetObjectHandle(
clientID, "Quadricopter_target", vrep.simx_opmode_oneshot_wait)
target_position = [[
-0.14999999105930328 + 0.5 * x, 0.19999998807907104 + sin(x), 0.5
print()
print('The data of the processed orders are as follows:')
print('(Patient ID, Type of order, Arrival time, Time in queue, Time processing)')
for d in data: print(d)
print()
start_time = time.time()
# For continuos remote API use 19997, else use 19999
port = 19997
print('Starting connection...')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5) # Connect to V-REP
if clientID != -1:
print('Connected.')
# Check status bar in V-REP to confirm connection
vrep.simxAddStatusbarMessage(clientID,'Hello V-REP! (from a python script)',vrep.simx_opmode_oneshot)
if port == 19997:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Start process here
time.sleep(1) # Make sure simulation started and ready
interactWithVREP() # clientID is global
# Try this for a sample process
if False:
p1 = waitForRobot('food',11)
print('ptime = %.2f seconds' % p1)
p2 = waitForRobot('linen',6)
print('ptime = %.2f seconds' % p2)
# Pause the simulation before closing connection
if port == 19997:
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
def print_message(self, text):
"""Print a message to v-rep console"""
vrep.simxAddStatusbarMessage(self.client_id, text, ONE_SHOT_MODE)
map_grid[j, i] = 255
map_pgm += '255 '
map_pgm += '\n'
# Persists the PGM map
file = open('map.pgm', 'w')
file.write(map_pgm)
file.close()
# Persists the yaml object
file = open('scene.yaml', 'w')
file.write(yaml.dump(yaml_node_list))
file.close()
print(yaml.dump(yaml_node_list))
#---------------------------------------------------
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(clientID, 'Finished generating scene graph.',
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)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
[0,0,0,0,0,0], \
[0.5*np.pi,0,0,0,0,0], \
[0,0.25*np.pi,0,0,0,0], \
[0,0,0.5*np.pi,0,0,0], \
[0,0,0,0.5*np.pi,0,0], \
[0,0,0,0,0.5*np.pi,0], \
[0,0,0,0,0,0.5*np.pi], \
# [0,0,0.45*np.pi,0,-0.5*np.pi,0], \
[0,0.1*np.pi,0.35*np.pi,0.05*np.pi,-0.5*np.pi,0], \
])
for i in range(len(Goal_joint_angles)):
SetJointPosition(Goal_joint_angles[i])
time.sleep(0.25)
X, Y, Z = get_joint()
vrep.simxAddStatusbarMessage(
clientID, "end effector relative to base: (" + str(X[5]) + ", " +
str(Y[5]) + ", " + str(Z[5]) + ").", vrep.simx_opmode_oneshot)
time.sleep(0.25)
current_joint_angle = [0, 0, 0, 0, 0, 0]
SetJointPosition(current_joint_angle)
# X,Y,Z = get_joint()
# print(X,Y,Z)
while counter < 10.0:
break
counter = vrep.simxGetFloatSignal(clientID, "counter",
vrep.simx_opmode_buffer)[1]
if counter != previousCounter:
current_joint_angle[0] += np.pi * 2
SetJointPosition(current_joint_angle)
previousCounter = counter
time.sleep(0.5)
right_motor_handle, v,
vrep.simx_opmode_streaming)
#print('err: ',errorCode)
errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, sensor_handle, vrep.simx_opmode_streaming)
print('Sensor: ', detectionState)
if detectionState and away:
v = -v
away = False
elif not (detectionState):
away = True
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(clientID, 'Finished execution.',
vrep.simx_opmode_oneshot)
errorCode = vrep.simxSetJointTargetVelocity(clientID, left_motor_handle, 0,
vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,
0, vrep.simx_opmode_streaming)
# 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)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
# 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()
vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_streaming) # Initialize streaming
while time.time()-startTime < 5:
returnCode,data=vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_buffer) # Try to retrieve the streamed data
if returnCode==vrep.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
print ('Mouse position x: ',data) # Mouse position x is actualized when the cursor is over V-REP's window
time.sleep(0.005)
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(clientID,'Hello V-REP!',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)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
def showStatusBarMessage(self, message):
vrep.simxAddStatusbarMessage(self._clientID, message,
vrep.simx_opmode_oneshot)
import numpy as np
import vrep
import sys
import time
import funcoes
# Encerra conexoes previas
vrep.simxFinish(-1)
# Faz a conexao com o Vrep
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP
# Verifica se a conexao foi efetiva
if clientID != -1:
print("Conectado ao VRep!! Obaaaaa!!!")
vrep.simxAddStatusbarMessage(clientID, "Conexao estabelecida!", operationMode=vrep.simx_opmode_oneshot)
else:
print("Nao conectado ao VRep!!!")
sys.exit("Xau!!")
# Instancia manipuladores para os atuadores(rodas) e os sensores
codErro, motorE = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor",
vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor",
vrep.simx_opmode_blocking)
codErro, sensorFrontal1 = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor5",
vrep.simx_opmode_blocking)
codErro, sensorFrontal2 = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor4",
vrep.simx_opmode_blocking)
def eval_robot(robot, chromosome):
# Enable the synchronous mode
vrep.simxSynchronous(settings.CLIENT_ID, True)
if (vrep.simxStartSimulation(settings.CLIENT_ID, settings.OP_MODE) == -1):
print('Failed to start the simulation\n')
print('Program ended\n')
return
robot.chromosome = chromosome
individual = robot
start_position = None
fitness_agg = np.array([])
# collistion detection initialization
errorCode, collision_handle = vrep.simxGetCollisionHandle(
settings.CLIENT_ID, 'robot_collision', vrep.simx_opmode_oneshot_wait)
collision = False
now = datetime.now()
id = uuid.uuid1()
if start_position is None:
start_position = individual.position
distance_acc = 0.0
previous = np.array(start_position)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_streaming)
if settings.DEBUG: individual.logger.info('Chromosome {}'.format(individual.chromosome))
while not collision and datetime.now() - now < timedelta(seconds=settings.RUNTIME):
# The first simulation step waits for a trigger before being executed
# start_time = time.time()
vrep.simxSynchronousTrigger(settings.CLIENT_ID)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_buffer)
individual.loop()
# # Traveled distance calculation
# current = np.array(individual.position)
# distance = math.sqrt(((current[0] - previous[0])**2) + ((current[1] - previous[1])**2))
# distance_acc += distance
# previous = current
# After this call, the first simulation step is finished
vrep.simxGetPingTime(settings.CLIENT_ID)
# Fitness function; each feature;
# V - wheel center
V = f_wheel_center(individual.norm_wheel_speeds[0], individual.norm_wheel_speeds[1])
if settings.DEBUG: individual.logger.info('f_wheel_center {}'.format(V))
# pleasure - straight movements
pleasure = f_straight_movements(individual.norm_wheel_speeds[0], individual.norm_wheel_speeds[1])
if settings.DEBUG: individual.logger.info('f_straight_movements {}'.format(pleasure))
# pain - closer to an obstacle more pain
pain = f_pain(individual.sensor_activation)
if settings.DEBUG: individual.logger.info('f_pain {}'.format(pain))
# fitness_t at time stamp
fitness_t = V * pleasure * pain
fitness_agg = np.append(fitness_agg, fitness_t)
# elapsed_time = time.time() - start_time
# dump individuals data
if settings.DEBUG:
with open(settings.PATH_EA + str(id) + '_fitness.txt', 'a') as f:
f.write('{0!s},{1},{2},{3},{4},{5},{6},{7},{8}, {9}\n'.format(id, individual.wheel_speeds[0],
individual.wheel_speeds[1], individual.norm_wheel_speeds[0], individual.norm_wheel_speeds[1], V, pleasure, pain, fitness_t, distance_acc))
# aggregate fitness function
# fitness_aff = [distance_acc]
# behavarioral fitness function
fitness_bff = [np.sum(fitness_agg)]
# tailored fitness function
fitness = fitness_bff[0] # * fitness_aff[0]
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(settings.CLIENT_ID, 'fitness: {}'.format(fitness), 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(settings.CLIENT_ID)
print('%s fitness: %f | fitness_bff %f | fitness_aff %f' % (str(id), fitness, fitness_bff[0], 0.0)) # , fitness_aff[0]))
# save individual as object
if settings.DEBUG:
if fitness > 30:
individual.save_robot(settings.PATH_EA + str(id) + '_robot')
if (vrep.simxStopSimulation(settings.CLIENT_ID, settings.OP_MODE) == -1):
print('Failed to stop the simulation\n')
print('Program ended\n')
return
time.sleep(1)
return [fitness]
print(d)
print()
start_time = time.time()
# For continuos remote API use 19997, else use 19999
port = 19997
print('Starting connection...')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected.')
# Check status bar in V-REP to confirm connection
vrep.simxAddStatusbarMessage(clientID,
'Hello V-REP! (from a python script)',
vrep.simx_opmode_oneshot)
if port == 19997:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Start process here
time.sleep(1) # Make sure simulation started and ready
interactWithVREP() # clientID is global
# Try this for a sample process
if False:
p1 = waitForRobot('food', 11)
print('ptime = %.2f seconds' % p1)
p2 = waitForRobot('linen', 6)
print('ptime = %.2f seconds' % p2)
# Pause the simulation before closing connection
if port == 19997:
def iaProlog():
# Encerra conexoes previas
vrep.simxFinish(-1)
# Faz a conexao com o Vrep
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP
# Verifica se a conexao foi efetiva
if clientID != -1:
print("Conectado ao VRep!! Obaaaaa!!!")
vrep.simxAddStatusbarMessage(clientID, "Conexao estabelecida!", operationMode=vrep.simx_opmode_oneshot)
else:
print("Nao conectado ao VRep!!!")
sys.exit("Xau!!")
# Instancia manipuladores para os atuadores(rodas) e os sensores
codErro, motorE = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor",
vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor",
vrep.simx_opmode_blocking)
codErro, sensorFrontal1 = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor5",
vrep.simx_opmode_blocking)
codErro, sensorFrontal2 = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor4",
vrep.simx_opmode_blocking)
codErro, sensorEsq = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor1", vrep.simx_opmode_blocking)
codErro, sensorDir = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor8", vrep.simx_opmode_blocking)
# Inicializa valores para a velocidade das rodas
codErro = vrep.simxSetJointTargetVelocity(clientID, jointHandle=motorE, targetVelocity=0.5,
operationMode=vrep.simx_opmode_streaming)
codErro = vrep.simxSetJointTargetVelocity(clientID, jointHandle=motorD, targetVelocity=0.5,
operationMode=vrep.simx_opmode_streaming)
# Inicializa leitura dos sensores
returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorFrontal1,
operationMode=vrep.simx_opmode_streaming)
returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorFrontal2,
operationMode=vrep.simx_opmode_streaming)
returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorEsq,
operationMode=vrep.simx_opmode_streaming)
returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorDir,
operationMode=vrep.simx_opmode_streaming)
# Loop para funcionamento do script
for i in range(0, 10000):
# Leitura dos sensores
codF1, detStateF1, detF1, detectedObjectHandleF1, detectedSurfaceNormalVectorF1 = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorFrontal1,
operationMode=vrep.simx_opmode_buffer)
codF2, detStateF2, detF2, detectedObjectHandleF2, detectedSurfaceNormalVectorF2 = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorFrontal2,
operationMode=vrep.simx_opmode_buffer)
codEsq, detStateEsq, detEsq, detectedObjectHandleEsq, detectedSurfaceNormalVectorEsq = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorEsq,
operationMode=vrep.simx_opmode_buffer)
codDir, detStateDir, detDir, detectedObjectHandleDir, detectedSurfaceNormalVectorDir = \
vrep.simxReadProximitySensor(clientID, sensorHandle=sensorDir,
operationMode=vrep.simx_opmode_buffer)
# Calcula a norma euclidiana de cada sensor
distF1 = np.float64(np.linalg.norm(detF1)).item()
distF2 = np.float64(np.linalg.norm(detF2)).item()
distEsq = np.float64(np.linalg.norm(detEsq)).item()
distDir = np.float64(np.linalg.norm(detDir)).item()
detectados = funcoes.estadoSensores(detStateF1, detStateF2, detStateDir, detStateEsq)
print detectados
print "{0:.2f}".format(distF1), "{0:.2f}".format(distF2), "{0:.2f}".format(distDir), "{0:.2f}".format(distEsq)
# Se os sensores detectarem algum obstaculo
if detStateF1 == True or detStateF2 == True or detStateEsq == True or detStateDir == True:
# Inicializa vRodas
vRodas = []
# Monta string de consulta
strConsulta = "controle(X,Y," + str(distF1) + "," + str(distF2) + "," + str(distDir) + "," + str(distEsq) + \
"," + str(detectados[0]) + "," + str(detectados[1]) + "," + str(detectados[2]) + "," + \
str(detectados[3]) + ")"
# Faz a consulta no Prolog atraves da funcao consultaProlog passando strConsulta como parametro e guarda os
# valores em vRodas
vRodas = funcoes.consultaProlog(strConsulta)
# Se a consulta retornar valores
if vRodas:
rodaE = vRodas[1]
rodaD = vRodas[0]
print rodaE, rodaD
codErro = vrep.simxSetJointTargetVelocity(clientID, jointHandle=motorE, targetVelocity=rodaE,
operationMode=vrep.simx_opmode_streaming)
codErro = vrep.simxSetJointTargetVelocity(clientID, jointHandle=motorD, targetVelocity=rodaD,
operationMode=vrep.simx_opmode_streaming)
codErro = vrep.simxAddStatusbarMessage(clientID, "Sensor F1 = " + "{0:.2f}".format(distF1) +
" Sensor F2 = " + "{0:.2f}".format(distF2) +
" Sensor Dir = " "{0:.2f}".format(distDir) +
" Sensor Esq = " + "{0:.2f}".format(distF1),
operationMode=vrep.simx_opmode_oneshot)
else:
codErro = vrep.simxSetJointTargetVelocity(clientID, jointHandle=motorE, targetVelocity=0.5,
operationMode=vrep.simx_opmode_streaming)
codErro = vrep.simxSetJointTargetVelocity(clientID, jointHandle=motorD, targetVelocity=0.5,
operationMode=vrep.simx_opmode_streaming)
time.sleep(1)
def show_msg(message):
vrep.simxAddStatusbarMessage(clientID, message, WAIT)
return
if True: # 初始化
# vrep初始化
# vrep.simxFinish(-1) # 断开所有已经连接的remote，这里不执行，避免把matlab中的连接断开
dr20ID = vrep.simxStart("127.0.0.1", 56666, True, True, 5000, 5)
if dr20ID < 0:
print("vrep_connection_failed")
exit()
else:
print("vrep_connection_success")
handle_left_wheel = vrep.simxGetObjectHandle(dr20ID,
"dr20_leftWheelJoint_",
vrep.simx_opmode_blocking)
handle_right_wheel = vrep.simxGetObjectHandle(
dr20ID, "dr20_rightWheelJoint_", vrep.simx_opmode_blocking)
vrep.simxAddStatusbarMessage(dr20ID, "python_remote_connected\n",
vrep.simx_opmode_oneshot)
# pygame初始化
pg.init()
window = pg.display.set_mode((320, 240))
pg.display.set_caption("vrep_car_control_window")
# 正常运行
end_flag = False
while not end_flag:
pg.time.delay(30)
# 显示，背景色
window.fill((100, 100, 100))
# pygame处理
