def reset(self): # connect with server if self.client is None: if self.robot_dynamics: client = airsim.CarClient() client.enableApiControl(True) client.confirmConnection() else: client = airsim.VehicleClient() self.client = client if self.blocking: self.client.simPause(True) self.time = 0 self.human_states = defaultdict(list) self.robot_states = list() if self.curriculum_learning: self.goal_distance = np.random.uniform(2, 4) self.goal_position = np.array((self.goal_distance, 0, -1)) if self.robot_dynamics: self.client.reset() else: self.client.reset() self. _move(self.initial_position, 0) self._update_states() obs = self.compute_observation() return obs
def plotEllipse(center, color="red", a=3, b=1, nPoints=20, VehicleClient=airsim.VehicleClient()): # a is semi major axis abd b is semi minor axis # nPoints is number of points you wish to define one half of ellipse with # Center h = center.x_val k = center.y_val # Coordinates at each step tailCoord = [0, 0, center.z_val] headCoord = [0, 0, center.z_val] #Create list of start and end points tailList = [] headList = [] # step is the difference in x value between each point plotted step = 2 * a / nPoints # Upper ellipse for i in range(nPoints): # Start position of the line tailCoord[0] = h - a + i * step tailCoord[1] = k + np.sqrt(b * b * (1 - ((tailCoord[0] - h)**2) / (a**2))) #End position of the line headCoord[0] = h - a + (i + 1) * step headCoord[1] = k + np.sqrt(b * b * (1 - ((headCoord[0] - h)**2) / (a**2))) # Store the point tailList.append(Vector3r(tailCoord[0], tailCoord[1], tailCoord[2])) headList.append(Vector3r(headCoord[0], headCoord[1], headCoord[2])) # Lower ellipse for i in range(nPoints): # Start position of the line tailCoord[0] = h - a + i * step tailCoord[1] = k - np.sqrt(b * b * (1 - ((tailCoord[0] - h)**2) / (a**2))) # End position of the lineamsm headCoord[0] = h - a + (i + 1) * step headCoord[1] = k - np.sqrt(b * b * (1 - ((headCoord[0] - h)**2) / (a**2))) # Store the point tailList.append(Vector3r(tailCoord[0], tailCoord[1], tailCoord[2])) headList.append(Vector3r(headCoord[0], headCoord[1], headCoord[2])) # Plot the ellipse mpcTest1AK.plotLine(tailList, headList, color)
def Plot(self, client=airsim.CarClient()): #client.simPause(True) car_state = client.getCarState() VehicleClient = airsim.VehicleClient() pos = car_state.kinematics_estimated.position #Plot red arrows persistently VehicleClient.simPlotArrows(points_start=[ Vector3r(self.startPosition[0], self.startPosition[1], self.startPosition[2]) ], points_end=[ Vector3r(self.endPosition[0], self.endPosition[1], self.endPosition[2]) ], color_rgba=[1.0, 0.0, 0.0, 1.0], arrow_size=10, thickness=5, is_persistent=True) self.startPosition = self.endPosition self.endPosition[0] -= 0.05 self.endPosition[1] += 0.05 #client.simPause(False) return 1
def __init__(self, img_benchmark_type='simGetImages', viz_image_cv2=False, save_images=False, img_type="scene"): self.airsim_client = airsim.VehicleClient() self.airsim_client.confirmConnection() self.image_benchmark_num_images = 0 self.image_benchmark_total_time = 0.0 self.avg_fps = 0.0 self.image_callback_thread = None self.viz_image_cv2 = viz_image_cv2 self.save_images = save_images self.img_type = cameraTypeMap[img_type] if img_benchmark_type == "simGetImage": self.image_callback_thread = threading.Thread( target=self.repeat_timer_img, args=(self.image_callback_benchmark_simGetImage, 0.001)) if img_benchmark_type == "simGetImages": self.image_callback_thread = threading.Thread( target=self.repeat_timer_img, args=(self.image_callback_benchmark_simGetImages, 0.001)) self.is_image_thread_active = False if self.save_images: self.tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_img_bm") print(f"Saving images to {self.tmp_dir}") try: os.makedirs(self.tmp_dir) except OSError: if not os.path.isdir(self.tmp_dir): raise
def __init__(self, config_file): self.client = airsim.VehicleClient() self.client.confirmConnection() self.config = self.load_config(config_file) self.init_dirs() self.init_vehicle_pose() self.init_instance_ids() self.init_object_combinations()
def start(self): client = airsim.VehicleClient('', self.port) client.confirmConnection() print("1111111111111111") client.simSetCameraOrientation( "0", airsim.to_quaternion(0.261799 * 3, 0, 0)) print("22222222222222222") return "complit"
def connect_airsim(): # airsim related car_client = airsim.VehicleClient() car_client.confirmConnection() car_client.enableApiControl(True) # Get the current state of the vehicle c_client = airsim.CarClient() c_client.confirmConnection() return car_client, c_client
def airsim_connect() -> airsim.VehicleClient: """ Connect to airsim client, exposing the CV mode UE4 graphic environment. :return: the airsim client object """ client = airsim.VehicleClient() client.confirmConnection() return client
def initialize(self): # Connect to airsim. self.cmdClient = airsim.VehicleClient() self.cmdClient.confirmConnection() self.imgType = [ airsim.ImageRequest(self.camID, airsim.ImageType.DepthPlanner, True) ]
def connect_to_airsim(): connected = False while not connected: timeout = False try: client = airsim.VehicleClient() client.confirmConnection() except: timeout = True if not timeout: connected = True return client
def connect(sim_mode: str): """ Returns a (Multirotor or ComputerVision) client connected to AirSim. """ assert sim_mode in [ff.SimMode.Multirotor, ff.SimMode.ComputerVision], sim_mode if sim_mode == ff.SimMode.Multirotor: client = airsim.MultirotorClient() client.confirmConnection() client.enableApiControl(True) client.armDisarm(True) else: client = airsim.VehicleClient() client.confirmConnection() return client
def plotLine(startPos, endPos, color="red", thickness=3, VehicleClient=airsim.VehicleClient()): rgba = [1.0, 0.0, 0.0, 1.0] if color == "blue": rgba = [0.0, 0.0, 1.0, 1.0] #Plot blue line from specified start point to end point VehicleClient.simPlotArrows(points_start=startPos, points_end=endPos, color_rgba=rgba, arrow_size=10, thickness=thickness, is_persistent=True)
def AirView(W, X, Y, Z, skeleton_recv, dir=1, rate=1): pp = pprint.PrettyPrinter(indent=4) client = airsim.VehicleClient() client.confirmConnection() # airsim.wait_key('Press any key to start the tracking') x_init, y_init, z_init = 0, 0, -1.6 while True: sk = skeleton_recv.recv() # print('AirSimCV received:', sk) if isinstance(sk, list) and len(sk) == 25: FacePos = sk[0] x_shift = -FacePos[2] / 50 y_shift = FacePos[0] / 212 z_shift = FacePos[1] / 256 #print("received HeadPose:", HeadPose[0]) n_w, n_qx, n_qy, n_qz = W.value, X.value, Y.value, Z.value if dir: client.simSetVehiclePose( airsim.Pose( airsim.Vector3r(x_init + x_shift, y_init + rate * y_shift, z_init + rate * z_shift), airsim.Quaternionr(n_qx, n_qy, n_qz, n_w)), True) else: client.simSetVehiclePose( airsim.Pose( airsim.Vector3r(x_init - x_shift, y_init - rate * y_shift, z_init - rate * z_shift), airsim.Quaternionr(n_qx, n_qy, n_qz, n_w)), True) elif sk == "Break": print("Tracking terminating...") client.simSetPose( airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.0, 0, 0)), True) break elif sk == 'Empty': i = 1 client.simSetPose( airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True)
def main(): client = airsim.VehicleClient() client.confirmConnection() client.enableApiControl(True, "Drone1") client.enableApiControl(True, "Drone2") client.armDisarm(True, "Drone1") client.armDisarm(True, "Drone2") tmp_dir = "airsim_drone" # Define start position, goal and size of UAV pos = [0, 5, -1] # start position x,y,z goals = [[120, 0]] # , [0, 100], [100, 100], [0, 0]] # x,y uav_size = [0.29*3, 0.98*2] # height:0.29 x width:0.98 - allow some tolerance # initial position moveUAV(client, pos, yaw=0) remove_background(client=client) find_corners('filtered.png')
def __init__(self, W, H, save=False, debug=False): self.ev_sim = EventSimulator(W, H) self.W = W self.H = H self.image_request = airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) self.client = airsim.VehicleClient() self.client.confirmConnection() self.init = True self.start_ts = None self.rgb_image_shape = [H, W, 3] self.debug = debug self.save = save self.event_file = open("events.pkl", "ab") self.event_fmt = "%1.7f", "%d", "%d", "%d" if debug: self.fig, self.ax = plt.subplots(1, 1)
def __init__(self): self.cmd_client = airsim.VehicleClient() self.cmd_client.confirmConnection() self.tf_broad_ = tf.TransformBroadcaster() self.odom_pub_ = rospy.Publisher('pose', Odometry, queue_size=1) self.cloud_pub_ = rospy.Publisher('cloud_in', PointCloud2, queue_size=1) self.camid = 0 self.img_type = [ airsim.ImageRequest(self.camid, airsim.ImageType.DepthPlanner, True) ] self.FAR_POINT = 22 self.cam_pos = [0., 0., 0.] self.fov = 95.0 self.path_skip = 7 self.last_list_len = 10 self.last_ten_goals = [[ 0., 0., 0. ]] * self.last_list_len # detect and avoid occilation self.lfg_ind = 0 self.replan_step = 1
def __init__(self): # connect to the AirSim simulator self.client = airsim.VehicleClient() self.client.confirmConnection() print('Connected!\n')
class NHtestingAK: client = airsim.CarClient() client.confirmConnection() client.enableApiControl(True) car_controls = airsim.CarControls() car_state = client.getCarState() VehicleClient = airsim.VehicleClient() pos = car_state.kinematics_estimated.position print(pos) #Let it set time.sleep(3) #Clear previous plot VehicleClient.simFlushPersistentMarkers() #Plot the path #Straight line VehicleClient.simPlotArrows( points_start=[Vector3r(pos.x_val, pos.y_val, 0)], points_end=[Vector3r(68, 0, 0)], color_rgba=[1.0, 0.0, 0.0, 1.0], arrow_size=10, thickness=10, is_persistent=True) car_state = client.getCarState() pos = car_state.kinematics_estimated.position # go forward car_controls.throttle = 0.75 car_controls.steering = 0 client.setCarControls(car_controls) print("Go Forward") # time.sleep(9.5) # let car drive a bit while pos.x_val < 68: car_state = client.getCarState() pos = car_state.kinematics_estimated.position car_state = client.getCarState() pos = car_state.kinematics_estimated.position print(pos) # Go forward + steer left car_controls.throttle = 0.5 car_controls.steering = -1 client.setCarControls(car_controls) print("Go Forward, steer left") time.sleep(1.463) # let car drive a bit car_state = client.getCarState() pos = car_state.kinematics_estimated.position print(pos) #Get current position of the car and plot accordingly car_state = client.getCarState() pos = car_state.kinematics_estimated.position #Clear previous plot VehicleClient.simFlushPersistentMarkers() #Left line VehicleClient.simPlotArrows( points_start=[Vector3r(pos.x_val, pos.y_val, 0)], points_end=[Vector3r(pos.x_val, -120, 0)], color_rgba=[1.0, 0.0, 0.0, 1.0], arrow_size=10, thickness=10, is_persistent=True) # go forward car_controls.throttle = 0.75 car_controls.steering = 0 client.setCarControls(car_controls) print("Go Forward") time.sleep(7) # let car drive a bit # apply brakes car_controls.brake = 1 client.setCarControls(car_controls) print("Apply brakes") time.sleep(3) # let car drive a bit car_controls.brake = 0 #remove brake car_state = client.getCarState() pos = car_state.kinematics_estimated.position print(pos) #restore to original state client.reset() client.enableApiControl(False)
def main(args): client = airsim.VehicleClient() client.confirmConnection() ts = datetime.datetime.now().isoformat()[:-7].replace(':', '') tmp_dir = os.path.join(args.out_path, args.env, ts) print("Saving images to %s" % tmp_dir) try: os.makedirs(tmp_dir) except OSError: if not os.path.isdir(tmp_dir): raise nseqs = 3600 h5file = tables.open_file(os.path.join(tmp_dir, 'output.h5'), mode="w", title="Flythroughs") seq_len = 40 short_seq_len = 10 nominal_fps = 30 labels_table = h5file.create_table('/', 'labels', Particle, expectedrows=nseqs) video_table = h5file.create_earray('/', 'videos', tables.UInt8Atom(), shape=(0, seq_len, 3, 112, 112), expectedrows=nseqs) short_video_table = h5file.create_earray('/', 'short_videos', tables.UInt8Atom(), shape=(0, short_seq_len, 3, 112, 112), expectedrows=nseqs) depth_table = h5file.create_earray('/', 'depth', tables.Float64Atom(), shape=(0, 112, 112), expectedrows=nseqs) for i in tqdm(range(nseqs)): # do few times # For flat environments, start ground plane localization not too high. ground_from = 5 # 3 meters/s -> jogging speed MAX_SPEED = 3 collisions = True pause = 0 # Manually define areas of interest. Note that inside one of the airsim # environments, you can pull up coordinates using `;`. However, the # coordinates listed are multiplied by 100 (i.e. the numbers are in cm # rather than meters); divide by 100 to define reasonable boundaries # for the capture area. if args.env == 'blocks': x = np.random.uniform(-50, 50) y = np.random.uniform(-50, 50) pause = .05 # blocks is too fast sometimes elif args.env.startswith('nh'): x = np.random.uniform(-150, 150) y = np.random.uniform(-150, 150) client.simEnableWeather(True) for k in range(8): client.simSetWeatherParameter(k, 0.0) if args.env == 'nh_fall': client.simSetWeatherParameter( airsim.WeatherParameter.MapleLeaf, 1.0) client.simSetWeatherParameter(airsim.WeatherParameter.RoadLeaf, 1.0) if args.env == 'nh_winter': client.simSetWeatherParameter(airsim.WeatherParameter.Snow, 1.0) client.simSetWeatherParameter(airsim.WeatherParameter.RoadSnow, 1.0) elif args.env == 'mountains': # Most of the interesting stuff (e.g. the lake, roads) is on the # diagonal of this very large environment (several kilometers!). xy = np.random.uniform(0, 2500) xmy = np.random.uniform(-100, 100) x = xy + xmy y = xy - xmy # This environment varies a lot in height, start from higher ground_from = 100 elif args.env == 'trap': x = np.random.uniform(-10, 10) y = np.random.uniform(-10, 10) # This one has lots of branches, keep sequences that have collisions collisions = False else: raise NotImplementedError(args.env) # Time of day effects works in blocks and trap only. time_of_day = np.random.randint(5, 21) client.simSetTimeOfDay( True, start_datetime=f"2020-03-21 {time_of_day:02}:00:00", is_start_datetime_dst=True, celestial_clock_speed=0, update_interval_secs=60, move_sun=True) if pause > 0: time.sleep(pause) pitch = np.random.uniform( -.25, .25) # Sometimes we look a little up, sometimes a little down roll = 0 # Should be 0 unless something is wrong # 360 degrees lookaround yaw = np.random.uniform(-np.pi, np.pi) heading_yaw = draw_von_mises(2.5) heading_pitch = draw_von_mises(16) # Max ~90 degrees per second head rotation rotation_yaw = 30 * np.pi / 180 * np.random.randn() rotation_pitch = 10 * np.pi / 180 * np.random.randn() speed = MAX_SPEED * np.random.rand() # Figure out the height of the ground. Move the camera way far above # ground, and get the distance to the ground via the depth buffer # from the bottom camera. client.simSetVehiclePose( airsim.Pose(airsim.Vector3r(x, y, -ground_from), airsim.to_quaternion(0, 0, 0)), True) if pause > 0: time.sleep(pause) responses = client.simGetImages([ airsim.ImageRequest("bottom_center", airsim.ImageType.DepthPlanner, pixels_as_float=True) ]) response = responses[0] the_arr = airsim.list_to_2d_float_array(response.image_data_float, response.width, response.height) # Then move to ~5.5 feet above the ground rgy = range(int(.4 * the_arr.shape[0]), int(.6 * the_arr.shape[0])) rgx = range(int(.4 * the_arr.shape[0]), int(.6 * the_arr.shape[0])) the_min = np.median(the_arr[rgy, rgx]) z = the_min - ground_from - np.random.uniform(1.4, 2) if z > 50: # More than 50 meters below sea level, bad draw. continue #client.startRecording() z = z.item() depths = [] frames = [] booped = False for k in range(seq_len): if booped: continue # In nominal seconds t = (k - (seq_len - 1) / 2) / nominal_fps d = t * speed client.simSetVehiclePose( airsim.Pose( airsim.Vector3r( x + d * np.cos(yaw + heading_yaw) * np.cos(pitch + heading_pitch), y + d * np.sin(yaw + heading_yaw) * np.cos(pitch + heading_pitch), z + d * np.sin(pitch + heading_pitch)), airsim.to_quaternion(pitch + t * rotation_pitch, roll, yaw + t * rotation_yaw)), True) responses = client.simGetImages([ airsim.ImageRequest("front_center", airsim.ImageType.Scene, False, False), airsim.ImageRequest("front_center", airsim.ImageType.DepthPlanner, True, False), ]) for j, response in enumerate(responses): if j == 0: frames.append( airsim.string_to_uint8_array( response.image_data_uint8).reshape( response.height, response.width, 3)[:, :, ::-1]) if j == 1: zbuff = airsim.list_to_2d_float_array( response.image_data_float, response.width, response.height) depths.append(zbuff) # Did we bump into something? if collisions: closest = zbuff[zbuff.shape[0] // 4:-zbuff.shape[0] // 4, zbuff.shape[1] // 4:-zbuff.shape[1] // 4].min() if closest < .5: # oops I booped it again. booped = True if j == 0 and args.save_images: filename = os.path.join(tmp_dir, f"{i:02}_{k:02}.png") matplotlib.image.imsave(filename, frames[-1]) if pause > 0: time.sleep(pause) row = labels_table.row if not booped and not args.save_images: # Let's save this! row['x'] = x row['y'] = y row['z'] = z row['yaw'] = yaw row['pitch'] = pitch row['speed'] = speed row['heading_yaw'] = heading_yaw row['heading_pitch'] = heading_pitch row['rotation_yaw'] = rotation_yaw row['rotation_pitch'] = rotation_pitch row.append() V = np.stack(frames, axis=0).transpose((0, 3, 1, 2)) V = V.reshape((1, ) + V.shape) video_table.append(V) # Take a subset of the data. mid_seq = range((seq_len - short_seq_len) // 2, (seq_len + short_seq_len) // 2) assert V.shape[1] == seq_len short_video_table.append(V[:, mid_seq, :, :, :]) # Only record the mid-point z buffer n = seq_len // 2 D = depths[n] D = D.reshape((1, ) + D.shape) depth_table.append(D) h5file.close() # currently reset() doesn't work in CV mode. Below is the workaround client.simSetVehiclePose( airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True)
# In settings.json first activate computer vision mode: # https://github.com/Microsoft/AirSim/blob/master/docs/image_apis.md#computer-vision-mode import airsim import cv2 import numpy as np import setup_path client = airsim.VehicleClient() client.confirmConnection() airsim.wait_key('Press any key to set all object IDs to 0') found = client.simSetSegmentationObjectID("[\w]*", 0, True); print("Done: %r" % (found)) #for block environment # airsim.wait_key('Press any key to change one ground object ID') # found = client.simSetSegmentationObjectID("Ground", 20); # print("Done: %r" % (found)) # #regex are case insensitive # airsim.wait_key('Press any key to change all ground object ID') # found = client.simSetSegmentationObjectID("ground[\w]*", 22, True); # print("Done: %r" % (found)) ##for neighborhood environment #set object ID for sky # found = client.simSetSegmentationObjectID("SkySphere", 42, True); # print("Done: %r" % (found))
def main(camera_name, align_to_left_camera, image_width, image_heightm, requests, level_name, human_name, item_name, num_capture): timestamp = int(time.time()) client = airsim.VehicleClient() camera_info = camera_info_list[camera_name] level_info = level_info_list[level_name] item_info = item_info_list[item_name] # camera config # TODO: we might add random lense distortion for getting more realistic images. lc, rc = (0.0, 1.0) if align_to_left_camera else (-0.5, 0.5) client.simSetCameraPose( 'front_left', airsim.Pose(airsim.Vector3r(0., lc * camera_info.baseline, 0.), airsim.Quaternionr()).UE4ToAirSim()) client.simSetCameraFov('front_left', camera_info.HFOV) client.simSetCameraImageSize('front_left', image_width, image_height) client.simSetCameraPose( 'front_right', airsim.Pose(airsim.Vector3r(0., rc * camera_info.baseline, 0.), airsim.Quaternionr()).UE4ToAirSim()) client.simSetCameraFov('front_right', camera_info.HFOV) client.simSetCameraImageSize('front_right', image_width, image_height) # load level #client.simLoadLevel(level_name) # TODO: after loading different level, position reference differs from what we expect. fix it. # swapn humans/items # TODO: find a way to immediately reflect segID change human_actor_name = 'human_{}'.format(timestamp) human_BP_name = metahumans_bp_path_template.format(human_name) client.simSpawnObject(human_actor_name, human_BP_name, physics_enabled=False, from_actorBP=True) client.simSetSegmentationObjectID(human_actor_name, 1) if item_name != 'none': item_actor_name = 'item_{}'.format(timestamp) client.simSpawnObject(item_actor_name, item_name, physics_enabled=False, from_actorBP=False) client.simSetSegmentationObjectID(item_actor_name, 2) # sometimes the metahuman's hands are splited away from the body in the first frame... wait a little to avoid it time.sleep(0.5) # generate data capture_folder = './capture_{}_{}_{}_{}/'.format(level_name, human_name, item_name, timestamp) os.makedirs(capture_folder, exist_ok=True) for idx in range(num_capture): # TODO: some metahumans's face are unnaturally lightened in Room environment. find how to fix it! # configure objects in the scene. # we follow the UE4's coordinates/units convention for ease of working with the UE4 environment. # airsim uses NED coordinates with unit 'm' while UE4 uses NEU coordinates with unit 'cm'. # base position/horizontal rotation where we arragnge the stereo camera, humans, items. # here we set the base position to near the bottom center of the booth shelf. base_position = generate_random_position(level_info.position_range) base_rotation = generate_random_rotation((0, 0, 1), (0, 360)) #base_rotation = airsim.Quaternionr() #base_rotation = generate_random_rotation((0, 0, 1), (90, 90)) world_to_base = airsim.Pose(base_position, base_rotation) # derive the stereo camera position camera_roll_error = 0 #5 camera_pitch_error = 0 #3 camera_height_error = 0 #5 camera_horiz_error = 0 #3 #camera_position = generate_random_position((80, 80, 40, 40, 160, 160)) #camera_rotation = generate_random_rotation((0, 0, 1), (-150, -150)) camera_position = generate_random_position( (-17 - camera_horiz_error, -17 + camera_horiz_error, 0 - camera_horiz_error, 0 + camera_horiz_error, 212.5 - camera_height_error, 212.5 + camera_height_error)) camera_rotation = generate_random_rotation( (0, 0, 1), (-camera_roll_error, camera_roll_error)) * generate_random_rotation( (0, 1, 0), (90 - camera_pitch_error, 90 + camera_pitch_error)) base_to_camera = airsim.Pose(camera_position, camera_rotation) world_to_camera = base_to_camera * world_to_base client.simSetVehiclePose(world_to_camera.UE4ToAirSim()) # derive the human position # TODO: add human skelton scaling variation crouching_diff = 0 # TODO: impl human_position = generate_random_position( (-70, -30, -20, 20, -crouching_diff, -crouching_diff)) #human_position = generate_random_position((-40, -40, 0, 0, -crouching_diff, -crouching_diff)) human_rotation_diff = 20 #20#60 human_rotation = generate_random_rotation( (0, 0, 1), (-90 - human_rotation_diff, -90 + human_rotation_diff)) base_to_human = airsim.Pose(human_position, human_rotation) world_to_human = base_to_human * world_to_base client.simSetObjectPose(human_actor_name, world_to_human.UE4ToAirSim()) # derive the human pose left_hand_IKposition = airsim.Vector3r( 50, 0, -100) # relative to the clavicle left_hand_rotation = generate_random_position((-30, -30, 0, 0, 0, 0)) # TODO: find natural range right_hand_IKposition = generate_random_hand_reach( (60, 100)) * -1 # relative to the clavicle right_hand_rotation = generate_random_position( metahumans_hand_rotation_range) # relative to the bone # TODO: find a way to make crouching pose, natural foots, waist pose client.simSetMetahumanPose(human_actor_name, left_hand_IKposition, left_hand_rotation, right_hand_IKposition, right_hand_rotation) # derive the item pose if item_name != 'none': hand_pose_world = client.simGetMetahumanBonePose( human_actor_name, 'middle_01_r').AirSimToUE4() hand_to_item = airsim.Pose( airsim.Vector3r(0, item_info.grab_offset, 0), generate_random_rotation((0, 0, 1), (0, 360))) world_to_item = hand_to_item * hand_pose_world client.simSetObjectPose(item_actor_name, world_to_item.UE4ToAirSim()) # request image captures exposure_bias = generate_random_scalar( level_info.auto_exposure_bias_range) for camera_name in ["front_left", "front_right"]: client.simSetCameraPostProcess( camera_name, auto_exposure_bias=exposure_bias, auto_exposure_max_brightness=1.0, auto_exposure_min_brightness=1.0, lens_flare_intensity=0.0, motion_blur_amount=0.0, ) responses = client.simGetImages(requests) # request attribute data # TODO: gather data useful for machine learning (camera params, key points, etc) # save images and attribute data for response, request in zip(responses, requests): img = airsim.decode_image_response(response) if img.dtype == np.float16: img = np.asarray( (img * 1000).clip(0, 65535), dtype=np.uint16 ) # convert to an uint16 depth image with unit mm. (we are expecting that the depth camera range is up to 65m) if request.image_type == airsim.ImageType.Scene: name = 'rgb' elif request.image_type == airsim.ImageType.DepthPlanar: name = 'depth' # be careful that simulated depth values are not so accurate at far range due to the limited bit-depth of rengdering depth buffer. elif request.image_type == airsim.ImageType.Segmentation: name = 'mask' else: raise Exception('no impl') if request.camera_name == 'front_left': name += 'L' elif request.camera_name == 'front_right': name += 'R' else: raise Exception('no impl') airsim.write_png( '{}{:0>8}_{}.png'.format(capture_folder, idx, name), img) if item_name != 'none': client.simDestroyObject(item_actor_name) client.simDestroyObject(human_actor_name)
def Freq(): client = airsim.CarClient() VehicleClient = airsim.VehicleClient() sensor_state = VehicleClient.getImuData() car_controls = airsim.CarControls() testCases = 10 revTime = 0 #seconds time1 = time.time() for sensor in range(5): idx = 0 tot = 0 if sensor == 0: print("\n\n\nIMU Data:") elif sensor == 1: print("\n\n\nBarometer Data:") elif sensor == 2: print("\n\n\nMagnetometer Data:") elif sensor == 3: print("\n\n\nGps Data:") elif sensor == 4: print("\n\n\nDistance Sensor Data:") #prevTime = datetime.now().timestamp() prevTime = sensor_state.time_stamp / 1000000000 while idx <= testCases: #Go reverse car_controls.throttle = -0.5 car_controls.is_manual_gear = True car_controls.manual_gear = -1 car_controls.steering = 0 client.setCarControls(car_controls) #print("Go reverse") time.sleep(revTime) # let car drive a bit car_controls.is_manual_gear = False # change back gear to auto car_controls.manual_gear = 0 if sensor == 0: sensor_state = VehicleClient.getImuData() elif sensor == 1: sensor_state = VehicleClient.getBarometerData() elif sensor == 2: sensor_state = VehicleClient.getMagnetometerData() elif sensor == 3: sensor_state = VehicleClient.getGpsData() elif sensor == 4: sensor_state = VehicleClient.getDistanceSensorData() #Time calculations #currentTime = datetime.now().timestamp() #car_state = client.getCarState() currentTime = sensor_state.time_stamp / 1000000000 #convert nanoseconds to seconds diff = ( ((currentTime - prevTime) - revTime) * 1000) #miliseconds prevTime = currentTime if diff != 0: freq = 1000 / diff #Hertz tot = tot + freq else: #print("0 difference encountered") continue #print("Difference (In miliseconds): %f Frequency (Hz): %f" % (diff,freq)) idx = idx + 1 time2 = time.time() print("\nAverage frequency: %f" % (float(idx) / (time2 - time1)))
def __init__(self): self.client = airsim.VehicleClient() try: self.client.confirmConnection() except msgpackrpc.error.TransportError: raise Exception("Can't connect to Unreal Engine") from None
def main(self): #Time step Ts = 0.1 # Maximum angular velocity max_angular_vel = 6393.667 * 2 * np.pi / 60 #Final state x_bar = np.array([[10.0], [10.0], [10.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]]) #Gain matrix K, u_bar = calK.gainMatrix(Ts,max_angular_vel) # #Setup airsim multirotor multirotorClient multirotorClient = airsim.MultirotorClient() multirotorClient.confirmConnection() multirotorClient.enableApiControl(True) vehicleClient = airsim.VehicleClient() state = multirotorClient.getMultirotorState() print(state.kinematics_estimated.position) # Arm the drone print("arming the drone...") multirotorClient.armDisarm(True) if state.landed_state == airsim.LandedState.Landed: print("taking off...") multirotorClient.takeoffAsync().join() else: multirotorClient.hoverAsync().join() time.sleep(2) # Declare u matrix 4 x 1 # u = [0, # 0, # 0, # 0] # pwm = np.array([0, # 0, # 0, # 0]) print("Controls start") #time.sleep(2) #multirotorClient.moveByMotorPWMsAsync(1, 1, 1, 1,3).join() #newX = [[],[],[],[],[],[],[],[],[],[],[],[]] # Start step loop for index in range(1000): # Re initilize u for every iteration # u = [0, # 0, # 0, # 0] # Get state of the multiorotor state = multirotorClient.getMultirotorState() state = state.kinematics_estimated initialState = state.position #Convert from quaternion to euler angle #euler = ls.quaternion_to_euler(state.orientation.x_val,state.orientation.y_val, state.orientation.z_val,state.orientation.w_val) q = R.from_quat([state.orientation.x_val, state.orientation.y_val, state.orientation.z_val, state.orientation.w_val]) e = q.as_euler('zyx') # q = Quaternion(state.orientation.w_val, # state.orientation.x_val, # state.orientation.y_val, # state.orientation.z_val) # e = q.to_euler() # rotationMatrix = np.linalg.inv([[0, 1, 0], # [1, 0, 0], # [0, 0, -1]]) # position = [[state.position.x_val], # [state.position.y_val], # [state.position.z_val]] # linear_velocity = [[state.linear_velocity.x_val], # [state.linear_velocity.x_val], # [state.linear_velocity.z_val]] #Store the current state of multirotor in x #e[2] = e[2] + np.pi if e[2]<=np.pi else e[2] - np.pi x = np.array([[state.position.x_val], [-state.position.y_val], [-state.position.z_val], [e[0]], [-e[1]], [-e[2]], [state.linear_velocity.x_val], [-state.linear_velocity.y_val], [-state.linear_velocity.z_val], [state.angular_velocity.x_val], [-state.angular_velocity.y_val], [-state.angular_velocity.z_val]]) # Compute u u = np.dot(K, x_bar-x) + u_bar #print(np.dot(K, x_bar - x)) #squared_angular_velocity = u_bar # pwmHover = 0.5937 # # Compute required pwm signal # sq_ctrl_hover = (pwmHover * max_angular_vel)**2 #sq_ctrl_delta = np.dot(K, x_bar - x) sq_ctrl = [max(u[0][0], 0.0), max(u[1][0], 0.0), max(u[2][0], 0.0), max(u[3][0], 0.0)] # max is just in case norm of sq_ctrl_delta is too large (can be negative) pwm1 = min((np.sqrt(sq_ctrl[0])/max_angular_vel),1.0) pwm2 = min((np.sqrt(sq_ctrl[1])/max_angular_vel),1.0) pwm3 = min((np.sqrt(sq_ctrl[2])/max_angular_vel),1.0) pwm4 = min((np.sqrt(sq_ctrl[3])/max_angular_vel),1.0) #pwm = np.sqrt(max(squared_angular_velocity + (pwmHover*max_angular_vel)**2, 0)) / max_angular_vel multirotorClient.moveByMotorPWMsAsync(pwm4, pwm1, pwm3, pwm2,Ts).join() #multirotorClient.moveToPositionAsync(x_bar[0], x_bar[1], x_bar[2], 0, 1200, #airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode(False,0), -1, 1).join() #multirotorClient.moveByMotorPWMsAsync(pwmHover, pwmHover, pwmHover, pwmHover, Ts).join() # print(x_bar[0][0]) # multirotorClient.moveToPositionAsync(x_bar[0][0], x_bar[1][0], -x_bar[2][0], 1.0).join() state = multirotorClient.getMultirotorState() state = state.kinematics_estimated # print(state) time.sleep(10) print("Free fall") multirotorClient.moveByMotorPWMsAsync(0, 0, 0, 0, 10).join time.sleep(10) print("disarming...") multirotorClient.armDisarm(False) multirotorClient.enableApiControl(False) print("done.")