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.")