MRSL Motion Primitive Library for quadrotor v1.0

Motion Primitive Library is a search-based planner to compute dynamically feasible trajectories for a quadrotor flying in an obstacle-cluttered environment. Our approach searches for smooth, minimum-time trajectories by exploring the map using a set of short-duration motion primitives. The primitives are generated by solving an optimal control problem and induce a finite lattice discretization on the state space which can be explored using a graph-search algorithm. The proposed approach is able to generate resolution-complete (i.e., optimal in the discretized space), safe, dynamically feasibility trajectories efficiently by exploiting the explicit solution of a Linear Quadratic Minimum Time problem. It does not assume a hovering initial condition and, hence, is suitable for fast online re-planning while the robot is moving.

For technical details, refer to the original paper "Search-based Motion Planning for Quadrotors using Linear Quadratic Minimum Time Control" that has been published in IROS 2017.

New Features

Reformat the repo structure

Installation

Prerequisite:

PCL: apt install libpcl-dev
Eigen: apt install libeigen3-dev
YAML-CPP: apt install libyaml-cpp-dev

or simply run following commands:

$ sudo apt-get update
$ sudo apt install -y libeigen3-dev libpcl-dev libyaml-cpp-dev libproj-dev cmake

A) Simple cmake

mkdir build && cd build && cmake .. && make

B) Using Catkin (not recognizable by catkin_make)

$ mv motion_primitive_library ~/catkin_ws/src
$ cd ~/catkin_ws & catkin_make_isolated -DCMAKE_BUILD_TYPE=Release

CTest

Run following command in the build folder for testing the executables:

$ make test

If everything works, you should see the results as:

Running tests...
Test project /home/sikang/cpp_ws/src/mpl_ros/motion_primitive_library/build
    Start 1: test_planner_2d
1/2 Test #1: test_planner_2d ..................   Passed    0.93 sec
    Start 2: test_planner_2d_prior_traj
2/2 Test #2: test_planner_2d_prior_traj .......   Passed    1.00 sec

100% tests passed, 0 tests failed out of 2

Total Test time (real) =   1.94 sec

Include in other projects:

To link this lib properly, add following in the CMakeLists.txt

find_package(motion_primitive_library REQUIRED)
include_directories(${MOTION_PRIMITIVE_LIBRARY_INCLUDE_DIRS})
...
add_executable(test_xxx src/test_xxx.cpp)
target_link_libraries(test_xxx ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})

Example Usage

Preparation

Three components are required to be set properly before running the planner:

1) Start and Goal:

We use theclass Waypoint for the start and goal. A Waypoint contains coordinates of position, velocity, etc and the flag use_xxx to indicate the control input. An example for 2D planning is given as:

Waypoint2 start, goal; // Initialize start and goal as Waypoint 2D
start.pos = Vec3f(2.5, -3.5);
start.use_pos = true;
start.use_vel = true;
start.use_acc = false;
start.use_jrk = false;
goal.pos = Vec3f(35, 2.5);
goal.use_pos = start.use_pos;
goal.use_vel = start.use_vel;
goal.use_acc = start.use_acc;
goal.use_jrk = start.use_jrk;

The flag use_xxx indicates the planner to plan in different control space. For example, the one above is control in acc space. Four options are provided by setting those flags as below:

Vel	Acc	Jrk	Snp
`use_pos = true`	`use_pos = true`	`use_pos = true`	`use_pos = true`
`use_vel = false`	`use_vel = true`	`use_vel = true`	`use_vel = true`
`use_acc = false`	`use_acc = false`	`use_acc = true`	`use_acc = true`
`use_jrk = false`	`use_jrk = false`	`use_jrk = false`	`use_jrk = true`

2) Set collision checking:

For planner MapPlanner, we use class MapUtil to handle collision checking in a voxel map. An example for 2D collision checking based on OccMapUtil is given as:

std::shared_ptr<MPL::OccMapUtil> map_util;
map_util.reset(new MPL::OccMapUtil); // Initialize map_util
map_util->setMap(origin, dim, data, resolution);

3) Set control input:

An example for the control input U for 2D planning is given as following, in this case, U simply include 9 elements:

