Struct QPpolyTrajPlanner

pub struct QPpolyTrajPlanner {
    pub coeff: DMatrix<f64>,
    pub polyorder: usize,
    pub min_deriv: usize,
    pub smooth_upto: usize,
    pub segment_times: Vec<f32>,
    pub waypoints: Vec<Vec<f32>>,
    pub max_velocity: f32,
    pub max_acceleration: f32,
    pub start_time: f32,
    pub dt: f32,
}

Waypoint planner that generates a quadratic polynomial trajectory between waypoints

Example

use peng_quad::QPpolyTrajPlanner;
use nalgebra::Vector3;
let waypoints: Vec<Vec<f32>> = vec![vec![0.0,0.0,1.0,0.0], vec![1.0,0.0,1.0,0.0]];
let segment_times = vec![6.0];
let min_deriv = 3;
let polyorder = 9;
let smooth_upto = 4;
let max_velocity = 4.0;
let max_acceleration = 3.0;
let dt = 0.1;
let start_time = 0.0;
let mut qp_planner = QPpolyTrajPlanner::new(waypoints,segment_times,polyorder, min_deriv, smooth_upto,max_velocity,max_acceleration, start_time, dt);

Fields

coeff: DMatrix<f64>

Matrix of coefficients for each segment and each dimension, organized as nrows: polyorder*segment_times.len(), ncols: 4 (for x, y, z, yaw)

polyorder: usize

Order of the polynomial to be used in computing trajectory

min_deriv: usize

Minimize which derivative in the QP problem (1->Velocity, 2->Acceleration, 3->Snap, 4->Jerk. Please note that derivative value greater than 4 is not supported)

smooth_upto: usize

Ensure continuity upto which derivative. NOTE: This MUST be <= polynomial_order (1->Velocity, 2->Acceleration, 3->Snap, 4->Jerk. Please note that derivative value greater than 4 is not supported)

segment_times: Vec<f32>

Vector of time values for each segment, which tells the planner how much time each segment should take to complete. Expressed in seconds.

waypoints: Vec<Vec<f32>>

Waypoints for each segment. Note that there should be segment_times.len() + 1 values for waypoints, with the position of the quadrotor being the very first waypoint.

max_velocity: f32

Maximum velocity constraint. Set to 0.0 to disregard inequality constraints. Please set reasonable values for this as it influences solver convergence or failure.

max_acceleration: f32

Maximum acceleration constraint. Set to 0.0 to disregard inequality constraints. Please set reasonable values for this as it influences solver convergence or failure.

start_time: f32

Time at which the simulation starts. This value has no bearing to QPpolyTraj itself, but is used during simulation since we use relative time internally when computing values.

dt: f32

Step time used while generating inequality constraints. Has no bearing if max_velocity or max_acceleration is set to 0.0. Please set reasonable value for this as it has a huge impact on OSQP solve time.

Implementations

impl QPpolyTrajPlanner

pub fn new( waypoints: Vec<Vec<f32>>, segment_times: Vec<f32>, polyorder: usize, min_deriv: usize, smooth_upto: usize, max_velocity: f32, max_acceleration: f32, start_time: f32, dt: f32, ) -> Result<Self, SimulationError>

Generate a new QPpolyTraj planner

Arguments

• waypoints - The waypoints for the trajectory
• segment_times - The times for each segment of the trajectory
• polyorder - The order of the polynomial
• min_deriv - The minimum derivative to be considered
• smooth_upto - The maximum derivative to be considered
• max_velocity - The maximum velocity
• max_acceleration - The maximum acceleration
• start_time - The start time of the trajectory
• dt - The time step for the trajectory

Errors

• Returns an error if the specified waypoints, segment times, smooth_upto or polyorder is invalid

Examples

use peng_quad::QPpolyTrajPlanner;
use nalgebra::{DMatrix, DVector, Vector3};
let waypoints: Vec<Vec<f32>> = vec![vec![0.0, 0.0, 0.0, 0.0], vec![1.0, 1.0, 1.5, 1.5707963267948966], vec![-1.0, 1.0, 1.75, 3.141592653589793], vec![0.0, -1.0, 1.0, -1.5707963267948966], vec![0.0, 0.0, 0.5, 0.0]];
let segment_times = vec![5.0, 5.0, 5.0, 5.0];
let polyorder = 9;
let min_deriv = 3;
let smooth_upto = 4;
let max_velocity = 4.0;
let max_acceleration = 2.0;
let start_time = 0.0;
let dt = 0.1;
let mut qp_planner = QPpolyTrajPlanner::new(waypoints,segment_times,polyorder, min_deriv, smooth_upto,max_velocity,max_acceleration, start_time, dt).unwrap();

pub fn evaluate_polynomial( &self, t: f32, segment: usize, ) -> (Vector3<f32>, Vector3<f32>, f32, f32)

Compute the position, velocity, yaw, and yaw_rate for a given time t in given segment

Arguments

• t - The time at which to evaluate the polynomial
• segment - The segment index for which to evaluate the polynomial

Returns

• A tuple containing the position, velocity, yaw, and yaw_rate at time t in segment segment

Trait Implementations

impl Planner for QPpolyTrajPlanner

Implement the Planner trait for QPpolyTrajPlanner

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