mavtrajectorygeneration

This repository contains tools for polynomial trajectory generation and optimization based on methods described in [1]. These techniques are especially suitable for rotary-wing micro aerial vehicles (MAVs). This README provides a brief overview of our trajectory generation utilities with some examples.

Authors: Markus Achtelik, Michael Burri, Helen Oleynikova, Rik Bähnemann, Marija Popović
Maintainer: Rik Bähnemann, brik@ethz.ch
Affiliation: Autonomous Systems Lab, ETH Zurich

Bibliography

This implementation is largely based on the work of C. Richter et al, who should be cited if this is used in a scientific publication (or the preceding conference papers):
[1] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in International Journal of Robotics Research, Springer, 2016. @incollection{richter2016polynomial, title={Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments}, author={Richter, Charles and Bry, Adam and Roy, Nicholas}, booktitle={Robotics Research}, pages={649--666}, year={2016}, publisher={Springer} }

Furthermore, the nonlinear optimization features our own extensions, described in:

Michael Burri, Helen Oleynikova, Markus Achtelik, and Roland Siegwart, “Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Previously Unknown Environments”. In IEEE Int. Conf. on Intelligent Robots and Systems (IROS), September 2015. @inproceedings{burri2015real-time, author={Burri, Michael and Oleynikova, Helen and and Achtelik, Markus W. and Siegwart, Roland}, booktitle={Intelligent Robots and Systems (IROS 2015), 2015 IEEE/RSJ International Conference on}, title={Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Unknown Environments}, year={2015}, month={Sept} }

How To Use

Environment Setup

1. Install pybind bindings git submodule update --init

2. Install Eigen sudo apt install libeigen3-dev

3. Install glog From outside this directory e.g. code directory, run ```

   Fetch glog in version 6

   git clone https://github.com/google/glog.git --branch v0.6.0 cd glog

cmake -S . -B build -G "Unix Makefiles" cmake --build build cmake --build build --target install ```

1. Install YAML CPP sudo apt-get install libyaml-cpp-dev

Build package

Building generates a python binding making a trajectoryg generation function accessible from python. The package is built using CMAKE. To gurantee that the python installation is in the right environemt, always build only in the drone environment. General steps are described below mkdir build cd build cmake .. make

Call in Python

In python code import the package via import polynomial_trajectory. Call the function as polynomial_trajectory.generate_trajectory(waypoints: np.ndarray, v_max: double, a_max: double, sampling_intervall: double) -> np.ndarray([x,x_dot, y, y_dot, z, z_dot, t])

Basics

A vertex describes the properties of a support point of a polynomial path. Pairs of vertices are connected together to form segments. Each vertex has a set of constraints: the values of position derivatives that must be matched during optimization procedures. Typically, only the positions are specified for vertices along a path, while start and end vertices have all derivatives of position set to zero. x----------x-----------------x vertex segment In the case of multi-dimensional vertices, the derivative constraints exist in all dimensions, with possibly different values.

Linear Optimization

In this section, we consider how to generate polynomial segments passing through a set of arbitrary vertices using the unconstrained linear optimization approach described in [1].

Necessary includes: ```c++

include

```

1. Create a list of three (x,y,z) vertices to fly through, e.g. (0,0,1) -> (1,2,3) -> (2,1,5), and define some parameters. The dimension variable denotes the spatial dimension of the path (here, 3D). The derivative_to_optimize should usually be set to the last derivative that should be continuous (here, snap).

c++ mav_trajectory_generation::Vertex::Vector vertices; const int dimension = 3; const int derivative_to_optimize = mav_trajectory_generation::derivative_order::SNAP; mav_trajectory_generation::Vertex start(dimension), middle(dimension), end(dimension);

1. Add constraints to the vertices.

```c++ start.makeStartOrEnd(Eigen::Vector3d(0,0,1), derivativetooptimize); vertices.push_back(start);

middle.addConstraint(mavtrajectorygeneration::derivativeorder::POSITION, Eigen::Vector3d(1,2,3)); vertices.pushback(middle);

end.makeStartOrEnd(Eigen::Vector3d(2,1,5), derivativetooptimize); vertices.push_back(end); ```

1. Compute the segment times.

c++ std::vector<double> segment_times; const double v_max = 2.0; const double a_max = 2.0; segment_times = estimateSegmentTimes(vertices, v_max, a_max);

1. Create an optimizer object and solve. The template parameter (N) denotes the number of coefficients of the underlying polynomial, which has to be even. If we want the trajectories to be snap-continuous, N needs to be at least 10; for minimizing jerk, 8.

