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.
- Add incremental trajectory planning
- Fix many small bugs in previous version
PCL: apt install libpcl-devEigen: apt install libeigen3-devYAML-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 cmakemkdir build && cd build && cmake .. && make$ mv motion_primitive_library ~/catkin_ws/src
$ cd ~/catkin_ws & catkin_make_isolated -DCMAKE_BUILD_TYPE=ReleaseTo 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})
Three components are required to be set properly before running the planner:
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 |
For class MPMapUtil, 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<OccMapUtil> map_util;
map_util.reset(new OccMapUtil); // Initialize map_util
map_util->setMap(origin, dim, data, resolution);
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));
After set up above 3 components, a planner can be initialized as:
std::unique_ptr<MPMap2DUtil> planner(new MPMap2DUtil(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
Run following command for test a 2D planning in a given map:
$ ./build/devel/lib/test_planner_2d ../data/corridor.yamlYou 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).
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.yamlIn the following output image, the black curve is the prior trajectory:
For API, please refer to Doxygen.
The interface with ROS for visualization and implementation can be found in mpl_ros.