decimal_t u_max = 0.5;
vec_Vec2f U;
const decimal_t du = u_max / num;
for(decimal_t dx = -u_max; dx <= u_max; dx += du )
  for(decimal_t dy = -u_max; dy <= u_max; dy += du )
      U.push_back(Vec2f(dx, dy));

Run the planner:

After set up above 3 components, a planner can be initialized as:

std::unique_ptr<MPL::OccMapPlanner> planner(new MPL::OccMapPlanner(true)); // Declare a 2D planner with verbose on
planner->setMapUtil(map_util); // Set collision checking util
planner->setEpsilon(1.0); // Set greedy param (default equal to 1)
planner->setVmax(1.0); // Set max velocity
planner->setAmax(1.0); // Set max acceleration
planner->setUmax(u_max); // Set max control input
planner->setDt(1.0); // Set dt for each primitive
planner->setW(10); // Set weight of time
planner->setU(U); // Set control input
planner->setTol(0.5); // Tolerance for goal region, 0.5m in position

bool valid = planner->plan(start, goal); // Plan from start to goal

Test

Example1 (simple planning):

Run following command for test a 2D planning in a given map:

$ ./build/devel/lib/test_planner_2d ../data/corridor.yaml

You should see following messages if it works properly:

[MPPlanner] PLANNER VERBOSE ON
[MPBaseUtil] set epsilon: 1.000000
[MPBaseUtil] set v_max: 1.000000
[MPBaseUtil] set a_max: 1.000000
[MPBaseUtil] set u_max: 0.500000
[MPBaseUtil] set dt: 1.000000
[MPBaseUtil] set w: 10.000000
[MPBaseUtil] set max num: -1
[MPBaseUtil] set tol_dis: 0.500000
[MPBaseUtil] set tol_vel: 0.000000
[MPBaseUtil] set tol_acc: 0.000000
Start:
pos:  2.5 -3.5
vel: 0 0
acc: 0 0
jrk: 0 0
use_pos: 1
use_vel: 1
use_acc: 0
use_jrk: 0
Goal:
pos:  37 2.5
vel: 0 0
acc: 0 0
jrk: 0 0
use_pos: 1
use_vel: 1
use_acc: 0
use_jrk: 0
[MPBaseUtil] set effort in acc
Start from new node!
goalNode fval: 361.213182, g: 352.000000!
Expand [2618] nodes!
Reached Goal !!!!!!
...

MP Planner takes: 65.000000 ms
MP Planner expanded states: 2618
Total time T: 35.000000
Total J:  J(1) = 41.416667, J(2) = 2.000000, J(3) = 0.000000, J(4) = 0.000000

The output image output.svg is saved in the current folder: (blue dots show the expended states, red circles indicate start and goal).

It is recommended to visualize the svg in web browser (for example, firefox output.svg).

Example2 (planning with a prior trajectory):

Run following command for test a 2D planning, it first finds a trajector in low dimensional space (acceleration-driven), then it uses the planned trajectory to refine for a trajectory in high dimensional space (jerk-driven):

$ ./build/devel/lib/test_planner_2d_prior_traj ../data/corridor.yaml

In the following output image, the black curve is the prior trajectory:

Doxygen

For API, please refer to Doxygen.

ROS Wrapper

The interface with ROS for visualization and implementation can be found in mpl_ros.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

README.md

README.md

MRSL Motion Primitive Library for quadrotor v1.0

New Features

Installation

Prerequisite:

A) Simple cmake

B) Using Catkin (not recognizable by catkin_make)

CTest

Include in other projects:

Example Usage

Preparation

1) Start and Goal:

2) Set collision checking:

3) Set control input:

Run the planner:

Test

Example1 (simple planning):

Example2 (planning with a prior trajectory):

Doxygen

ROS Wrapper

Files

README.md

Latest commit

History

README.md

File metadata and controls

MRSL Motion Primitive Library for quadrotor v1.0

New Features

Installation

Prerequisite:

A) Simple cmake

B) Using Catkin (not recognizable by catkin_make)

CTest

Include in other projects:

Example Usage

Preparation

1) Start and Goal:

2) Set collision checking:

3) Set control input:

Run the planner:

Test

Example1 (simple planning):

Example2 (planning with a prior trajectory):

Doxygen

ROS Wrapper