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 __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 close(self): sim.simxAddStatusbarMessage(self.clientID, 'Goodbye CoppeliaSim!', sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot) time.sleep(2) sim.simxFinish(self.clientID)
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 __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 execute(self): if(self.activo == False): return while(1): #Guardar frame de la camara, rotarlo y convertirlo a BGR imgL = self.get_image(self.camLeft) imgM = self.get_image(self.camMid) imgR = self.get_image(self.camRight) #Mostrar frame y salir con "ESC" cv2.imshow('Left', imgL) cv2.imshow('Right', imgR) cv2.imshow('Middle', imgM) tecla = cv2.waitKey(5) & 0xFF if tecla == 27: break elif tecla != 255: self.mover_robot_tecladov2(tecla) #cerrar sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot) sim.simxFinish(self.clientID)
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 execute(self): if (self.activo == False): return list_estados = [] model = load_model("modelrobotica") cont = 0 while (1): img = self.get_image(self.camSup) cv2.imshow('Image', img) image = cv2.resize(img, (32, 32)) image = img_to_array(image) image = np.array(image, dtype="float") / 255.0 image = image.reshape(-1, 32, 32, 3) prediction = np.argmax(model.predict(image)) print("model pred: {}".format(prediction), end=" ") tecla = cv2.waitKey(5) & 0xFF if tecla == 27: break self.vel_follow() cont = cont + 1 #d = self.get_distance(self.ultra,self.dmax) #print("distancia sensado: {}".format(d)) #cerrar sim.simxFinish(self.clientID)
def vision(): while (sim.simxGetConnectionId(clientID) != -1): err, resolution, image = sim.simxGetVisionSensorImage( clientID, v1, 0, sim.simx_opmode_buffer) if err == sim.simx_return_ok: img = np.array(image, dtype=np.uint8) img.resize([resolution[1], resolution[0], 3]) # image was originally upside down, turn it 180 degree img180 = cv2.flip(img, 0) #convert image from bgr -> rgb imgfinal = cv2.cvtColor(img180, cv2.COLOR_RGB2BGR) # show image cv2.imshow('image', imgfinal) # press 'q' to exit if cv2.waitKey(1) & 0xFF == ord('q'): break elif err == sim.simx_return_novalue_flag: print('no image') pass else: print(err) else: print("Failed to connect to remote API Server") sim.simxFinish(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 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): # 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 __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 close(self): if self.viewer: self.viewer.close() self.viewer = None sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking) # Now close the connection to CoppeliaSim: sim.simxFinish(self.clientID)
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 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 close_connection(self): sim.simxGetPingTime( self.client_id ) # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. sim.simxFinish( self.client_id) # Now close the connection to CoppeliaSim: print('Connection closed')
def close_connection(self): self.actuate([0]*len(self.motors)) # Before closing the connection of CoppeliaSim, # make sure that the last command sent out had time to arrive. sim.simxGetPingTime(self.client_id) sim.simxFinish(self.client_id) # Now close the connection of CoppeliaSim: print('Connection closed')
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, 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 exit_remote_api_server(): """ Purpose: --- This function should wait for the last command sent to arrive at the Coppeliasim server before closing the connection and then end the communication thread with server i.e. CoppeliaSim using simxFinish Remote API. Input Arguments: --- None Returns: --- None Example call: --- exit_remote_api_server() NOTE: This function will be automatically called by test_task_2a executable after ending the simulation. """ global client_id ############## ADD YOUR CODE HERE ############## sim.simxGetPingTime(client_id) sim.simxFinish(client_id)
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 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 __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 __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 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 __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 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