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)