def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.reset:
client.reset()
client.simFlushPersistentMarkers()
plot_pose(client, TEST_POSE)
plot_xyz_axis(client, X, Y, Z, origin=O)
with airsimy.pose_at_simulation_pause(client) as pose:
plot_pose(client, pose)
client.simPlotArrows([pose.position], [LOOK_AT_TARGET],
Rgba.White,
is_persistent=True)
# NOTE use x' = (LOOK_AT_TARGET - p) as the new x-axis (i.e. front vector),
# and project the current up/down vector (z-axis in AirSim) into the plane
# that is normal to x' at point p. This way we can get the remaining right
# vector by computing cross(down, front).
x_prime = LOOK_AT_TARGET - pose.position
_, _, z_axis = AirSimNedTransform.local_axes_frame(pose)
z_prime = airsimy.vector_projected_onto_plane(z_axis,
plane_normal=x_prime)
# NOTE don't forget to normalize! Not doing so will break the orientation below.
x_prime /= x_prime.get_length()
z_prime /= z_prime.get_length()
y_prime = z_prime.cross(x_prime)
plot_xyz_axis(
client,
x_prime * 1.25,
y_prime * 1.25,
z_prime * 1.25,
origin=pose.position,
colors=CMY,
thickness=1.5,
)
# Now, find the orientation that corresponds to the x'-y'-z' axis frame:
new_pose = Pose(
pose.position,
airsimy.quaternion_that_rotates_axes_frame(
source_xyz_axes=(X, Y, Z),
target_xyz_axes=(x_prime, y_prime, z_prime),
),
)
plot_pose(client, new_pose
) # NOTE this should be the same as the plot_xyz_axis above!
if args.set:
client.simSetVehiclePose(new_pose, ignore_collision=True)
def plot_xyz_axis(
client: airsim.MultirotorClient,
x_axis: Vector3r,
y_axis: Vector3r,
z_axis: Vector3r,
origin: Vector3r,
colors=RGB,
thickness=2.5,
) -> None:
client.simPlotArrows([origin], [origin + x_axis],
colors[0],
thickness,
is_persistent=True)
client.simPlotArrows([origin], [origin + y_axis],
colors[1],
thickness,
is_persistent=True)
client.simPlotArrows([origin], [origin + z_axis],
colors[2],
thickness,
is_persistent=True)