c++ const int N = 10; mav_trajectory_generation::PolynomialOptimization<N> opt(dimension); opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); opt.solveLinear();

1. Obtain the polynomial segments.

c++ mav_trajectory_generation::Segment::Vector segments; opt.getSegments(&segments);

Nonlinear Optimization

In this section, we consider how to generate polynomial segments passing through a set of arbitrary vertices using the unconstrained nonlinear optimization approach described in [1]. The same approach is followed as in the previous section.

Necessary includes:

```c++

include

```

1. Set up the problem by following Steps 1-3 in the Linear Optimization section.

2. Set the parameters for nonlinear optimization. Below is an example, but the default parameters should be reasonable enough to use without fine-tuning.

c++ NonlinearOptimizationParameters parameters; parameters.max_iterations = 1000; parameters.f_rel = 0.05; parameters.x_rel = 0.1; parameters.time_penalty = 500.0; parameters.initial_stepsize_rel = 0.1; parameters.inequality_constraint_tolerance = 0.1;

1. Create an optimizer object and solve. The third argument of the optimization object (true/false) specifies whether the optimization is run over the segment times only.

c++ const int N = 10; PolynomialOptimizationNonLinear<N> opt(dimension, parameters, false); opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max); opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max); opt.optimize();

1. Obtain the polynomial segments.

c++ mav_trajectory_generation::Segment::Vector segments; opt.getPolynomialOptimizationRef().getSegments(&segments);

Creating Trajectories

In this section, we consider how to use our trajectory optimization results. We first need to convert our optimization object into the Trajectory class:

```c++

include

mavtrajectorygeneration::Trajectory trajectory; opt.getTrajectory(&trajectory); ```

