# 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](matlab_src/), 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](https://doc.rust-lang.org/book/ch01-01-installation.html), 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:
```toml
[dependencies]
fast_motion_planning = "0.1.0"
```
Then in your `main.rs` file, you can import `fast_motion_planning` using
```rust
use fast_motion_planning
```
See the [example project](examples/example_rust) 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.

```python
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](examples/example_rust) 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](fast_motion_planning). To build the python library, first install the [`maturin`](https://github.com/PyO3/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`](examples/examples_rust) for details

### Using `fast_motion_planning`
To use `fast_motion_planning`, ensure you import the following
```rust
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:
```rust
 // 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