Fast Motion Primitive Motion Planning for Quadcopters

This project is a rust implmenetation of the following paper:

S. Upadhyay, T. Richardson, and A. Richards, “Fast Generation of Obstacle-Avoiding Motion Primitives forQuadrotors”, Accepted in 2021 IEEERSJ International Conference on Intelligent Robots and Systems (IROS 2021).

The paper abstract is the following:

This work considers the problem of generating computationally efficient quadrotor motion primitives betweena given pose (position, velocity, and acceleration) and a goalplane in the presence of obstacles. A new motion primitivetool based on the logistic curve is proposed and a closed-formanalytic approach is developed to satisfy constraints on startingpose, goal plane, velocity, acceleration, and jerk. The geometricobstacle avoidance problem is represented as a combinatorialset problem and a heuristic approach is proposed to acceleratethe solution search. Numerical examples are presented tohighlight the fast motion primitive generation in multi-obstaclepose-to-plane scenarios

The project provides a solver api which samples from motion primitives in order to efficiently generate trajectories which avoid specified obstacles, for UAVs to follow. It was originally implmented in Matlab, and this project re-implements the project in rust along with a python interface.

Installation and Use

This project (will be) is on crates.io here: This project (will be) is also on pip here:

Creating your own rust project

First setup the rust toolchain here, and set up a new project using cargo new <project name>. Then simply add this crate as a dependency in your Cargo.toml file like so:

[dependencies]
fast_motion_planning = "0.1.0"

Then in your main.rs file, you can import fast_motion_planning using

use fast_motion_planning

See the example project for more details

Adding this library to your Python project via pip

Install the library using pip install .... The library fast_motion_planning will then be available to import.

import fast_motion_planning as fmp

Extending this github repository or building from source.

Then clone this repository into your local files. To compile and build the project, in the root directory run cargo build. This will build the core libraries.

For a rust application, refer to the example project and how it has been added to the workspace within Cargo.toml so it has access to the fast_motion_planning library. To run the example project, go into examples/examples_rust and run cargo run. This will run the example main.rs.

For a python application, refer to the example scripts. To build the python library, first install the maturin tool (pip install maturin), then either

1. Run maturin develop in the root. This will generate the libraries into the fast_motion_planning directory, and the examples can be run directly. However the generated file will only be accessible in that folder
2. Run pip install . in the root. This reads the pyproject.toml file and builds and installs the project into your pip workspace. This way fast_motion_planning will be available anywhere you use pip.

Rust API

See the example at examples/examples_rust for details

Using fast_motion_planning

To use fast_motion_planning, ensure you import the following

use fast_motion_planning::{BruteForceFastMotionPlanner, HeuristicFastMotionPlanner, MPProblem, MPComponent, utils};
use fast_motion_planning::{Object, ObjectList, Bounds};

Examples

The api can be used like the following:

 // Define the axis component ranges
 // comp id, start position, goal position min, goal position max, start velocity, start acceleration
 let x_param = MPComponent::new("x".to_string(), 0.0, 4.0, 4.0, 3.0, 1.0);
 let y_param = MPComponent::new("y".to_string(), 1.5, 2.8, 3.2, 3.0, 0.5);
 let z_param = MPComponent::new("z".to_string(), 1.0, 1.7, 2.3, 1.0, 0.1);

 // Create Objects
 let mut objects = ObjectList::new();
 objects.push(Object::new(
     Bounds::new(3.75, 4.25),
     Bounds::new(2.3, 2.8),
     Bounds::new(1.7, 2.3)));
 objects.push(Object::new(
     Bounds::new(3.75, 4.25),
     Bounds::new(3.2, 3.7),
     Bounds::new(1.7, 2.3)));

 // Create the problem
 let problem = MPProblem::new(x_param, y_param, z_param, objects);

 // Specify parameters
 let time_division = 5;
 let epsilon = 0.01;

 println!("Testing Heuristic solution");
 let planner = HeuristicFastMotionPlanner::new(time_division, epsilon);
 let solution = planner.solve(problem.clone());
 println!("Found {} solutions", solution.len());

 println!("Testing BruteForce solution");
 let planner = BruteForceFastMotionPlanner::new(time_division, epsilon);
 let solution = planner.solve(problem.clone());
 println!("Found {} solutions", solution.len());

 println!("Testing Heuristic solution");
 let planner = HeuristicFastMotionPlanner::new(time_division, epsilon);
 let num_sols = planner.solver(problem.clone()).into_iter().count();
 println!("Found {} solutions", num_sols);

 println!("Testing BruteForce solution iterator");
 let planner = BruteForceFastMotionPlanner::new(time_division, epsilon);
 let num_sols = planner.solver(problem.clone()).into_iter().count();
 println!("Found {} solutions", num_sols);

 println!("Testing Heuristic solution and trajectory iterator");
 let time_start = 0.0;
 let num_samples = 100;
 let planner = HeuristicFastMotionPlanner::new(time_division, epsilon);
 let num_sols = planner.solver(problem.clone()).into_iter().map(|x| (x, x.generate_fourpl_trajectory(time_start, num_samples))).count();
 println!("Found {} solutions", num_sols);

 println!("Testing BruteFroce solution and trajectory iterator");
 let time_start = 0.0;
 let num_samples = 100;
 let planner = BruteForceFastMotionPlanner::new(time_division, epsilon);
 let num_sols = planner.solver(problem.clone()).into_iter().map(|x| (x, x.generate_fourpl_trajectory(time_start, num_samples))).count();
 println!("Found {} solutions", num_sols);

Python API