def worker(port_num, input_queue, output_queue, conf, logger):
name = multiprocessing.current_process().name
clientID = start_simulator(port_num)
if clientID!=-1:
joint_handles = np.zeros((conf['num_joints'],1),dtype=np.float64)
particle_handles = np.zeros((conf['num_particles'],1),dtype=np.float64)
# get object handle for joints
for i in range(1,conf['num_joints']+1):
stringJoint = 'LBR_iiwa_14_R820_joint' +str(round(i))
print(stringJoint)
returnCode,joint_handles[i-1] = vrep.simxGetObjectHandle(clientID,stringJoint, vrep.simx_opmode_oneshot_wait)
if returnCode < 0:
print('Can not find all joint handles!' + str(returnCode))
logger.error('Can not find all joint handles!' + str(returnCode))
print(joint_handles)
#get end-effector handle
returnCode, end_effector_handle = vrep.simxGetObjectHandle(clientID, 'pandaHand_attachment', vrep.simx_opmode_blocking)
# get object handle for particles
for i in range(conf['num_particles']):
# From Sphere0 to Sphere19
Particle = 'Sphere' +str(round(i))
returnCode,particle_handles[i] = vrep.simxGetObjectHandle(clientID,Particle, vrep.simx_opmode_blocking)
if returnCode < 0:
logger.warning('Can not find particle handles! + str(returnCode)')
# get object handle for cup
returnCode,cup_handle = vrep.simxGetObjectHandle(clientID, 'Cup0', vrep.simx_opmode_blocking)
if returnCode != 0:
print('Can not find cup handles! ' + str(returnCode))
collision_handle = []
returnCode, collision_handle1 = vrep.simxGetCollisionHandle(clientID, "Collision", vrep.simx_opmode_blocking)
returnCode, collision_handle2 = vrep.simxGetCollisionHandle(clientID, "Collision0", vrep.simx_opmode_blocking)
collision_handle.append(collision_handle1)
collision_handle.append(collision_handle2)
if returnCode != 0:
logger.error('Can not find collision handles! ' + str(returnCode))
conf.update({'clientID':clientID, 'joint_handles':joint_handles, 'collision_handle':collision_handle, 'end_effector_handle': end_effector_handle, 'cup_handle':cup_handle, 'particle_handles':particle_handles})
# ----------------------------------------------Initialization is done -----------------------------------------------
while True:
reinitialization_process(conf)
query_point = input_queue.get() # This will automatically block until the worker gets the content
query_point = query_point.reshape((conf['num_joints'],-1))
# -----------------------------------------------start evaluation-----------------------------------------------------
cost, record_episodic_spillage, episodic_spillage, record_step_spillage, step_spillage, max_cup_rotation_angle, final_cup_resting_angle = evaluation_and_stat(query_point, conf)
# transmit the answer to main process
print(name + '\'s cost: %f' %cost)
print('Max_cup_roataion_angle : {}'.format(max_cup_rotation_angle))
print('Final angle : {}'.format(final_cup_resting_angle))
print('episodic_spillage : {}'.format(episodic_spillage))
query_point = query_point.reshape((1, -1)).squeeze()
output_queue.put([query_point[0], cost, record_episodic_spillage, episodic_spillage, record_step_spillage, step_spillage, max_cup_rotation_angle, final_cup_resting_angle ])
else:
print('Connection failed')
sys.exit('could not connect') # Let python script exit
def setup_devices():
""" Assign the devices from the simulator to specific IDs """
global robotID, left_motorID, right_motorID, ultraID, rewardRefID, goalID, wall0_collisionID, wall1_collisionID, wall2_collisionID, wall3_collisionID, target_collisionID
# res: result (1(OK), -1(error), 0(not called))
# robot
res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT)
# motors
res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT)
res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#',
WAIT)
# ultrasonic sensors
for idx, item in enumerate(config.ultra_distribution):
res, ultraID[idx] = vrep.simxGetObjectHandle(clientID, item, WAIT)
# reward reference distance object
res, rewardRefID = vrep.simxGetDistanceHandle(clientID, 'Distance#', WAIT)
# if res == vrep.simx_return_ok: # [debug]
# print("vrep.simxGetDistanceHandle executed fine")
# goal reference object
res, goalID = vrep.simxGetObjectHandle(clientID, 'Dummy#', WAIT)
# collision object
res, target_collisionID = vrep.simxGetCollisionHandle(
clientID, "targetCollision#", BLOCKING)
res, wall0_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall0#", BLOCKING)
res, wall1_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall1#", BLOCKING)
res, wall2_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall2#", BLOCKING)
res, wall3_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall3#", BLOCKING)
# start up devices
# wheels
vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING)
vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING)
# pose
vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI)
vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI)
# reading-related function initialization according to the recommended operationMode
for i in ultraID:
vrep.simxReadProximitySensor(clientID, i, STREAMING)
vrep.simxReadDistance(clientID, rewardRefID, STREAMING)
vrep.simxReadCollision(clientID, wall0_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall1_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall2_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall3_collisionID, STREAMING)
vrep.simxReadCollision(clientID, target_collisionID, STREAMING)
def _load_scene_handles(self):
def handle_failure(name):
raise SimulatorException(
'Failed to get handle for {}'.format(name))
# load all objects
print 'Loading all objects...'
object_handles = {}
for obj_name in LINKS + JOINTS + PROXIMITY_SENSORS + [
TIP_OBJECT, GOAL_OBJECT
]:
code, handle = vrep.simxGetObjectHandle(self._client_id, obj_name,
vrep.simx_opmode_blocking)
if code == vrep.simx_return_ok:
object_handles[obj_name] = handle
else:
handle_failure(obj_name)
# load collisions -- use the same map as for objects
print 'Loading all collisions...'
for coll_name in COLLISION_OBJECTS:
code, handle = vrep.simxGetCollisionHandle(
self._client_id, coll_name, vrep.simx_opmode_blocking)
if code == vrep.simx_return_ok:
object_handles[coll_name] = handle
else:
handle_failure(coll_name)
for index, joint in enumerate(JOINTS):
self._joint_positions[index] = (object_handles[joint], 0)
return object_handles
def _chk_handle_list(self):
assert self.clientID != -1, 'Failed to connect to the V-rep server'
self.handle_robot = vrep.simxGetObjectHandle(
self.clientID, 'UR3', vrep.simx_opmode_blocking)[1]
# Tray Handle
self.handle_tray = vrep.simxGetObjectHandle(
self.clientID, 'Tray', vrep.simx_opmode_blocking)[1]
# End effector Handle
self.handle_end = vrep.simxGetObjectHandle(
self.clientID, 'End_Effector', vrep.simx_opmode_blocking)[1]
# Object Handle
for i in OBJ_LIST:
self.handle_obj.append(
vrep.simxGetObjectHandle(self.clientID, i,
vrep.simx_opmode_blocking)[1])
# UR3 Joint Handle
for i in range(6):
self.handle_joint.append(
vrep.simxGetObjectHandle(self.clientID,
"UR3_joint{}".format(i + 1),
vrep.simx_opmode_blocking)[1])
# Vision camera Handle
for i in range(2):
self.handle_cam.append(
vrep.simxGetObjectHandle(self.clientID,
'Vision_sensor_0%d' % (i + 1),
vrep.simx_opmode_blocking)[1])
# Collision Handle
for i in range(124):
self.handle_collision.append(
vrep.simxGetCollisionHandle(self.clientID, 'Collision%d' % i,
vrep.simx_opmode_blocking)[1])
def setup_devices():
""" Assign the devices from the simulator to specific IDs """
global robotID, left_motorID, right_motorID, kinect_depthID, collisionID
# res: result (1(OK), -1(error), 0(not called))
# robot
res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT)
# people
# motors
res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT)
res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#',
WAIT)
# kinetic
res, kinect_depthID = vrep.simxGetObjectHandle(clientID, 'kinect_depth#',
WAIT)
# if res == vrep.simx_return_ok: # [debug]
# print("vrep.simxGetDistanceHandle executed fine")
# collision object
res, collisionID = vrep.simxGetCollisionHandle(clientID, "Collision#",
BLOCKING)
# start up devices
# wheels
vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING)
vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING)
# pose
vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI)
vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI)
# collision
vrep.simxReadCollision(clientID, collisionID, STREAMING)
# kinect
vrep.simxGetVisionSensorDepthBuffer(clientID, kinect_depthID, STREAMING)
return
def __init__(self):
# 建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
_, collisionHandle = vrep.simxGetCollisionHandle(self.clientID, 'Collision', vrep.simx_opmode_blocking)
_, self.end1_Handle = vrep.simxGetObjectHandle(self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.end2_Handle = vrep.simxGetObjectHandle(self.clientID, 'end0', vrep.simx_opmode_blocking)
_, self.goal1_Handle = vrep.simxGetObjectHandle(self.clientID, 'goal_1', vrep.simx_opmode_blocking)
_, self.goal2_Handle = vrep.simxGetObjectHandle(self.clientID, 'goal_2', vrep.simx_opmode_blocking)
_, self.ball1Handle = vrep.simxGetObjectHandle(self.clientID, 'ball1',
vrep.simx_opmode_blocking)
_, self.ball2Handle = vrep.simxGetObjectHandle(self.clientID, 'ball2',
vrep.simx_opmode_blocking)
_,self.base1Handle = vrep.simxGetObjectHandle(self.clientID, 'base', vrep.simx_opmode_blocking)
_,self.base2Handle = vrep.simxGetObjectHandle(self.clientID, 'base0', vrep.simx_opmode_blocking)
_,self.distanceHandle=vrep.simxGetDistanceHandle(self.clientID, 'robots_dist', vrep.simx_opmode_blocking)
_, self.collisionHandle = vrep.simxGetCollisionHandle(self.clientID, 'Collision', vrep.simx_opmode_blocking)
print('Handles available!')
# _, self.pos1 = vrep.simxGetObjectPosition(self.clientID,self.ball1Handle, -1, vrep.simx_opmode_blocking)
# _, self.pos2 = vrep.simxGetObjectPosition(self.clientID, self.ball2Handle, -1, vrep.simx_opmode_blocking)
self.pos1 = [-0.5750,0.2959,0.0750] # robot1末端初始位置
self.pos2 = [0.5750,-0.2959,0.0750] # robot2末端初始位置
_, self.goal_1 = vrep.simxGetObjectPosition(self.clientID, self.goal1_Handle,self.base1Handle, vrep.simx_opmode_blocking)
_, self.goal_2 = vrep.simxGetObjectPosition(self.clientID, self.goal2_Handle,self.base2Handle, vrep.simx_opmode_blocking)
del self.goal_1[2]
del self.goal_2[2]
self.goal_1 = np.array(self.goal_1)
self.goal_2 = np.array(self.goal_2)
# 获取 joint 句柄
self.robot1_jointHandle = np.zeros((self.jointNum,), dtype=np.int) # joint 句柄
self.robot2_jointHandle = np.zeros((self.jointNum,), dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(self.clientID, self.jointName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_jointHandle[i] = returnHandle
def connect():
print("Connecting to remote API server")
del clientID[:]
lan_ip = socket.gethostbyname(socket.gethostname())
vrep.simxFinish(19997) # just in case, close all opened connections
#ID = vrep.simxStart('192.168.0.3', 19997, True, True, 5000, 5) #IP address
ID = vrep.simxStart(lan_ip, 19997, True, True, 5000, 5) #IP address
clientID.append(ID)
vrep.simxStartSimulation(clientID[0], vrep.simx_opmode_oneshot)
if (clientID == -1):
print('Not connected')
print('Connect again')
else:
del Objects_handle[:]
del UR3_joint_handle[:]
del UR3_handle[:]
del Tray_handle[:]
del vision_sensor[:]
del collision_handle[:]
del end_effector[:]
#Object Handle
for i in Objects_name:
err, Handle = vrep.simxGetObjectHandle(clientID[0], i,
vrep.simx_opmode_blocking)
Objects_handle.append(Handle)
#UR3 Handle
err, UR3 = vrep.simxGetObjectHandle(clientID[0], 'UR3',
vrep.simx_opmode_blocking)
UR3_handle.append(UR3)
#Tray Handle
err, Tray = vrep.simxGetObjectHandle(clientID[0], 'Tray',
vrep.simx_opmode_blocking)
Tray_handle.append(Tray)
#End Effector Handle - endeffector
err, end = vrep.simxGetObjectHandle(clientID[0], 'End_Effector',
vrep.simx_opmode_blocking)
end_effector.append(end)
#UR3 Joint Handle
for i in range(1, 7):
err, Joint_handle = vrep.simxGetObjectHandle(
clientID[0], "UR3_joint%d" % i, vrep.simx_opmode_oneshot_wait)
UR3_joint_handle.append(Joint_handle)
#Vision camera Handle
for i in range(1, 3):
err, vision = vrep.simxGetObjectHandle(clientID[0],
'Vision_sensor_0%d' % i,
vrep.simx_opmode_blocking)
vision_sensor.append(vision)
#Collision Handle
for i in range(0, 14):
err, collision = vrep.simxGetCollisionHandle(
clientID[0], 'Collision%d' % i, vrep.simx_opmode_oneshot_wait)
collision_handle.append(collision)
#Start Simulation
vrep.simxStartSimulation(clientID[0], vrep.simx_opmode_oneshot)
def getCollisionlib(clientID):
collision_list = ['CL', 'CR']
collision_lib = {}
for item in collision_list:
res, collision = vrep.simxGetCollisionHandle(clientID, item,
vrep.simx_opmode_blocking)
collision_lib[item] = collision
return collision_lib
def CollisionHandle(clientID, objectList):
# Get all the handles at once
shape = len(objectList)
errorcode =[]
handle = []
for x in range(1,shape+1):
get_errorCode, get_handle = vrep.simxGetCollisionHandle(clientID, objectList[x-1], vrep.simx_opmode_blocking)
errorcode.append(get_errorCode)
handle.append(get_handle)
return errorcode, handle
def CollisionHandle(clientID, objectList):
# Get all the handles at once
shape = len(objectList)
errorcode = []
handle = []
for x in range(1, shape + 1):
get_errorCode, get_handle = vrep.simxGetCollisionHandle(
clientID, objectList[x - 1], vrep.simx_opmode_blocking)
errorcode.append(get_errorCode)
handle.append(get_handle)
return errorcode, handle
def get_collision_handles(clientID):
# Get "handle" to the UR3 to UR3#0 collision object
result, collision_Handle_1 = vrep.simxGetCollisionHandle(clientID, 'Collision', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for UR3 to UR3#0 collision object')
# Get "handle" to the UR3 to sofa collision object
result, collision_Handle_2 = vrep.simxGetCollisionHandle(clientID, 'Collision0', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for UR3 to MicoHand#0 collision object')
# Get "handle" to the UR3#0 to sofa collision object
result, collision_Handle_3 = vrep.simxGetCollisionHandle(clientID, 'Collision1', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for MicoHand to UR3#0 collision object')
# Get "handle" to the UR3 to UR3 collision object
result, collision_Handle_4 = vrep.simxGetCollisionHandle(clientID, 'Collision2', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for MicoHand to MicoHand#0 collision object')
collision_handles = np.array([collision_Handle_1, collision_Handle_2, collision_Handle_3, collision_Handle_4])
return collision_handles
def obtener_punteros_a_actuadores():
global leftMotor, rightMotor, proximitySensors, collision
#Motores, rotores, ruedas
errorCode, leftMotor = vrep.simxGetObjectHandle(
clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
errorCode, rightMotor = vrep.simxGetObjectHandle(
clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
#Evento de Collision
errorCode, collision = vrep.simxGetCollisionHandle(
clientID, 'Collision', vrep.simx_opmode_oneshot_wait)
#Vector de sensores
for i in range(0, 8):
errorCode, proximitySensors[i] = vrep.simxGetObjectHandle(
clientID, 'ePuck_proxSensor' + str(i + 1),
vrep.simx_opmode_oneshot_wait)
def getCollisionlib(clientID):
collision_listL = ['C1L', 'C2L', 'C3L', 'C4L', 'C5L', 'C6L', 'C7L']
collision_listR = ['C1R', 'C2R', 'C3R', 'C4R', 'C5R', 'C6R', 'C7R']
collision_list = ['CL', 'CR']
collisionL_handle = []
collisionR_handle = []
collision_lib = []
for item in collision_list:
res, collision = vrep.simxGetCollisionHandle(clientID, item, vrep.simx_opmode_blocking)
collision_lib.append(collision)
#for item in collision_listL:
#res, collision = vrep.simxGetCollisionHandle(clientID, item, vrep.simx_opmode_blocking)
#collisionL_handle.append(collision)
#collision_lib.append(collisionL_handle)
#for item in collision_listR:
# res, collision = vrep.simxGetCollisionHandle(clientID, item, vrep.simx_opmode_blocking)
#collisionR_handle.append(collision)
#collision_lib.append(collisionR_handle)
return collision_lib
def get_collision_handle(self, name): handle, = self.RAPI_rc(vrep.simxGetCollisionHandle(self.cID, name, vrep.simx_opmode_blocking)) return handle
from keras.models import Sequential
from keras.layers.core import Dense, Activation, Dropout
from keras.optimizers import RMSprop
#Pre-Allocation
PI=math.pi #pi=3.14..., constant
GAMMA = 0.9
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)
modelHandle = vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx',vrep.simx_opmode_blocking)
collisionHandle = vrep.simxGetCollisionHandle(clientID,'Pioneer_p3dx',vrep.simx_opmode_blocking)
cH = collisionHandle[0]
print(cH)
if clientID!=-1: #check if client connection successful
print ('Connected to remote API server')
else:
print ('Connection not successful')
sys.exit('Could not connect')
#retrieve motor handles
errorCodeLeftMotor,left_motor_handle=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_blocking)
errorCodeRightMotor,right_motor_handle=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
#Car handlers
errorCode, cam_handle = vrep.simxGetObjectHandle(clientID, 'kinect_depth',
vrep.simx_opmode_oneshot_wait)
errorCode, motor_handle = vrep.simxGetObjectHandle(
clientID, 'motor_joint#0', vrep.simx_opmode_oneshot_wait)
errorCode, steer_handle = vrep.simxGetObjectHandle(
clientID, 'steer_joint#0', vrep.simx_opmode_oneshot_wait)
errorCode, fl_handle = vrep.simxGetObjectHandle(clientID, 'fl_brake_joint#0',
vrep.simx_opmode_oneshot_wait)
errorCode, fr_handle = vrep.simxGetObjectHandle(clientID, 'fr_brake_joint#0',
vrep.simx_opmode_oneshot_wait)
errorCode, bl_handle = vrep.simxGetObjectHandle(clientID, 'bl_brake_joint#0',
vrep.simx_opmode_oneshot_wait)
errorCode, br_handle = vrep.simxGetObjectHandle(clientID, 'br_brake_joint#0',
vrep.simx_opmode_oneshot_wait)
errorCode, collision_handle1 = vrep.simxGetCollisionHandle(
clientID, 'Collision1', vrep.simx_opmode_blocking)
errorCode, collision_handle2 = vrep.simxGetCollisionHandle(
clientID, 'Collision2', vrep.simx_opmode_blocking)
errorCode, collision_handle3 = vrep.simxGetCollisionHandle(
clientID, 'Collision3', vrep.simx_opmode_blocking)
errorCode, collisionState1 = vrep.simxReadCollision(clientID,
collision_handle1,
vrep.simx_opmode_streaming)
errorCode, collisionState2 = vrep.simxReadCollision(clientID,
collision_handle2,
vrep.simx_opmode_streaming)
errorCode, collisionState3 = vrep.simxReadCollision(clientID,
collision_handle3,
vrep.simx_opmode_streaming)
#errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,cam_handle,vrep.simx_opmode_streaming)
opmode=vrep.simx_opmode_blocking
res, cuboid0Handle=vrep.simxGetObjectHandle(clientID,"Cuboid0",vrep.simx_opmode_oneshot_wait)
res, cuboidHash0Handle=vrep.simxGetObjectHandle(clientID,"Cuboid0#0",opmode)
errorCode,cuboid0=vrep.simxGetObjectHandle(clientID,'cuboid0',vrep.simx_opmode_blocking)
returnCode_1,cuboid_position=vrep.simxGetObjectPosition(clientID,cuboid0Handle,-1,vrep.simx_opmode_streaming)
print(res)
print(cuboid_position)
while True:
joint_force1(0.0)
joint_force2(1.6)
joint_force3(0.0)
joint_force4(1.75)
#returnCode=vrep.simxSetJointTargetVelocity(clientID,phantom_gripper_close_joint,0.00,vrep.simx_opmode_oneshot)
#returnCode = vrep.simxSetJointForce(clientID, phantom_gripper_close_joint,5.0, vrep.simx_opmode_oneshot)
#print(force)
operate_gripper(0)
#print(position)
res, cuboid0Handle = vrep.simxGetObjectHandle(clientID, "Cuboid0", vrep.simx_opmode_oneshot_wait)
returnCode_1, cuboid_position = vrep.simxGetObjectPosition(clientID, cuboid0Handle, -1, vrep.simx_opmode_streaming)
#print(cuboid_position)
res, landing_zone = vrep.simxGetObjectHandle(clientID,"landing_zone",vrep.simx_opmode_oneshot_wait)
returnCode_2, landing_zone_position = vrep.simxGetObjectPosition(clientID, landing_zone, -1, vrep.simx_opmode_streaming)
#print("Landing Zone:", landing_zone_position)
returnCode,handle_1=vrep.simxGetCollisionHandle(clientID,"Collision0",vrep.simx_opmode_blocking)
returnCode, collisionState=vrep.simxReadCollision(clientID,handle_1,vrep.simx_opmode_streaming)
print(collisionState)
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
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
_, ballHandle = vrep.simxGetObjectHandle(clientID, 'ball1',
vrep.simx_opmode_blocking)
_, ball0Handle = vrep.simxGetObjectHandle(clientID, 'ball2',
vrep.simx_opmode_blocking)
_, collisionHandle = vrep.simxGetCollisionHandle(clientID, 'Collision',
vrep.simx_opmode_blocking)
# 获取 joint 句柄
robot1_jointHandle = np.zeros((jointNum, ), dtype=np.int) # joint 句柄
robot2_jointHandle = np.zeros((jointNum, ), dtype=np.int) # joint 句柄
for i in range(jointNum):
_, returnHandle = vrep.simxGetObjectHandle(clientID,
jointName + str(i + 1),
vrep.simx_opmode_blocking)
robot1_jointHandle[i] = returnHandle
for i in range(jointNum):
_, returnHandle = vrep.simxGetObjectHandle(clientID,
jointName + str(i + 4),
vrep.simx_opmode_blocking)
robot2_jointHandle[i] = returnHandle
_, base1Handle = vrep.simxGetObjectHandle(clientID, 'base',
vrep.simx_opmode_blocking)
if clientID!=-1: #check if client connection successful
print ('Connected to remote API server')
else:
print ('Connection not successful')
#sys.exit('Could not connect')
#Car handlers
errorCode, cam_handle = vrep.simxGetObjectHandle(clientID, 'kinect_depth', vrep.simx_opmode_oneshot_wait)
errorCode, motor_handle=vrep.simxGetObjectHandle(clientID, 'motor_joint#0',vrep.simx_opmode_oneshot_wait)
errorCode, steer_handle=vrep.simxGetObjectHandle(clientID, 'steer_joint#0',vrep.simx_opmode_oneshot_wait)
errorCode, fl_handle=vrep.simxGetObjectHandle(clientID, 'fl_brake_joint#0',vrep.simx_opmode_oneshot_wait)
errorCode, fr_handle=vrep.simxGetObjectHandle(clientID, 'fr_brake_joint#0',vrep.simx_opmode_oneshot_wait)
errorCode, bl_handle=vrep.simxGetObjectHandle(clientID, 'bl_brake_joint#0',vrep.simx_opmode_oneshot_wait)
errorCode, br_handle=vrep.simxGetObjectHandle(clientID, 'br_brake_joint#0',vrep.simx_opmode_oneshot_wait)
errorCode, collision_handle1=vrep.simxGetCollisionHandle(clientID,'Collision1',vrep.simx_opmode_blocking)
errorCode, collision_handle2=vrep.simxGetCollisionHandle(clientID,'Collision2',vrep.simx_opmode_blocking)
errorCode, collision_handle3=vrep.simxGetCollisionHandle(clientID,'Collision3',vrep.simx_opmode_blocking)
errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_streaming)
errorCode, collisionState2=vrep.simxReadCollision(clientID,collision_handle2,vrep.simx_opmode_streaming)
errorCode, collisionState3=vrep.simxReadCollision(clientID,collision_handle3,vrep.simx_opmode_streaming)
#errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,cam_handle,vrep.simx_opmode_streaming)
#Virtual Collision Sensor
#errorCode,sensor_handle=vrep.simxGetObjectHandle(clientID,'Proximity_sensor',vrep.simx_opmode_oneshot_wait)
#errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_handle,vrep.simx_opmode_streaming)
def log_results(filename, data_collect, loss_log):
# Save the results to a file so we can graph it later.
with open('CSVresults/' + filename + '_route.csv', 'w') as data_dump:
wr = csv.writer(data_dump)
def __init__(self):
#建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
# # 设置机械臂仿真步长，为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt,\
vrep.simx_opmode_oneshot)
# # 然后打开同步模式
vrep.simxSynchronous(self.clientID, False)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# vrep.simxSynchronousTrigger(self.clientID)
#获取 joint 句柄
self.robot1_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
self.robot2_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_jointHandle[i] = returnHandle
# 获取 Link 句柄
self.robot1_linkHandle = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
self.robot2_linkHandle = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_linkHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_linkHandle[i] = returnHandle
# 获取碰撞句柄
_, self.robot1_collisionHandle = vrep.simxGetCollisionHandle(
self.clientID, 'Collision_robot1', vrep.simx_opmode_blocking)
_, self.robot2_collisionHandle = vrep.simxGetCollisionHandle(
self.clientID, 'Collision_robot2', vrep.simx_opmode_blocking)
# 获取距离句柄
# _,self.mindist_robot1_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot1',vrep.simx_opmode_blocking)
# _,self.mindist_robot2_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot2', vrep.simx_opmode_blocking)
# _,self.mindist_robots_Handle = vrep.simxGetDistanceHandle(self.clientID,'robots_dist', vrep.simx_opmode_blocking)
_, self.robot1_goal_Handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
_, self.robot2_goal_Handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot2_goal', vrep.simx_opmode_blocking)
# 获取末端句柄
_, self.end1_Handle = vrep.simxGetObjectHandle(
self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.end2_Handle = vrep.simxGetObjectHandle(
self.clientID, 'end0', vrep.simx_opmode_blocking)
_, self.goal1_Handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_1', vrep.simx_opmode_blocking)
_, self.goal2_Handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_2', vrep.simx_opmode_blocking)
print('Handles available!')
_, self.goal_1 = vrep.simxGetObjectPosition(self.clientID,
self.goal1_Handle, -1,
vrep.simx_opmode_blocking)
_, self.goal_2 = vrep.simxGetObjectPosition(self.clientID,
self.goal2_Handle, -1,
vrep.simx_opmode_blocking)
del self.goal_1[2]
del self.goal_2[2]
self.goal_1 = np.array(self.goal_1)
self.goal_2 = np.array(self.goal_2)
self.jointConfig1 = np.zeros((self.jointNum, ))
self.jointConfig2 = np.zeros((self.jointNum, ))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot2_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig2[i] = jpos
_, collision1 = vrep.simxReadCollision(self.clientID,
self.robot1_collisionHandle,
vrep.simx_opmode_blocking)
_, collision2 = vrep.simxReadCollision(self.clientID,
self.robot2_collisionHandle,
vrep.simx_opmode_blocking)
_, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,
-1, vrep.simx_opmode_blocking)
_, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,
-1, vrep.simx_opmode_blocking)
_, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle,
vrep.simx_opmode_blocking)
_, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle,
vrep.simx_opmode_blocking)
for i in range(self.jointNum):
_, returnpos = vrep.simxGetObjectPosition(
self.clientID, self.robot1_linkHandle[i], -1,
vrep.simx_opmode_blocking)
for i in range(self.jointNum):
_, returnpos = vrep.simxGetObjectPosition(
self.clientID, self.robot2_linkHandle[i], -1,
vrep.simx_opmode_blocking)
print('programing start!')
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
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("self_collision: ", col2_state)
# 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 joint handles
for i in range(0, 6):
h = get_handle(i)
joint_handles.append(h)
result, col_handle = vrep.simxGetCollisionHandle(clientID, 'Collision',
vrep.simx_opmode_blocking)
check_error(result, 'collision handle', 1)
result, col0_handle = vrep.simxGetCollisionHandle(clientID, 'Collision0',
vrep.simx_opmode_blocking)
check_error(result, 'collision0 handle', 1)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(2)
for i in range(10):
print(i)
set_joint_value(0, -np.pi / 4 + np.pi / 15 * (i + 1))
time.sleep(0.5)
set_joint_value(2, -np.pi)
def get_collision_handle(self, string):
#provides handle to the collision object in V-REP
errorCode, handle = vrep.simxGetCollisionHandle(
self.clientID, string, vrep.simx_opmode_blocking)
assert errorCode == 0, 'Getting ' + string + ' handle failed'
return handle
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)
codErro, pioneerDistDetect = vrep.simxGetDistanceHandle(
clientID, 'pioneerDistDetect', vrep.simx_opmode_oneshot)
# codErro, obstaculos = vrep.simxGetCollectionHandle(clientID, 'obstaculos', vrep.simx_opmode_oneshot)
codErro, robo = vrep.simxGetObjectHandle(clientID, 'Pionee_p3dx',
vrep.simx_opmode_oneshot)
codErro, chao = vrep.simxGetObjectHandle(clientID, 'ResizableFloor_5_25',
vrep.simx_opmode_oneshot)
posicao = vrep.simxGetObjectPosition(clientID, robo, -1,
vrep.simx_opmode_buffer)
angulos = vrep.simxGetObjectOrientation(clientID, robo, -1,
def getCollisionHandle(self, name):
returnCode, self.collision_handle = vrep.simxGetCollisionHandle(
self.clientID, name, vrep.simx_opmode_blocking)
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)
#Medidores de colisão
codErro, pioneerColDetect = vrep.simxGetCollisionHandle(
clientID, 'pioneerColDetect', operationMode=vrep.simx_opmode_blocking)
#Medidores de distância
codErro, roboObstDist = vrep.simxGetDistanceHandle(
clientID, 'roboObstDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, roboTargetDist = vrep.simxGetDistanceHandle(
clientID, 'roboTargetDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, obstaculos = vrep.simxGetCollectionHandle(
clientID, 'obstaculos#', operationMode=vrep.simx_opmode_oneshot)
codErro, robo = vrep.simxGetObjectHandle(
clientID, 'Pionee_p3dx', operationMode=vrep.simx_opmode_oneshot)
codErro, motorE = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor',
def get_collision_handle(self, name):
"""
Returns a handle of collision object defined in the scene manually
"""
return vrep.simxGetCollisionHandle(self.client_id, name, self.modes['blocking'])[1]
