def predict_velocities(self, img, p_o_b):
img = (img / 255.0) * 2 - 1.0
if self.regressor_type == 'full':
predictions = self.bc_model(img)
elif self.regressor_type == 'latent':
z, _, _ = self.cmvae_model.encode(img)
predictions = self.bc_model(z)
elif self.regressor_type == 'reg':
z = self.reg_model(img)
predictions = self.bc_model(z)
predictions = predictions.numpy()
predictions = racing_utils.dataset_utils.de_normalize_v(predictions)
# print('Predicted body vel: \n {}'.format(predictions[0]))
v_xyz_world = racing_utils.geom_utils.convert_t_body_2_world(
airsimdroneracingvae.Vector3r(predictions[0, 0], predictions[0, 1], predictions[0, 2]),
p_o_b.orientation
)
return np.array(
[
v_xyz_world.x_val,
v_xyz_world.y_val,
v_xyz_world.z_val,
predictions[0, 3]
]
)
def process_image(client, img_res):
'''
# this is the old image get method after decoupled
img_request = [airsimdroneracingvae.ImageRequest('0', airsimdroneracingvae.ImageType.Scene, False, False)]
image_array = client.simGetImages(img_request)
image_response = image_array[0]
'''
# img_request = [airsimdroneracingvae.ImageRequest('0', airsimdroneracingvae.ImageType.Scene, False, False)]
image_array = client.telloGetImages()
# image_response = image_array[0]
# img_1d = np.fromstring(image_response.image_data_uint8, dtype=np.uint8) # get numpy array
# img_bgr = img_1d.reshape(image_response.height, image_response.width, 3) # reshape array to 4 channel image array H X W X 3
img_resized = cv2.resize(image_array,
(img_res, img_res)).astype(np.float32)
img_batch_1 = np.array([img_resized])
# cam_pos = airsimdroneracingvae.Vector3r(5.5, -4, -1.5+offset[2])
# cam_orientation = airsimdroneracingvae.Quaternionr(0.4, 0.9, 0, 0)
cam_pos = airsimdroneracingvae.Vector3r()
#image_response.camera_position
cam_orientation = airsimdroneracingvae.Quaternionr()
# image_response.camera_orientation
return img_batch_1, cam_pos, cam_orientation
def takeoff_with_moveOnSpline(self, takeoff_height, vel_max, acc_max):
self.client.moveOnSplineAsync(
path=[airsimdroneracingvae.Vector3r(4, -2, takeoff_height)],
vel_max=vel_max,
acc_max=acc_max,
add_curr_odom_position_constraint=True,
add_curr_odom_velocity_constraint=True,
viz_traj=viz_traj,
vehicle_name=self.drone_name).join()
offset = [0, 0, -0]
gate_poses = racing_utils.trajectory_utils.RedGateSpawnerCircle(client, num_gates=8, radius=8, radius_noise=gate_noise, height_range=[0, -gate_noise], track_offset=offset)
# wait till takeoff complete
vel_max = 5.0
acc_max = 2.0
time.sleep(1.0)
# takeoff_position = airsimdroneracingvae.Vector3r(25, 7, -1.5)
# takeoff_orientation = airsimdroneracingvae.Vector3r(.2, -0.9, 0)
#
# takeoff_position = airsimdroneracingvae.Vector3r(25, -7, -1.5)
# takeoff_orientation = airsimdroneracingvae.Vector3r(-.2, 0.9, 0)
takeoff_position = airsimdroneracingvae.Vector3r(5.5, -4, -1.5+offset[2])
takeoff_orientation = airsimdroneracingvae.Vector3r(0.4, 0.9, 0)
# takeoff_position = airsimdroneracingvae.Vector3r(0, 0, -2)
# takeoff_position = airsimdroneracingvae.Vector3r(0, 0, 10)
# takeoff_orientation = airsimdroneracingvae.Vector3r(1, 0, 0)
# client.plot_tf([takeoff_pose], duration=20.0, vehicle_name=drone_name)
# client.moveOnSplineAsync([airsimdroneracingvae.Vector3r(0, 0, -3)], vel_max=15.0, acc_max=5.0, vehicle_name=drone_name, viz_traj=True).join()
client.moveOnSplineVelConstraintsAsync([takeoff_position], [takeoff_orientation], vel_max=vel_max, acc_max=acc_max, vehicle_name=drone_name, viz_traj=False).join()
# client.moveOnSplineVelConstraintsAsync([airsimdroneracingvae.Vector3r(1, 0, 8)], [airsimdroneracingvae.Vector3r(1, 0, 0)], vel_max=vel_max, acc_max=acc_max, vehicle_name=drone_name, viz_traj=True)
time.sleep(1.0)
img_res = 64
if policy_type == 'bc_con':
training_mode = 'latent'