We can also create new trajectories by splitting (getting a trajectory with a single dimension) or compositing (getting a trajectory by appending another trajectory:

```c++ // Splitting: mavtrajectorygeneration::Trajectory x_trajectory = trajectory.getTrajectoryWithSingleDimension(1);

// Compositing: mavtrajectorygeneration::Trajectory trajectorywithyaw; trajectory.getTrajectoryWithAppendedDimension(yawtrajectory, &trajectorywith_yaw); ```

Sampling Trajectories

In this section, we consider methods of evaluating the trajectory at particular instances of time. There are two methods of doing this.

1. By evaluating directly from the Trajectory class.

```c++ // Single sample: double samplingtime = 2.0; int derivativeorder = mavtrajectorygeneration::derivativeorder::POSITION; Eigen::VectorXd sample = trajectory.evaluate(samplingtime, derivative_order);

// Sample range: double tstart = 2.0; double tend = 10.0; double dt = 0.01; std::vectorEigen::VectorXd result; std::vector samplingtimes; // Optional. trajectory.evaluateRange(tstart, tend, dt, derivativeorder, &result, &sampling_times); ```

1. By conversion to mav_msgs::EigenTrajectoryPoint state(s). These functions support 3D or 4D trajectories (the 4th dimension is assumed to be yaw if it exists).

```c++

include

mavmsgs::EigenTrajectoryPoint state; mavmsgs::EigenTrajectoryPoint::Vector states;

// Single sample: double samplingtime = 2.0; bool success = mavtrajectorygeneration::sampleTrajectoryAtTime(trajectory, sampletime, &state);

// Sample range: double tstart = 2.0; double duration = 10.0; double dt = 0.01; success = mavtrajectorygeneration::sampleTrajectoryInRange(trajectory, tstart, duration, dt, &states);

// Whole trajectory: double samplinginterval = 0.01; success = mavtrajectorygeneration::sampleWholeTrajectory(trajectory, samplinginterval, &states); ```

Visualizing Trajectories

In this section, we describe how to visualize trajectories in rviz.

For a simple visualization:

```c++

include

visualizationmsgs::MarkerArray markers; double distance = 1.0; // Distance by which to seperate additional markers. Set 0.0 to disable. std::string frameid = "world";

// From Trajectory class: mavtrajectorygeneration::drawMavTrajectory(trajectory, distance, frame_id, &markers);

// From mavmsgs::EigenTrajectoryPoint::Vector states: mavtrajectorygeneration::drawMavSampledTrajectory(states, distance, frameid, &markers) ```

For a visualization including an additional marker at a set distance (e.g. hexacopter marker):

```c++ mav_visualization::HexacopterMarker hex(simple);

// From Trajectory class: mavtrajectorygeneration::drawMavTrajectoryWithMavMarker(trajectory, distance, frame_id, hex &markers);

// From mavmsgs::EigenTrajectoryPoint::Vector states: mavtrajectorygeneration::drawMavSampledTrajectoryWithMavMarker(states, distance, frameid, hex, &markers) ```

Checking Input Feasibility

The package contains three implementations to check generated trajectories for input feasibility. The checks are based on the rigid-body model assumption and flat state characteristics presented in Mellinger2011.

@inproceedings{mellinger2011minimum, title={Minimum snap trajectory generation and control for quadrotors}, author={Mellinger, Daniel and Kumar, Vijay}, booktitle={Robotics and Automation (ICRA), 2011 IEEE International Conference on}, pages={2520--2525}, year={2011}, organization={IEEE} }

The trajectories are checked for low and high thrust, high velocities, high roll and pitch rates, high yaw rates and high yaw angular accelerations.

FeasibilitySampling implements a naive sampling-based check. FeasibilityRecursive implements a slightly adapted recursive feasibility test presented in Müller2015. @article{mueller2015computationally, title={A computationally efficient motion primitive for quadrocopter trajectory generation}, author={Mueller, Mark W and Hehn, Markus and D'Andrea, Raffaello}, journal={IEEE Transactions on Robotics}, volume={31}, number={6}, pages={1294--1310}, year={2015}, publisher={IEEE} } We adapted RapidQuadrocopterTrajectories to check arbitrary polynomial order trajectories for yaw and velocity feasibility.

FeasibilityAnalytic analytically checks the magnitudes except for the roll and pitch rates, where it runs the recursive test (recommended: low in computation time, no false positives).

Example

```c++ // Create input constraints. typedef InputConstraintType ICT; InputConstraints inputconstraints; inputconstraints.addConstraint(ICT::kFMin, 0.5 * 9.81); // minimum acceleration in [m/s/s]. inputconstraints.addConstraint(ICT::kFMax, 1.5 * 9.81); // maximum acceleration in [m/s/s]. inputconstraints.addConstraint(ICT::kVMax, 3.5); // maximum velocity in [m/s]. inputconstraints.addConstraint(ICT::kOmegaXYMax, MPI / 2.0); // maximum roll/pitch rates in [rad/s]. inputconstraints.addConstraint(ICT::kOmegaZMax, MPI / 2.0); // maximum yaw rates in [rad/s]. inputconstraints.addConstraint(ICT::kOmegaZDotMax, MPI); // maximum yaw acceleration in [rad/s/s].

// Create feasibility object of choice (FeasibilityAnalytic, // FeasibilitySampling, FeasibilityRecursive). FeasibilityAnalytic feasibilitycheck(inputconstraints); feasibilitycheck.settings.setMinSectionTimeS(0.01);

// Check feasibility. Segment dummysegment; InputFeasibilityResult result = feasibilitycheck.checkInputFeasibility(dummy_segment); std::cout << "The segment input is " << getInputFeasibilityResultName(result); << "." << std::endl; ```

Benchmarking

Both recursive and analytic checks are comparably fast. The recursive check may have a couple more false negatives, i.e., segments, that can not be determined feasible although they are. But this is neglectable. The sampling based check is both slow and may have false positives, i.e., consider segments feasible although they are not. We do not recommend using this.

Here are the computational results over 1000 random segments with different parameter settings:

Checking Half-Space Feasibility

The package also contains a method to check if a trajectory or segment is inside an arbitrary set of half spaces based on RapidQuadrocopterTrajectories. This is useful to check if a segment is inside a box or above ground.

Example ground plane feasibility: c++ // Create feasibility check. FeasibilityBase feasibility_check; // Create ground plane. Eigen::Vector3d point(0.0, 0.0, 0.0); Eigen::Vector3d normal(0.0, 0.0, 1.0); feasibility_check.half_plane_constraints_.emplace_back(point, normal); // Check feasibility. Segment dummy_segment; if(!feasibility_check.checkHalfPlaneFeasibility(segment)) { std::cout << "The segment is in collision with the ground plane." << std::endl; }

Example box feasibility: c++ // Create feasibility check. FeasibilityBase feasibility_check; // Create box constraints. Eigen::Vector3d box_center(0.0, 0.0, 0.0); Eigen::Vector3d box_size(1.0, 1.0, 1.0); feasibility_check.half_plane_constraints_ = HalfPlane::createBoundingBox(box_center, box_size); // Check feasibility. Segment dummy_segment; if(!feasibility_check.checkHalfPlaneFeasibility(segment)) { std::cout << "The segment is not inside the box." << std::endl; }