Struct MinimumSnapWaypointPlanner

pub struct MinimumSnapWaypointPlanner {
    pub waypoints: Vec<Vector3<f32>>,
    pub yaws: Vec<f32>,
    pub times: Vec<f32>,
    pub coefficients: Vec<Vec<Vector3<f32>>>,
    pub yaw_coefficients: Vec<Vec<f32>>,
    pub start_time: f32,
}

Waypoint planner that generates a minimum snap trajectory between waypoints

Example

use peng_quad::MinimumSnapWaypointPlanner;
use nalgebra::Vector3;
let planner = MinimumSnapWaypointPlanner::new(
    vec![Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0)],
    vec![0.0, 0.0],
    vec![1.0],
    0.0,
);

Fields

waypoints: Vec<Vector3<f32>>

List of waypoints

yaws: Vec<f32>

List of yaw angles

times: Vec<f32>

List of segment times to reach each waypoint

coefficients: Vec<Vec<Vector3<f32>>>

Coefficients for the x, y, and z components of the trajectory

yaw_coefficients: Vec<Vec<f32>>

Coefficients for the yaw component of the trajectory

start_time: f32

Start time of the trajectory

Implementations

impl MinimumSnapWaypointPlanner

Implementation of the MinimumSnapWaypointPlanner

pub fn new( waypoints: Vec<Vector3<f32>>, yaws: Vec<f32>, segment_times: Vec<f32>, start_time: f32, ) -> Result<Self, SimulationError>

Generate a new minimum snap waypoint planner

Arguments

• waypoints - List of waypoints
• yaws - List of yaw angles
• segment_times - List of segment times to reach each waypoint
• start_time - Start time of the trajectory

Returns

• A new minimum snap waypoint planner

Errors

• Returns an error if the number of waypoints, yaws, and segment times do not match

Example

use peng_quad::MinimumSnapWaypointPlanner;
use nalgebra::Vector3;
let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
let yaws = vec![0.0, 0.0];
let segment_times = vec![1.0];
let start_time = 0.0;
let planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time);

pub fn compute_minimum_snap_trajectories( &mut self, ) -> Result<(), SimulationError>

Compute the coefficients for the minimum snap trajectory, calculated for each segment between waypoints

Errors

• Returns an error if the nalgebra solver fails to solve the linear system

Example

use peng_quad::MinimumSnapWaypointPlanner;
use nalgebra::Vector3;
let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
let yaws = vec![0.0, 0.0];
let segment_times = vec![1.0];
let start_time = 0.0;
let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
planner.compute_minimum_snap_trajectories();

pub fn compute_minimum_acceleration_yaw_trajectories( &mut self, ) -> Result<(), SimulationError>

Compute the coefficients for yaw trajectories The yaw trajectory is a cubic polynomial and interpolated between waypoints

Errors

• Returns an error if nalgebra fails to solve for the coefficients

Example

use peng_quad::MinimumSnapWaypointPlanner;
use nalgebra::Vector3;
let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
let yaws = vec![0.0, 0.0];
let segment_times = vec![1.0];
let start_time = 0.0;
let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
planner.compute_minimum_snap_trajectories();
planner.compute_minimum_acceleration_yaw_trajectories();

pub fn evaluate_polynomial( &self, t: f32, coeffs: &[Vector3<f32>], yaw_coeffs: &[f32], ) -> (Vector3<f32>, Vector3<f32>, f32, f32)

Evaluate the trajectory at a given time, returns the position, velocity, yaw, and yaw rate at the given time

Arguments

• t - The time to evaluate the trajectory at
• coeffs - The coefficients for the position trajectory
• yaw_coeffs - The coefficients for the yaw trajectory

Returns

• position - The position at the given time (meters)
• velocity - The velocity at the given time (meters / second)
• yaw - The yaw at the given time (radians)
• yaw_rate - The yaw rate at the given time (radians / second)

Example

use nalgebra::Vector3;
use peng_quad::MinimumSnapWaypointPlanner;
let waypoints = vec![Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0)];
let yaws = vec![0.0, 0.0];
let segment_times = vec![1.0];
let start_time = 0.0;
let mut planner = MinimumSnapWaypointPlanner::new(waypoints, yaws, segment_times, start_time).unwrap();
planner.compute_minimum_snap_trajectories();
planner.compute_minimum_acceleration_yaw_trajectories();
let (position, velocity, yaw, yaw_rate) = planner.evaluate_polynomial(0.5, &planner.coefficients[0], &planner.yaw_coefficients[0]);

Trait Implementations

impl Planner for MinimumSnapWaypointPlanner

Implement the Planner trait for MinimumSnapWaypointPlanner

fn plan( &self, _current_position: Vector3<f32>, _current_velocity: Vector3<f32>, time: f32, ) -> (Vector3<f32>, Vector3<f32>, f32)

Plans the trajectory based on the current state and time

fn is_finished( &self, current_position: Vector3<f32>, time: f32, ) -> Result<bool, SimulationError>

Checks if the current trajectory is finished