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]