def startvrep(): global clientID2 sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart(flask_ip, vrep_port, True, True, 5000, 5) # Get the client ID clientID2 = sim.simxStart(flask_ip, vrep_port2, True, True, 5000, 5) res = sim.simxLoadScene(clientID, scene_path, 0, sim.simx_opmode_blocking) x = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) if clientID != -1 or clientID2 != -1: #check if client connection successful print('Connected to remote API server') else: print('Connection not successful') sys.exit('Could not connect') # Initialize car control object AirHockey = air_Hockey(clientID) #for i in range(150): while True: # Start time for image process err, img = AirHockey.get_image() ret, jpeg = cv2.imencode('.jpg', img) #jpeg.tobytes() yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + jpeg.tobytes() + b'\r\n') return "startvrep"
def create_dataset(path): try: shutil.rmtree(path + "/mav0") except: print("Removed before") os.makedirs(path + "/mav0/cam0/data") os.makedirs(path + "/mav0/cam1/data") print('Argument List:', str(sys.argv[1])) sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim print('Connected to remote API server') #Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking) er, t_rightWheel = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', sim.simx_opmode_blocking) er, t_leftWheel = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', sim.simx_opmode_blocking) er, cam_handle_left = sim.simxGetObjectHandle( clientID, 'anaglyphStereoSensor_leftSensor', sim.simx_opmode_blocking) er, cam_handle_right = sim.simxGetObjectHandle( clientID, 'anaglyphStereoSensor_rightSensor', sim.simx_opmode_blocking) i = 0 fl = open(path + "/mav0/timestamps.txt", "w") while (True): err, resolution_left, colorCam_left = sim.simxGetVisionSensorImage( clientID, cam_handle_left, 0, sim.simx_opmode_oneshot_wait) err, resolution_right, colorCam_right = sim.simxGetVisionSensorImage( clientID, cam_handle_right, 0, sim.simx_opmode_oneshot_wait) img_left_1 = np.array(colorCam_left, dtype=np.uint8) img_right_1 = np.array(colorCam_right, dtype=np.uint8) img_left_1.resize([resolution_left[1], resolution_left[0], 3]) img_right_1.resize([resolution_right[1], resolution_right[0], 3]) img_left_2 = np.flipud(img_left_1) img_right_2 = np.flipud(img_right_1) img_left_3 = img_left_2[..., ::-1].copy() img_right_3 = img_right_2[..., ::-1].copy() milli_time = int(round(time.time() * 1000)) * 1000000000 st = str(milli_time) + '\n' st_left = path + '/mav0/cam0/data/{}.png'.format(milli_time) st_right = path + '/mav0/cam1/data/{}.png'.format(milli_time) cv2.imwrite(st_left, img_left_3) cv2.imwrite(st_right, img_right_3) cv2.waitKey(1) i = i + 1 fl.write(st) sim.simxSetJointTargetVelocity(clientID, t_rightWheel, 1, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, t_leftWheel, 1, sim.simx_opmode_streaming) fl.close()
def vrepInterface(port): global angles_handler angles_handler = np.zeros(12) global angles_error angles_error = np.zeros(12) global clientID clientID = 0 global inital_name print('Program started') sim.simxFinish(-1) # just in case, close all opened connections ID = sim.simxStart('127.0.0.1', port, True, True, 5000, 5) # Connect to V-REP print(ID) if ID != -1: print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(ID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) else: print('DIDNOT CONNECT!!!') intial_name = [ 'ab3', 'bc3', 'cd3', 'ab4', 'bc4', 'cd4', 'ab1', 'bc1', 'cd1', 'ab2', 'bc2', 'cd2' ] for i in range(angles_handler.shape[0]): angles_error[i], angles_handler[i] = sim.simxGetObjectHandle( clientID, intial_name[i], sim.simx_opmode_blocking) return ID
def __init__(self): ip = '127.0.0.1' port = 19997 sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart(ip, port, True, True, -5000, 5) if self.clientID == -1: import sys sys.exit('\nV-REP remote API server connection failed (' + ip + ':' + str(port) + '). Is V-REP running?') print('Connected to Remote API Server') with open( 'primitive_base/dance_primitive_library_interpole.json') as f: self.dance_primitive_library = json.load(f) self.Body = {} get_first_handles( self.clientID, self.Body) #get first handles of Nao in the virtual environment self.joint_actuator = Joint_Actuator(self.clientID, self.Body) self.init_time = time.time() self.time_sleep = 0.01 self.i = 0
def init_remote_api_server(): """ Purpose: --- This function should first close any open connections and then start communication thread with server i.e. CoppeliaSim. NOTE: In this Task, do not call the exit_remote_api_server function in case of failed connection to the server. The test_task_2a executable script will handle that condition. Input Arguments: --- None Returns: --- `client_id` : [ integer ] the client_id generated from start connection remote API, it should be stored in a global variable Example call: --- client_id = init_remote_api_server() NOTE: This function will be automatically called by test_task_2a executable before starting the simulation. """ global client_id ############## ADD YOUR CODE HERE ############## sim.simxFinish(-1) client_id = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) ################################################## return client_id
def __init__(self): try: sim.simxFinish(-1) #close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print('connect successfully') else: sys.exit("connect error") except: print('Check if CoppeliaSim is open') _, Quadcopter_target = sim.simxGetObjectHandle( clientID, 'Quadricopter_target', sim.simx_opmode_blocking) _, targetPosition = sim.simxGetObjectPosition(clientID, Quadcopter_target, -1, sim.simx_opmode_buffer) print(targetPosition) ArrayPosition = [-0.18570, 0.99366, 0.615] sim.simxSetObjectPosition(clientID, Quadcopter_target, -1, ArrayPosition, sim.simx_opmode_oneshot) self.clientID = clientID self.Quadcopter_target = Quadcopter_target self.targetPosition = targetPosition
def conect_and_load(port, ip): clientID = sim.simxStart(ip, port, True, True, 5000, 5) # Connect to CoppeliaSim scene_path = os.path.join('vrep-scene', 'poppy_two_target_pos_z.ttt') print('-' * 5, 'Scene path:', scene_path, '-' * 5) sim.simxLoadScene(clientID, scene_path, 0xFF, sim.simx_opmode_blocking) return clientID
def onConnect(self): if self.clientID != -1: return self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if self.clientID == -1: messagebox.showinfo("No se puede conectar") else: for i in range(5): j = i + 1 nom = self.base + str(j) _, handle = sim.simxGetObjectHandle(self.clientID, nom, sim.simx_opmode_blocking) self.handles.append(handle) _, self.j1 = sim.simxGetObjectHandle(self.clientID, 'youBotGripperJoint1', sim.simx_opmode_blocking) _, self.j2 = sim.simxGetObjectHandle(self.clientID, 'youBotGripperJoint2', sim.simx_opmode_blocking) print("Conectado y envio comando al motor") print(self.j1, self.j2) self.init_robot()
def __init__(self, robot, host="127.0.0.1", port=19997): self.robot = robot self.sim_joints = [0, 0, 0, 0, 0, 0] sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart(host, port, True, True, 5000, 5) # Connect to CoppeliaSim if self.clientID != -1: print('Connected to remote API server') else: print("coppeliasim connection failed") return res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) self.jh = [0, 0, 0, 0, 0, 0] for i in range(6): res, self.jh[i] = sim.simxGetObjectHandle( self.clientID, 'UR5_joint{}'.format(i + 1), sim.simx_opmode_blocking) sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
def __enter__(self): self.executedMovId1 = 'notReady' self.executedMovId2 = 'notReady' sim.simxFinish(-1) # just in case, close all opened connections self.id = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to CoppeliaSim return self
def start_Simulation(self): print('Program started') # Just in case, close all opened connections sim.simxFinish(-1) # Connect to CoppeliaSim, as continuous remote API Server self.clientID = sim.simxStart('127.0.0.1', 19997, False, True, 5000, 5) if self.clientID != -1: print('Connected to remote API server') #Start simulation sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) # Now send some data to CoppeliaSim in a non-blocking fashion: sim.simxAddStatusbarMessage(self.clientID, 'Connection Succeed!', sim.simx_opmode_oneshot) # Before closing the connection to CoppeliaSim, make sure that the last command sent # out had time to arrive. You can guarantee this with (for example): sim.simxGetPingTime(self.clientID) # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.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) else: print('Failed connecting to remote API server') return self.clientID
def __init__(self): sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if self.clientID != -1: print('Connected to remote API server') else: print('Failed connecting to remote API server')
def startStopSim(): global PORT, client, cameras if client is None: # No client, then Start PORT = int(request.args.get('port')) print("waiting") while portpicker.is_port_free(PORT): sleep(5 * againIn) print(".") client = sim.simxStart('127.0.0.1', PORT, True, True, 5000, 5) verboseResp(client, "simxStart") return str(client) else: # There is a client, then Stop stop() sleep(.5) response = sim.simxSetIntegerSignal(client, 'doClose', 1, sim.simx_opmode_oneshot_wait) verboseResp(response, "simxSetIntegerSignal doClose") try: finish() except: verboseResp("already finished?", "finish") reset() if response == -1: return "-1" else: return "0"
def conect_and_load(port, ip): sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart(ip,port,True,True,5000,5) # Connect to CoppeliaSim scene_path = os.path.join('vrep_scene', 'hexapod_scene.ttt') print('-'*5, 'Scene path:', scene_path, '-'*5) sim.simxLoadScene(clientID, scene_path, 0xFF, sim.simx_opmode_blocking) return clientID
def __enter__(self): self.intSignalName='legacyRemoteApiStepCounter' self.stepCounter=0 self.maxForce=100 sim.simxFinish(-1) # just in case, close all opened connections self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim return self
def __init__(self): sim.simxFinish(-1) self.clientID = sim.simxStart(self.DOCKER_IP, 5555, True, True, 5000, 5) if self.clientID == -1: rospy.logerr('Failed connecting to remote API server') sim.simxFinish(-1) sys.exit(1) rospy.loginfo('Connected to remote API server') rospy.loginfo('Testing connection') objs = self.run_sim_function( sim.simxGetObjects, (self.clientID, sim.sim_handle_all, sim.simx_opmode_blocking)) rospy.loginfo(f'Number of objects in the scene: {len(objs)}') self.robot = self.run_sim_function( sim.simxGetObjectHandle, (self.clientID, "Rob", sim.simx_opmode_blocking)) gate_names = ["Gate", "GateLeftChild", "GateRightChild"] self.gate = [ self.run_sim_function( sim.simxGetObjectHandle, (self.clientID, name, sim.simx_opmode_blocking)) for name in gate_names ] self.set_position_to_zero() rospy.sleep(0.1) self.init_streaming() rospy.loginfo("Starting main loop")
def __init__(self): # n_actions:3 (target pos), n_states:6 (3pos+3force) self.metadata = {'render.modes': ['human']} super().__init__() sim.simxFinish(-1) for _ in range(5): self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: # print('[INFO] Connected to CoppeliaSim.') break if self.clientID == -1: raise IOError('[ERROR] Could not connect to CoppeliaSim.') sim.simxSynchronous(self.clientID, True) # sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) self.stepCount = 0 self.reward = 0 self.n_substeps = 10 # self.sim_timestep = 0.5 # set in coppeliaSim (not implemented) self.n_actions = 3 self.n_states = 6 self.action_space = spaces.Box(-1., 1, shape=(self.n_actions, ), dtype='float32') self.observation_space = spaces.Box(-np.inf, np.inf, shape=(self.n_states, ), dtype='float32') self._getHandles() sim.simxGetPingTime(self.clientID)
def main(): print('### Program started') print('### Number of arguments:', len(sys.argv), 'arguments.') print('### Argument List:', str(sys.argv)) sim.simxFinish(-1) # just in case, close all opened connections port = int(sys.argv[1]) clientID = sim.simxStart('127.0.0.1', port, True, True, 2000, 5) if clientID == -1: print('### Failed connecting to remote API server') else: print('### Connected to remote API server') hRobot = getRobotHandles(clientID) while sim.simxGetConnectionId(clientID) != -1: # Perception sonar = getSonar(clientID, hRobot) #print ('### s', sonar) blobs, coord = getImageBlob(clientID, hRobot) nspeed = 1.25 if blobs == 1: if coord[0] > 0.5: pd = abs(0.5 - coord[0]) / 0.5 pi = 0 else: pi = (0.5 - coord[0]) / 0.5 pd = 0 if coord[1] >= 0.6: res = 0.5 else: res = 0 print('pd= ', pd, 'pi= ', pi, 'Y= ', coord[1]) lspeed, rspeed = nspeed + (1.5 * pd) - res, nspeed + (1.5 * pi) - res else: lspeed, rspeed = avoid(sonar) #lspeed, rspeed = +1.5,+0 # Planning #lspeed, rspeed = avoid(sonar) # Action setSpeed(clientID, hRobot, lspeed, rspeed) time.sleep(0.1) print('### Finishing...') sim.simxFinish(clientID) print('### Program ended')
def __enter__(self): self.intSignalName = 'legacyRemoteApiStepCounter' self.stepCounter = 0 self.lastImageAcquisitionTime = -1 sim.simxFinish(-1) # just in case, close all opened connections self.id = sim.simxStart('192.168.15.55', 19999, True, True, 5000, 5) # Connect to CoppeliaSim return self
def connect(self): print ('Programa inicio') sim.simxFinish(-1) # cerrar todas las conexiones # Conectar a CoppeliaSim self.clientID=sim.simxStart(self.ip,self.port,True,True,5000,5) if(self.clientID == -1): print("Imposible conectar") self.activo = False
def __init__(self): sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: #check if client connection successful print('Connected to remote API server') # enable the synchronous mode on the client: sim.simxSynchronous(self.clientID, True) else: print('Connection not successful') sys.exit('Could not connect') self.Vmax = 0.05 self.Wmax = np.pi / 4 # Action Space self.action_space = spaces.Discrete(3) # Observation Space self.observation_space = spaces.Box( low=np.array([0, 0, 0., -np.pi / 4, 0, 0, 0, 0, 0, 0, 0, 0], dtype=np.float32), high=np.array( [3, 2 * np.pi, 0.08, np.pi / 4, 1, 1, 1, 1, 1, 1, 1, 1], dtype=np.float32), dtype=np.float32) # Objetcs in the Simulation Scene errorCode, self.right_motor = sim.simxGetObjectHandle( self.clientID, 'K4_Right_Motor', sim.simx_opmode_oneshot_wait) errorCode, self.left_motor = sim.simxGetObjectHandle( self.clientID, 'K4_Left_Motor', sim.simx_opmode_oneshot_wait) errorCode, self.khepera = sim.simxGetObjectHandle( self.clientID, 'Khepera_IV', sim.simx_opmode_oneshot_wait) errorCode, self.target = sim.simxGetObjectHandle( self.clientID, 'Target', sim.simx_opmode_oneshot_wait) errorCode, self.camera = sim.simxGetObjectHandle( self.clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait) self.sensor = {} for i in range(8): handle = 'K4_Infrared_{}'.format(i + 1) errorCode, self.sensor[i] = sim.simxGetObjectHandle( self.clientID, handle, sim.simx_opmode_oneshot_wait) self.viewer = None self.seed() self.steps = 0 self.radius = 0.8 self.MaxSteps = 800 self.problem = True self.Velocities = [0, 0] self.Movements = [[4.285, 0.515], [4.285, 4.285], [0.515, 4.285]] self.xp, self.yp = self.getPositionTarget() self.ResetSimulationScene() self.Randomize = True self.RobotOrientationRand = True
def __init__(self): print('Program started') sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to CoppeliaSim if self.clientID != -1: print('Connected to remote API server') sim.simxSynchronous(self.clientID, True) sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
def drone_position(args): drones = [[] for i in range(3)] drones_names = [ 'Quadricopter_base', 'Quadricopter_base#0', 'Quadricopter_base#1' ] nodes = [] data = [[] for i in range(3)] if len(args) > 1: for n in range(1, len(args)): nodes.append(args[n]) else: info("No nodes defined") exit() info('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: res = None info('Connected to remote API server') # Getting the ID of the drones from the simulation for i in range(0, len(drones)): [res, drones[i]] = sim.simxGetObjectHandle(clientID, drones_names[i], sim.simx_opmode_oneshot_wait) if res == sim.simx_return_ok: info('Connected with CoppeliaSim') else: info('Remote API function call returned with error code: ', res) time.sleep(2) # Starting the getPosition function streaming for i in range(0, len(drones)): sim.simxGetObjectPosition(clientID, drones[i], -1, sim.simx_opmode_streaming) while True: # Getting the positions as buffers for i in range(0, len(drones)): # Try to retrieve the streamed data returnCode, data[i] = sim.simxGetObjectPosition( clientID, drones[i], -1, sim.simx_opmode_buffer) # Storing the position in data files for i in range(0, len(data)): send_file(data[i], nodes[i]) time.sleep(1) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID) else: info('Failed connecting to remote API server') info('Program ended')
def open_connection(self): sim.simxFinish(-1) # just in case, close all opened connections self.client_id = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if self.client_id != -1: print('Robot connected') else: print('Connection failed') return self.client_id
def initCon(): clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) #comprobamos que se haya podido conectar con CoppeliaSim if clientID != -1: print("conexión establecida!") else: sys.exit("Error: No se puede conectar") #ocurre cuando se lanza primero python y después vrep. Debe hacerse al contrario return clientID
def verificaConexao(self): conectado = sim.simxGetConnectionId(self.id) != -1 while not conectado: sim.simxFinish(-1) self.id = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim conectado = sim.simxGetConnectionId(self.id) != -1 return conectado
def move_right(): clientID2 = sim.simxStart(flask_ip, vrep_port2, True, True, 5000, 5) # Get the client ID errorCode, player_x_handle = sim.simxGetObjectHandle( clientID2, 'Pla_X_joint', sim.simx_opmode_oneshot_wait) sim.simxSetJointTargetVelocity(clientID2, player_x_handle, -1, sim.simx_opmode_oneshot_wait) sim.simxFinish(clientID2) print("move_right") return ("nothing")
def startSimulation(): sim.simxFinish(-1) clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 50) if clientID != -1: print('Connected to remote API server') else: print('Failed connecting to remote API server') sys.exit('Could not connect to remote API server') sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) return clientID
def connect(port): # Establece la conexión a VREP # port debe coincidir con el puerto de conexión en VREP # retorna el número de cliente o -1 si no puede establecer conexión sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', port, True, True, 2000, 5) # Conectarse if clientID == 0: print("conectado a", port) else: print("no se pudo conectar") return clientID
def reset(): clientID2 = sim.simxStart(flask_ip, vrep_port2, True, True, 5000, 5) # Get the client ID sim.simxStopSimulation(clientID2, sim.simx_opmode_oneshot_wait) time.sleep(1) sim.simxStartSimulation(clientID2, sim.simx_opmode_oneshot_wait) time.sleep(0.5) sim.simxFinish(clientID2) print("reset") return ("nothing")