Git Product home page Git Product logo

mav_trajectory_generation's Introduction

mav_trajectory_generation

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, [email protected]
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}
}

Installation Instructions (Ubuntu)

To install this package with ROS Indigo or ROS Kinetic:

  1. Install additional system dependencies (swap indigo for kinetic or melodic as necessary):

** Note: ROS melodic requires libyaml-cpp-dev and does not build with yaml_cpp_catkin in your catkin workspace!

sudo apt-get install python-wstool python-catkin-tools ros-indigo-cmake-modules libyaml-cpp-dev
  1. Set up a catkin workspace (if not already done):
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
catkin config --extend /opt/ros/indigo
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin config --merge-devel
  1. Install the repository and its dependencies (with rosinstall):
cd src
wstool init
wstool set --git mav_trajectory_generation [email protected]:ethz-asl/mav_trajectory_generation.git -y
wstool update
wstool merge mav_trajectory_generation/install/mav_trajectory_generation_https.rosinstall
wstool update -j8
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source /opt/ros/indigo/setup.bash

In case you have your SSH keys for github set up, feel free to use the ssh rosinstall instead:

wstool merge mav_trajectory_generation/install/mav_trajectory_generation_ssh.rosinstall
  1. Use catkin_build to build the repository:
catkin build mav_trajectory_generation_ros

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:

#include <mav_trajectory_generation/polynomial_optimization_linear.h>
  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).
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.
start.makeStartOrEnd(Eigen::Vector3d(0,0,1), derivative_to_optimize);
vertices.push_back(start);

middle.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(1,2,3));
vertices.push_back(middle);

end.makeStartOrEnd(Eigen::Vector3d(2,1,5), derivative_to_optimize);
vertices.push_back(end);
  1. Compute the segment times.
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.
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.
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:

#include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>
  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.

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.
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.
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:

#include <mav_trajectory_generation/trajectory.h>

mav_trajectory_generation::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:

// Splitting:
mav_trajectory_generation::Trajectory x_trajectory = trajectory.getTrajectoryWithSingleDimension(1);

// Compositing:
mav_trajectory_generation::Trajectory trajectory_with_yaw; trajectory.getTrajectoryWithAppendedDimension(yaw_trajectory, &trajectory_with_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.
// Single sample:
double sampling_time = 2.0;
int derivative_order = mav_trajectory_generation::derivative_order::POSITION;
Eigen::VectorXd sample = trajectory.evaluate(sampling_time, derivative_order);

// Sample range:
double t_start = 2.0;
double t_end = 10.0;
double dt = 0.01;
std::vector<Eigen::VectorXd> result;
std::vector<double> sampling_times; // Optional.
trajectory.evaluateRange(t_start, t_end, dt, derivative_order, &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).
#include <mav_trajectory_generation_ros/trajectory_sampling.h>

mav_msgs::EigenTrajectoryPoint state;
mav_msgs::EigenTrajectoryPoint::Vector states;

// Single sample:
double sampling_time = 2.0;
bool success = mav_trajectory_generation::sampleTrajectoryAtTime(trajectory, sample_time, &state);

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

// Whole trajectory:
double sampling_interval = 0.01;
success = mav_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_interval, &states);

Visualizing Trajectories

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

For a simple visualization:

#include <mav_trajectory_generation_ros/ros_visualization.h>

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

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

// From mav_msgs::EigenTrajectoryPoint::Vector states:
mav_trajectory_generation::drawMavSampledTrajectory(states, distance, frame_id, &markers)

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

mav_visualization::HexacopterMarker hex(simple);

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

// From mav_msgs::EigenTrajectoryPoint::Vector states:
mav_trajectory_generation::drawMavSampledTrajectoryWithMavMarker(states, distance, frame_id, 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

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

// Create feasibility object of choice (FeasibilityAnalytic,
// FeasibilitySampling, FeasibilityRecursive).
FeasibilityAnalytic feasibility_check(input_constraints);
feasibility_check.settings_.setMinSectionTimeS(0.01);

// Check feasibility.
Segment dummy_segment;
InputFeasibilityResult result =
    feasibility_check.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:

// 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:

// 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;
}

mav_trajectory_generation's People

Contributors

alexmillane avatar helenol avatar john-tornblom avatar karenbodie avatar magrimm avatar maxb91 avatar michaelpantic avatar mikeallenspach avatar plusk01 avatar raghavkhanna avatar rikba avatar vanurag avatar zacharytaylor avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

mav_trajectory_generation's Issues

2nd constraints problem

Hello ,today I test tested the code and I added the 2th order constraints on one of the middle point.I found the trajectory got much more illogical than only position constraints added.About time allocation part I used 'runSegmentViolationScaling' function and just tested the linear optimization. I attached my trajectory and listed the coordinates of points. Could you help me solve this problem? What will affect the shape of trajectory generated? The reason is this optimization method is reformed as an unconstraints optimization? I do not need velocity constraints on way points?
I am looking forward to receiving your feedback.
Thanks in advanced!
core_points (0, 0, 0)); //start point
core_points(0.9, 1, 0));
core_points(0.7, 2, 0)); // at this point velocity constraints added 1m/s
core_points(3, 3, 0));
core_points(4, 4, 0)); // goal point
v_max = 2.0
a_max = 1.0
2018-07-17 16-35-11
@rikba @helenol

catkin_build problem

when i catkin_build this package, i encounter a problem.as follows:

/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp: In function ‘bool mav_trajectory_generation::sampleTrajectoryInRange(const mav_trajectory_generation::Trajectory&, double, double, double, mav_msgs::EigenTrajectoryPointVector*)’:
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:82:11: error: ‘struct mav_msgs::EigenTrajectoryPoint’ has no member named ‘degrees_of_freedom’
state.degrees_of_freedom = mav_msgs::MavActuation::DOF4;
^~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:82:42: error: ‘mav_msgs::MavActuation’ has not been declared
state.degrees_of_freedom = mav_msgs::MavActuation::DOF4;
^~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:102:17: error: ‘matrixFromRotationVector’ is not a member of ‘mav_msgs’
mav_msgs::matrixFromRotationVector(rot_vec, &rot_matrix);
^~~~~~~~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:104:44: error: ‘omegaFromRotationVector’ is not a member of ‘mav_msgs’
state.angular_velocity_W = mav_msgs::omegaFromRotationVector(rot_vec, rot_vec_vel);
^~~~~~~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:105:48: error: ‘omegaDotFromRotationVector’ is not a member of ‘mav_msgs’
state.angular_acceleration_W = mav_msgs::omegaDotFromRotationVector(rot_vec, rot_vec_vel, rot_vec_acc);
^~~~~~~~~~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:106:13: error: ‘struct mav_msgs::EigenTrajectoryPoint’ has no member named ‘degrees_of_freedom’
state.degrees_of_freedom = mav_msgs::MavActuation::DOF6;
^~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:106:44: error: ‘mav_msgs::MavActuation’ has not been declared
state.degrees_of_freedom = mav_msgs::MavActuation::DOF6;
^~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp: In function ‘bool mav_trajectory_generation::sampleFlatStateAtTime(const T&, double, mav_msgs::EigenTrajectoryPoint*)’:
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:153:10: error: ‘struct mav_msgs::EigenTrajectoryPoint’ has no member named ‘degrees_of_freedom’
state->degrees_of_freedom = mav_msgs::MavActuation::DOF4;
^~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:153:41: error: ‘mav_msgs::MavActuation’ has not been declared
state->degrees_of_freedom = mav_msgs::MavActuation::DOF4;
^~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:172:15: error: ‘matrixFromRotationVector’ is not a member of ‘mav_msgs’
mav_msgs::matrixFromRotationVector(rot_vec, &rot_matrix);
^~~~~~~~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:174:43: error: ‘omegaFromRotationVector’ is not a member of ‘mav_msgs’
state->angular_velocity_W = mav_msgs::omegaFromRotationVector(rot_vec, rot_vec_vel);
^~~~~~~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:175:47: error: ‘omegaDotFromRotationVector’ is not a member of ‘mav_msgs’
state->angular_acceleration_W = mav_msgs::omegaDotFromRotationVector(rot_vec, rot_vec_vel, rot_vec_acc);
^~~~~~~~~~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:176:12: error: ‘struct mav_msgs::EigenTrajectoryPoint’ has no member named ‘degrees_of_freedom’
state->degrees_of_freedom = mav_msgs::MavActuation::DOF6;
^~~~~~~~~~~~~~~~~~
/home/lab/catkin1_ws/src/mav_trajectory_generation/mav_trajectory_generation/src/trajectory_sampling.cpp:176:43: error: ‘mav_msgs::MavActuation’ has not been declared
state->degrees_of_freedom = mav_msgs::MavActuation::DOF6;
^~~~~~~~~~~~
make[2]: *** [CMakeFiles/mav_trajectory_generation.dir/src/trajectory_sampling.cpp.o] Error 1
make[2]: *** 正在等待未完成的任务....
make[1]: *** [CMakeFiles/mav_trajectory_generation.dir/all] Error 2

Delay in keyframe

Hey guys. Tell me what is better to implement a delay at the keyframe?
Can I set 2 frames at one point and set the time between them, but the trajectory is generated incorrectly, what can you advise?

What's the meaning of the orange line?

image

Here is my settings.

start_pos : 0.0, 0.0, 0.0
middle1 : 5.0, 0.0, 1.5
middle1 : 5.0, 10.0, 3.0
middle1 : 0.0, 10.0, 5.0
goal_pos: 0.0, 0.0, 6.0


I have two questions about the results.

  1. The markers with 3-axis(red-green-blue) are representing time-scaled trajectory, but don't they have yaw component? I can see only x,y,z components in Rviz.
  2. What is the meaning of the orange line?

catkin build error

catkin build error like below:
`By not providing "Findyaml_cpp_catkin.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"yaml_cpp_catkin", but CMake did not find one.

Could not find a package configuration file provided by "yaml_cpp_catkin"
with any of the following names:

yaml_cpp_catkinConfig.cmake
yaml_cpp_catkin-config.cmake

Add the installation prefix of "yaml_cpp_catkin" to CMAKE_PREFIX_PATH or
set "yaml_cpp_catkin_DIR" to a directory containing one of the above files.
If "yaml_cpp_catkin" provides a separate development package or SDK, be
sure it has been installed.
Call Stack (most recent call first):
CMakeLists.txt:5 (catkin_simple)
`
Anyone can help me ? Thanks!

Tragectory generated is not smooth

Hi!

I continue testing this amazing repo. Really like its performance, but become not sure about smoothness of resulting trajectory.
Could I ask question about the resulting trajectory it generates. I think, it is not so smooth, as it should be.

rviz_screenshot_2017_05_27-14_51_17

In picture there is orange trajectory by this repo and blue by another realization of original Minimum snap trajectory algorithm.
Could you give me idea, what is my mistake?

Trajectory key points are

  • {0, 0, 0}
  • {0.9, 1, 0}
  • {0.7, 2, 0}
  • {3, 3, 0}
  • {4, 4, 0}

Here is code mostly the same as in readme:

    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), middle1(dimension), middle2(dimension), middle3(dimension), end(dimension);

    //Add constraints to the vertices.
    start.makeStartOrEnd(Eigen::Vector3d(0,0,0), derivative_to_optimize);
    vertices.push_back(start);

    middle1.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(0.9,1,0));
    vertices.push_back(middle1);

    middle2.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(0.7,2,0));
    vertices.push_back(middle2);

    middle3.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(3,3,0));
    vertices.push_back(middle3);

    end.makeStartOrEnd(Eigen::Vector3d(4,4,0), derivative_to_optimize);
    vertices.push_back(end);


    //Compute the segment times.
    std::vector<double> segment_times;
    const double v_max = 2.0;
    const double a_max = 2.0;
    const double magic_fabian_constant = 6.5; // A tuning parameter.
    segment_times = estimateSegmentTimes(vertices, v_max, a_max, magic_fabian_constant);

    //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.
    const int N = 10;
    mav_trajectory_generation::PolynomialOptimization<N> opt(dimension);
    opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
    opt.solveLinear();

    //Obtain the polynomial segments.
    mav_trajectory_generation::Segment::Vector segments;
    opt.getSegments(&segments);

Can I use this for car like trajectory generation?

That means only liner velocity and rotate speed. I try some test and found its result only mav can follow,I am not sure how to force only x dimension speed for robot's local posion.

BTW,the doc in readme about Nonlinear Optimization
There's only 2 parameters for PolynomialOptimizationNonLinear now.

estimateSegmentTimes

Hi, I am puzzled about the "estimateSegmentTimes" fuction.
Is there any reference for the choice of the segment times? In (Richter et al.), they don't really mention how they get an initial guess for segment times. In Burri et al., the initial segment times are obtained just by choosing t = 0.5*v_max.
As for within estimateSegmentTimes, a very puzzling function was chosen, which even depends on the so called "magic_fabian_constant".
Any reference for this choice?

How could I set up drone physical constraints (such as maximum horizontal and vertical velocity and acceleration, maximum height and flight distance)

Hi, developers!

Could you, please, consult me how could I set up drone physical constraints such as

  • maximum horizontal velocity;
  • maximum and minimum vertical velocity;
  • maximum horizontal acceleration;
  • maximum and minimum vertical acceleration;
  • maximum height and flight distance.
    I see, there are a_max and v_max used for time estimation, but no option to set up different horizontal and vertical maximum acceleration and speed.
    There is also addConstraint function and I could set constraints on different derivative_order, but, I think, it is equality constraint, not inequality.

Thank you!

Isolated build fails

Isolated builds are important for using the ros1_bridge to interact with ROS2 packages. Building normally will succeed, but creating an isolated build by running

catkin_make_isolated --install

Will eventually yield the following error:

In file included from /home/oren/catkin_ws/src/mav_trajectory_generation/mav_trajectory_generation_ros/src/ros_conversions.cpp:21:0:
/home/oren/catkin_ws/src/mav_trajectory_generation/mav_trajectory_generation_ros/include/mav_trajectory_generation_ros/ros_conversions.h:26:10: fatal error: mav_planning_msgs/conversions.h: No such file or directory
 #include <mav_planning_msgs/conversions.h>

Full build output here

Time cost in nonlinear optimization

When reading polynomial_optimization_nonlinear_impl.h, in line 283, we have this:
cost_time = total_time * total_time * optimization_data->optimization_parameters_.time_penalty;
Thus, the time is constrained quadratically, instead of linearly as in the referenced paper. Is there any reason for this to be so?
I feel like that we can obtain better results (and more predictable) if the time is constrained linearly, instead of quadratically.

How do I update the planner in example.launch?

Hi! I've set things up so that example.launch itself works fine. However, when I update e.g. example_planner_node.cc, and do the standard catkin build procedure the updates are not reflected when I do roslaunch on the example.launch file.

I've tried to catkin build at the 'root' directory of my workspace, but that fails when building mav_trajectory_generation_example.

~/new_ws$ catkin build
-------------------------------------------------------
Profile:                     default
Extending:        [explicit] /opt/ros/kinetic
Workspace:                   /home/kevin/new_ws
-------------------------------------------------------
Build Space:        [exists] /home/kevin/new_ws/build
Devel Space:        [exists] /home/kevin/new_ws/devel
Install Space:      [unused] /home/kevin/new_ws/install
Log Space:          [exists] /home/kevin/new_ws/logs
Source Space:       [exists] /home/kevin/new_ws/src
DESTDIR:            [unused] None
-------------------------------------------------------
Devel Space Layout:          merged
Install Space Layout:        None
-------------------------------------------------------
Additional CMake Args:       -DCMAKE_BUILD_TYPE=Release
Additional Make Args:        None
Additional catkin Make Args: None
Internal Make Job Server:    True
Cache Job Environments:      False
-------------------------------------------------------
Whitelisted Packages:        None
Blacklisted Packages:        None
-------------------------------------------------------
Workspace configuration appears valid.
-------------------------------------------------------
[build] Found '14' packages in 0.0 seconds.                                                                              
[build] Package table is up to date.                                                                                     
Starting  >>> catkin_simple                                                                                              
Starting  >>> mav_msgs                                                                                                   
Finished  <<< catkin_simple                                    [ 0.1 seconds ]                                           
Starting  >>> mav_state_machine_msgs                                                                                     
Finished  <<< mav_state_machine_msgs                           [ 0.4 seconds ]                                           
Starting  >>> mav_system_msgs                                                                                            
Finished  <<< mav_msgs                                         [ 0.8 seconds ]                                           
Starting  >>> nlopt                                                                                                      
Finished  <<< nlopt                                            [ 0.2 seconds ]                                           
Starting  >>> eigen_catkin                                                                                               
Finished  <<< eigen_catkin                                     [ 0.2 seconds ]                                           
Finished  <<< mav_system_msgs                                  [ 0.5 seconds ]                                           
Starting  >>> glog_catkin                                                                                                
Starting  >>> mav_planning_msgs                                                                                          
Finished  <<< glog_catkin                                      [ 0.2 seconds ]                                           
Starting  >>> mav_visualization                                                                                          
Finished  <<< mav_visualization                                [ 0.8 seconds ]                                           
Starting  >>> eigen_checks                                                                                               
Finished  <<< eigen_checks                                     [ 0.1 seconds ]                                           
Starting  >>> mav_trajectory_generation                                                                                  
Finished  <<< mav_planning_msgs                                [ 1.2 seconds ]                                           
Finished  <<< mav_trajectory_generation                        [ 0.4 seconds ]                                           
Starting  >>> mav_trajectory_generation_ros                                                                              
Finished  <<< mav_trajectory_generation_ros                    [ 0.6 seconds ]                                           
Starting  >>> mav_trajectory_generation_example                                                                          
_________________________________________________________________________________________________________________________
Warnings   << mav_trajectory_generation_example:cmake /home/kevin/new_ws/logs/mav_trajectory_generation_example/build.cmake.005.log
CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:418 (message):
  catkin_package() include dir
  '/home/kevin/new_ws/src/mav_trajectory_generation/mav_trajectory_generation_example/include'
  should be placed in the devel space instead of the build space
Call Stack (most recent call first):
  /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  /home/kevin/catkin_ws/devel/share/catkin_simple/cmake/catkin_simple-extras.cmake:214 (catkin_package)
  CMakeLists.txt:27 (cs_export)

I did move the include folder into the devel folder of mav_trajectory_generation_example, but I get this error:

Starting  >>> mav_trajectory_generation                                                                                  
Finished  <<< mav_trajectory_generation                        [ 0.8 seconds ]                                           
Finished  <<< mav_planning_msgs                                [ 1.3 seconds ]                                           
Starting  >>> mav_trajectory_generation_ros                                                                              
Finished  <<< mav_trajectory_generation_ros                    [ 0.9 seconds ]                                           
Starting  >>> mav_trajectory_generation_example                                                                          
_________________________________________________________________________________________________________________________
Warnings   << mav_trajectory_generation_example:cmake /home/kevin/new_ws/logs/mav_trajectory_generation_example/build.cmake.004.log
CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:418 (message):
  catkin_package() include dir
  '/home/kevin/new_ws/src/mav_trajectory_generation/mav_trajectory_generation_example/include'
  should be placed in the devel space instead of the build space
Call Stack (most recent call first):
  /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  /home/kevin/catkin_ws/devel/share/catkin_simple/cmake/catkin_simple-extras.cmake:214 (catkin_package)
  CMakeLists.txt:27 (cs_export)


cd /home/kevin/new_ws/build/mav_trajectory_generation_example; catkin build --get-env mav_trajectory_generation_example | catkin env -si  /usr/bin/cmake /home/kevin/new_ws/src/mav_trajectory_generation/mav_trajectory_generation_example --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/kevin/new_ws/devel -DCMAKE_INSTALL_PREFIX=/home/kevin/new_ws/install -DCMAKE_BUILD_TYPE=Release; cd -
.........................................................................................................................
_________________________________________________________________________________________________________________________
Errors     << mav_trajectory_generation_example:make /home/kevin/new_ws/logs/mav_trajectory_generation_example/build.make.004.log
make: *** No targets specified and no makefile found.  Stop.
cd /home/kevin/new_ws/build/mav_trajectory_generation_example; catkin build --get-env mav_trajectory_generation_example | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -
.........................................................................................................................
Failed     << mav_trajectory_generation_example:make           [ Exited with code 2 ]                                    
Failed    <<< mav_trajectory_generation_example                [ 1.1 seconds ]                                           
[build] Summary: 10 of 11 packages succeeded.                                                                            
[build]   Ignored:   3 packages were skipped or are blacklisted.                                                         
[build]   Warnings:  1 packages succeeded with warnings.                                                                 
[build]   Abandoned: None.                                                                                               
[build]   Failed:    1 packages failed.                                                                                  
[build] Runtime: 5.1 seconds total.

Would anyone know how to fix this?

When waypoints contain a constant dimension (e.g. collinear, coplanar), nlopt throws an error

I may have found a bug with the nonlinear optimization. When the waypoints contain one or more dimensions the values for which stay constant (e.g. in 3D, collinear or coplanar points), nlopt throws an error like this:

E0613 19:50:25.126937 11907 polynomial_optimization_nonlinear_impl.h:214] error while setting up nlopt: nlopt invalid argument

Here is the minimal code (taken almost entirely from the README.md) which reproduces this error (I give a detailed analysis afterwards of what is causing this error):

#include <ros/ros.h>
#include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>
#include <mav_trajectory_generation_ros/ros_visualization.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "nonlinear_optimization_test");
  ros::NodeHandle nh;

  ros::Publisher vis_pub = nh.advertise<visualization_msgs::MarkerArray>( "trajectory", 0 );

  // Create a list of vertices to fly through
  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), middle1(dimension), end(dimension);

  // Add constraints to the vertices
  start.makeStartOrEnd(Eigen::Vector3d(0, 0, 1), derivative_to_optimize);
  vertices.push_back(start);

  middle1.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(1, 0, 1));
  vertices.push_back(middle1);

  end.makeStartOrEnd(Eigen::Vector3d(1, 1, 1), derivative_to_optimize);
  vertices.push_back(end);

  // Compute the segment times
  std::vector<double> segment_times;
  const double v_max = 2.0;
  const double a_max = 2.0;
  const double magic_fabian_constant = 6.5; // A tuning parameter
  segment_times = mav_trajectory_generation::estimateSegmentTimes(vertices, v_max, a_max, magic_fabian_constant);

  // Set the parameters for nonlinear optimization
  mav_trajectory_generation::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;

  // Create optimizer object and solve
  const int N = 10;
  mav_trajectory_generation::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.optimize();

  // Use trajectory optimization results
  mav_trajectory_generation::Trajectory trajectory;
  opt.getTrajectory(&trajectory);

  // Visualize trajectory
  visualization_msgs::MarkerArray markers;
  double distance = 0.5; // Distance by which to separate additional markers. Set 0.0 to disable.
  std::string frame_id = "world";
  mav_trajectory_generation::drawMavTrajectory(trajectory, distance, frame_id, &markers);

  ros::Rate pub_rate(1);
  while (ros::ok())
  {
    vis_pub.publish(markers);
    ros::spinOnce();
    pub_rate.sleep();
  }
}

For a polynomial with N=10 (10 coefficients) and three collinear vertices

Collinearity in x

Trajectory (0,0,1)------(1,0,1)------(2,0,1):

initial_solution = {std::vector<double, std::allocator>} 
 [0] = {double} 3.3912163676143754
 [1] = {double} 3.3912163676143754
 [2] = {double} 0.67085073713852295       // Velocity
 [3] = {double} -3.1240165433165028e-13   // Acceleration (~0 since second vertex is approx. between first and third)
 [4] = {double} -0.39487007462141988      // Jerk (<0, since vehicle goes from accelerating from vertex 1 to 2 to decelerating from vertex 2 to 3)
 [5] = {double} -6.6426196298050833e-12   // Snap (not much intuition here - rate of change of acceleration not changing)
 [6] = {double} 0
 [7] = {double} 0
 [8] = {double} 0
 [9] = {double} 0
 [10] = {double} 0
 [11] = {double} 0
 [12] = {double} 0
 [13] = {double} 0

Zeros appear for y and z components, since the waypoints are collinear in x. The comments above describe the intuition behind the numerical values, which make sense - since the trajectory requires the quadcopter to go only in the x direction, the linear solution indicates no intention to deviate in the y or z directions.

Collinearity in y

Trajectory (0,0,1)------(0,1,1)------(0,2,1):

initial_solution = {std::vector<double, std::allocator>} 
 [0] = {double} 3.3912163676143754
 [1] = {double} 3.3912163676143754
 [2] = {double} 0
 [3] = {double} 0
 [4] = {double} 0
 [5] = {double} 0
 [6] = {double} 0.67085073713852295
 [7] = {double} -3.1240165433165028e-13
 [8] = {double} -0.39487007462141988
 [9] = {double} -6.6426196298050833e-12
 [10] = {double} 0
 [11] = {double} 0
 [12] = {double} 0
 [13] = {double} 0

Zeros appear for x and z components, since the waypoints are collinear in y. The intuition is the same as before but shifted to the y axis (in fact, the values are the same - which is expected).

Collinearity in z

Trajectory (0,0,1)------(0,0,2)------(0,0,3):

initial_solution = {std::vector<double, std::allocator>} 
 [0] = {double} 3.3912163676143754
 [1] = {double} 3.3912163676143754
 [2] = {double} 0
 [3] = {double} 0
 [4] = {double} 0
 [5] = {double} 0
 [6] = {double} 0
 [7] = {double} 0
 [8] = {double} 0
 [9] = {double} 0
 [10] = {double} 0.67085073713852295
 [11] = {double} -3.1240165433165028e-13
 [12] = {double} -0.39487007462141988
 [13] = {double} -6.6426196298050833e-12

Zeros appear for x and y components, since the waypoints are collinear in z. The intuition is again the same.

For a polynomial with N=10 (10 coefficients) and coplanar vertices

Coplanar in xy plane

Trajectory (0,0,1)------(1,0,1)------(1,1,1):

image

initial_solution = {std::vector<double, std::allocator>} 
 [0] = {double} 3.3912163676143754
 [1] = {double} 3.3912163676143754
 [2] = {double} 0.33542536857166194       // Positive velocity in x
 [3] = {double} -0.39696339727696095      // Decelerating in x (to shed x-velocity and end up at x=+1 at third vertex)
 [4] = {double} -0.19743503731622555      // x-Jerk (not much intuition...)
 [5] = {double} 1.0769463243419433        // x-Snap (not much intuition...)
 [6] = {double} 0.33542536856686062       // Positive velocity in y (towards y=+1)
 [7] = {double} 0.39696339727664848       // Accelerating in y (towards y=+1)
 [8] = {double} -0.19743503730519338      // y-Jerk (not much intuition...)
 [9] = {double} -1.0769463243485866       // y-Snap (not much intuition...)
 [10] = {double} 0
 [11] = {double} 0
 [12] = {double} 0
 [13] = {double} 0

Zeros appear for the z component, since the waypoints lie on the xy plane. The comments above describe the intuition behind the numerical values, which make sense - since the trajectory requires the quadcopter to traverse the xy plane, the linear solution indicates no intention to deviate in the z direction.

Error being thrown

initial_solution containing zeros leads to initial_step also containing zeros (since initial_step is just a scaled version to initial_solution). Because of this presence of zeros, an error is thrown when evaluating nlopt_->set_initial_step(initial_step); here, More specifically, the error is thrown by the following function in ~/catkin_ws/build/nlopt/nlopt_src-prefix/src/nlopt_src-build/nlopt-2.4.2/api/options.c:

nlopt_result
NLOPT_STDCALL nlopt_set_initial_step(nlopt_opt opt, const double *dx)
{
     unsigned i;
     if (!opt) return NLOPT_INVALID_ARGS;
     if (!dx) {
	  free(opt->dx); opt->dx = NULL;
	  return NLOPT_SUCCESS;
     }
     for (i = 0; i < opt->n; ++i) if (dx[i] == 0) return NLOPT_INVALID_ARGS;
     if (!opt->dx && nlopt_set_initial_step1(opt, 1) == NLOPT_OUT_OF_MEMORY)
          return NLOPT_OUT_OF_MEMORY;
     memcpy(opt->dx, dx, sizeof(double) * (opt->n));
     return NLOPT_SUCCESS;
}

The dx values from my understanding are the initial_step values, so for some i, (dx[i] == 0) evaluates to true and NLOPT_INVALID_ARGS is returned. This enters the mythrow function (in the file ~/catkin_ws/devel/include/nlopt.hpp):

void mythrow(nlopt_result ret) const {
  switch (ret) {
  case NLOPT_FAILURE: throw std::runtime_error("nlopt failure");
  case NLOPT_OUT_OF_MEMORY: throw std::bad_alloc();
  case NLOPT_INVALID_ARGS: throw std::invalid_argument("nlopt invalid argument");
  case NLOPT_ROUNDOFF_LIMITED: throw roundoff_limited();
  case NLOPT_FORCED_STOP: throw forced_stop();
  default: break;
  }
}

So, "nlopt invalid argument" is returned and therefore the catch statement is entered back here, printing:

E0613 19:50:25.126937 11907 polynomial_optimization_nonlinear_impl.h:214] error while setting up nlopt: nlopt invalid argument

Conclusion

The nonlinear optimization does not run when this error is thrown, thus we're left with the linear solution in this case. This error is, in my understanding, associated with nlopt complaining that we are providing it with an initial step which does not "explore" some of the dimensions of the optimization variable - which is natural, because in the case of a coplanar trajectory the remaining dimension (e.g. z) has no intention of being explored. So we should not be stopping the nonlinear optimization from running because of it...

So, could something be done about this error being thrown, please? @simonlynen Perhaps fixing this requires a change to nlopt?

Adding orientation constraints

Can someone help me understand adding constraints for ORIENTATION.

I have specified 3 vertexes, in which the middle one has a position and orientation constrain. I am confused about how to specify to actual constraint. I have tried

middle.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(2,2,2)); middle.addConstraint(mav_trajectory_generation::derivative_order::ORIENTATION, Eigen::Vector3d(R,P,Y));

Also, the dimension of the trajectory is 3, so do I need trajectories in RPY for this constrain to work?

rviz crashes on drawMavTrajectory

Hi,

I'm not sure if this repo is the correct place to put this issue, but I seem to be having some trouble using the function drawMavTrajectory with ROS Kinetic on Ubuntu 16.04.

Here's what happened:
I tried to run waypoint_navigator from mav_pathplanning as an example of using mav_trajectory generation. As per the README, the node should publish /waypoint_navigator_polynomial_markers (visualization_msgs/MarkerArray - rviz marker for polynomial paths) using the function drawMavTrajectory. However, when I try displaying these markers in rviz, I get the following errors in the console, followed by a rviz crash:

[ INFO] [1492718927.355060126]: rviz version 1.12.4
[ INFO] [1492718927.355167276]: compiled against Qt version 5.5.1
[ INFO] [1492718927.355194424]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1492718927.986305039, 102.220000000]: Stereo is NOT SUPPORTED
[ INFO] [1492718927.986463253, 102.220000000]: OpenGl version: 3 (GLSL 1.3).
rviz: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreNode.cpp:630: virtual void Ogre::Node::setScale(const Ogre::Vector3&): Assertion `!inScale.isNaN() && "Invalid vector supplied as parameter"' failed.
Aborted (core dumped)

This might be a Kinetic/16.04 issue, because I remember the visualization tools worked fine on my old 14.04 machine. Has anyone experienced this before? Is there another example I could try for debugging? Any advice on how to proceed would be greatly appreciated!

API to provide full-orientation constraints

Unless I missed something, at the moment waypoints can only feature a position or a a position and a yaw angle, while internally they are stored as full orientation using quaternions.
It would be nice if it was possible to provide that as well using addConstraint

build error for catkin build

Hi
I get some trouble in the process of catkinbuild
Errors << glog_catkin:make /home/xl/catkin_ws/logs/glog_catkin/build.make.014.log
make[3]: 警告: 子 make 中强制 -jN: 关闭 jobserver 模式。
src/utilities_unittest-utilities_unittest.o: In function __static_initialization_and_destruction_0': /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:93: undefined reference to google::FlagRegisterer::FlagRegistererstd::string(char const*, char const*, char const*, std::string*, std::string*)'
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:94: undefined reference to google::FlagRegisterer::FlagRegisterer<std::string>(char const*, char const*, char const*, std::string*, std::string*)' /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:96: undefined reference to google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, bool*, bool*)'
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:100: undefined reference to google::FlagRegisterer::FlagRegisterer<int>(char const*, char const*, char const*, int*, int*)' collect2: error: ld returned 1 exit status make[3]: *** [utilities_unittest] 错误 1 make[3]: *** 正在等待未完成的任务.... src/demangle_unittest-demangle_unittest.o: In function __static_initialization_and_destruction_0':
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:93: undefined reference to google::FlagRegisterer::FlagRegisterer<std::string>(char const*, char const*, char const*, std::string*, std::string*)' /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:94: undefined reference to google::FlagRegisterer::FlagRegistererstd::string(char const*, char const*, char const*, std::string*, std::string*)'
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:96: undefined reference to google::FlagRegisterer::FlagRegisterer<bool>(char const*, char const*, char const*, bool*, bool*)' /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:100: undefined reference to google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, int*, int*)'
src/demangle_unittest-demangle_unittest.o: In function __static_initialization_and_destruction_0': /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/demangle_unittest.cc:49: undefined reference to google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, bool*, bool*)'
collect2: error: ld returned 1 exit status
make[3]: *** [demangle_unittest] 错误 1
src/symbolize_unittest-symbolize_unittest.o: In function __static_initialization_and_destruction_0': /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:93: undefined reference to google::FlagRegisterer::FlagRegistererstd::string(char const*, char const*, char const*, std::string*, std::string*)'
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:94: undefined reference to google::FlagRegisterer::FlagRegisterer<std::string>(char const*, char const*, char const*, std::string*, std::string*)' /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:96: undefined reference to google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, bool*, bool*)'
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:100: undefined reference to google::FlagRegisterer::FlagRegisterer<int>(char const*, char const*, char const*, int*, int*)' collect2: error: ld returned 1 exit status make[3]: *** [symbolize_unittest] 错误 1 src/logging_unittest-logging_unittest.o: In function __static_initialization_and_destruction_0':
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:93: undefined reference to google::FlagRegisterer::FlagRegisterer<std::string>(char const*, char const*, char const*, std::string*, std::string*)' /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:94: undefined reference to google::FlagRegisterer::FlagRegistererstd::string(char const*, char const*, char const*, std::string*, std::string*)'
/home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:96: undefined reference to google::FlagRegisterer::FlagRegisterer<bool>(char const*, char const*, char const*, bool*, bool*)' /home/xl/catkin_ws/build/glog_catkin/glog_src-prefix/src/glog_src/src/googletest.h:100: undefined reference to google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, int*, int*)'
collect2: error: ld returned 1 exit status
make[3]: *** [logging_unittest] 错误 1
make[2]: *** [glog_src-prefix/src/glog_src-stamp/glog_src-build] 错误 2
make[1]: *** [CMakeFiles/glog_src.dir/all] 错误 2
make: *** [all] 错误 2
cd /home/xl/catkin_ws/build/glog_catkin; catkin build --get-env glog_catkin | catkin env -si /usr/bin/make --jobserver-fds=6,7 -j; cd -
can you help me solve the question?thank you very much!

Unable to run example.launch

When I run the example.launch file and then press ENTER, I get the following error-:

[FATAL] [1582182912.640695319, 11.590000000]: Trying to publish message of type [mav_planning_msgs/PolynomialTrajectory/2daf5d705534e84f80980f4673a0e62b] on a publisher with type [mav_planning_msgs/PolynomialTrajectory4D/4d68d15524ede489eecd674bb6dc3ee8]
[FATAL] [1582182912.640714553, 11.590000000]:

Compilation under Mac OS

Hello. Were you able to compile the library under Mac OS
The fact that the function

// srs/vectex.cpp  162 line
std::vector<double> estimateSegmentTimes(const Vertex::Vector& vertices,
                                         double v_max, double a_max,
                                         double magic_fabian_constant = 6.5);

return NAN.

If you have experienced this, tell me how to solve this problem?

Best regards,
Devitt Dmitry

Feasibility and constraints

I am confused about input feasibility and constraints that you can specify for a 3 dimensional trajectory.

For example I can get infeasibility if I have the following set up:

std::vector<double> segment_times(vertices.size()-1,2);
const double v_max = 5.5;
const double a_max = 1.0*9.81;

nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max);

nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max);

now If I check the input_feasibility for my generated trajectory via:

mav_trajectory_generation::InputConstraints input_constraints;
input_constraints.addConstraint(ICT::kFMax, a_max); 
input_constraints.addConstraint(ICT::kVMax, v_max); 

I get isInfeasibleThrustHigh when I check all my segments.

I think this has something to do with how I setting my segment_times to all be 2 secs. So my question is: is segment_times a constraint that overcomes the velocity and accel constraints via addMaximumMagnitudeConstraint that I set.

FYI, In my application I need to specify the timing so setting segment_times is necessary. Any insight is greatly appreciated.

build error for mav_trajectory_generation

Hi,
I get the following build error. (I am trying to build in ros-kinetic).

CMake Error at /home/valada/catkin_ws_path_test/devel/share/catkin_simple/cmake/catkin_simple-extras.cmake:38 (find_package):
By not providing "Findmav_msgs.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "mav_msgs",
but CMake did not find one.

Could not find a package configuration file provided by "mav_msgs" with any
of the following names:

mav_msgsConfig.cmake
mav_msgs-config.cmake

Add the installation prefix of "mav_msgs" to CMAKE_PREFIX_PATH or set
"mav_msgs_DIR" to a directory containing one of the above files. If
"mav_msgs" provides a separate development package or SDK, be sure it has
been installed.

PS: first while building it asked for catkin_simple so i added it explicitly in the src folder.

Inertia matrix

How can I insert values of inertia matrix of a MAV for minimum snap trajectory generation ? And what are the default values of the inertia matrix which it is taking for calculation of minimum snap trajectory ?

how to use it with octomap

Hi,
I have recorded stereo data in rosbag. using which i can create a octomap and now i want to test this mav_trajectory_generation node.
what are all the topics needs to subscribed from the octomap to generate the smooth trajectory?
thanks!

--arun

maximizing velocity

Is there way to make sure that the optimizer maximizes velocity? I want to get a trajectory that allows me to reach the destination as fast as possible.

time budget

Is there a way to pass in a time budget for the solver, so that if it can't find the solution within the specified time duration, it should terminate?

reaching maximum velocity as soon as possible

Hi,

I would like the trajectory to be optimized for time (of arrival to destination), ie., reach maximum velocity as soon as possibly and maintain it so we can get to the destination as soon a possible.

However, I have noticed that the trajectory output slowly reaches the maximum velocity (about half away between the source and destination) and then goes back down as we get close to the destination. Is it possible to reach maximum velocity as soon as possible and maintain maximum velocity throughout the trajectory?

Thank you very much.

undefined reference to `YAML::detail::node_data::push_back(YAML::detail::node&, boost::shared_ptr<YAML::detail::memory_holder>)'

I got this error while running catkin build:

Errors     << mav_trajectory_generation:make /home/kadhir/catkin_ws/logs/mav_trajectory_generation/build.make.003.log                                                                   
/home/kadhir/catkin_ws/devel/lib/libmav_trajectory_generation.so: undefined reference to `YAML::detail::node_data::push_back(YAML::detail::node&, boost::shared_ptr<YAML::detail::memory_holder>)'
/home/kadhir/catkin_ws/devel/lib/libmav_trajectory_generation.so: undefined reference to `YAML::detail::node_data::convert_to_map(boost::shared_ptr<YAML::detail::memory_holder>)'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/kadhir/catkin_ws/devel/lib/mav_trajectory_generation/polynomial_timing_evaluation] Error 1
make[1]: *** [CMakeFiles/polynomial_timing_evaluation.dir/all] Error 2
make: *** [all] Error 2
cd /home/kadhir/catkin_ws/build/mav_trajectory_generation; catkin build --get-env mav_trajectory_generation | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

When following the installation instructions, I first ran into Could not find a package configuration file provided by "yaml_cpp_catkin". To solve that, I ran git clone https://github.com/ethz-asl/yaml_cpp_catkin.git inside ~/catkin_ws/src, and then reran catkin build, which is when I got this error. What steps do I take to fix it? Any help would be greatly appreciated!

wrong number of all constraints

Hi, as explained in the code below, you pre-allocate n_vertices * N / 2 memories for vector all_constraints.

all_constraints.reserve(
n_vertices_ * N /
2); // Will have exactly this number of elements in the end.

I think the right size should be (n_vertices - 1) * N, i.e. Num. of all constraints = Num. segments * N. But this result is still correct, because you re-calculate the size of total constraints.

catkin build fails for glog_catkin

I have tried to follow the installation and build instructions, however it would seem that the build process fails at the following step:

Workspace configuration appears valid.

[build] Found '8' packages in 0.0 seconds.
[build] Package table is up to date.
Starting >>> catkin_simple
Starting >>> nlopt
Finished <<< catkin_simple [ 0.1 seconds ]
Starting >>> eigen_catkin
Starting >>> glog_catkin
Finished <<< nlopt [ 0.1 seconds ]


Errors << glog_catkin:make /home/mbaldwin/logs/glog_catkin/build.make.008.log
CMake Error at glog_src-stamp/download-glog_src.cmake:21 (message):
error: downloading 'https://github.com/google/glog/archive/v0.3.5.zip'
failed

status_code: 1
status_string: "Unsupported protocol"
log: Protocol "https" not supported or disabled in libcurl

Closing connection -1

make[2]: *** [glog_src-prefix/src/glog_src-stamp/glog_src-download] Error 1
make[1]: *** [CMakeFiles/glog_src.dir/all] Error 2
make: *** [all] Error 2
cd /home/mbaldwin/catkin_ws/build/glog_catkin; catkin build --get-env glog_catkin | catkin env -si /usr/bin/make --jobserver-fds=6,7 -j; cd -
...............................................................................
Failed << glog_catkin:make [ Exited with code 2 ]
Failed <<< glog_catkin [ 0.1 seconds ]
Abandoned <<< eigen_checks [ Unrelated job failed ]
Abandoned <<< mav_visualization [ Unrelated job failed ]
Abandoned <<< mav_trajectory_generation [ Unrelated job failed ]
Abandoned <<< mav_trajectory_generation_ros [ Unrelated job failed ]
Finished <<< eigen_catkin [ 0.1 seconds ]
[build] Summary: 3 of 8 packages succeeded.
[build] Ignored: None.
[build] Warnings: None.
[build] Abandoned: 4 packages were abandoned.
[build] Failed: 1 packages failed.
[build] Runtime: 0.3 seconds total.

I have reinstalled and checked that curl does support https, and that it is pointing to the correct installation on my machine. Any ideas why this might be failing?

build error for glog_catkin

Hi,
I get the following build error. (I am trying to build in ros-kinetic).
configure: error: cannot guess build type; you must specify one
glog_catkin/CMakeFiles/glog_src.dir/build.make:108: recipe for target 'glog_catkin/glog_src-prefix/src/glog_src-stamp/glog_src-configure' failed
make[2]: *** [glog_catkin/glog_src-prefix/src/glog_src-stamp/glog_src-configure] Error 1

I tried to set the build type to release but that didn't work.

PS: I followed your install instruction
PS:Right before this error, I get the following message:
checking build system type... ./config.guess: unable to guess system type.

omav reference angular acceleraion

@maxb91 It seems that the reference angular acceleration in SquadOptimization::addToStates is not computed correctly, as states->at(i - 1).angular_velocity_W is zero when computing the omega_dot.
See the code here:

omega_dot = (omega - states->at(i - 1).angular_velocity_W) / dt;
states->at(i - 1).angular_velocity_W = omega;
states->at(i - 1).angular_acceleration_W = omega_dot;

I can update the code upon confirmation. Shouldn't be too hard.

Build error catkin_simple

I have a build error while following the installation process. The error that comes up is:

Errors << eigen_catkin:cmake /home/ashwin/catkin_ws/logs/eigen_catkin/build.cmake.001.log
CMake Error at /home/ashwin/catkin_ws/src/eigen_catkin/CMakeLists.txt:4 (find_package):
By not providing "Findcatkin_simple.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"catkin_simple", but CMake did not find one.

Could not find a package configuration file provided by "catkin_simple"
with any of the following names:

catkin_simpleConfig.cmake
catkin_simple-config.cmake

Add the installation prefix of "catkin_simple" to CMAKE_PREFIX_PATH or set
"catkin_simple_DIR" to a directory containing one of the above files. If
"catkin_simple" provides a separate development package or SDK, be sure it
has been installed.

OS : Ubuntu 16.04

Are there any dependencies that I'm missing?

Adding angular constraints

I'm using trajectory planner with obstacle avoidance. I have a front facing camera with my drone. So, I want the direction of the drone to be aligned with the direction of trajectory. How can I add the constraints to that?

Message type conflict: PolynomialTrajectory with PolynomialTrajectory4D

I am trying to use this package with waypoint_navigator, but I get the following error,

[ERROR] [1592562609.123919593]: Client [/trajectory_sampler] wants topic /path_segments to have datatype/md5sum [mav_planning_msgs/PolynomialTrajectory/2daf5d705534e84f80980f4673a0e62b], but our version has [mav_planning_msgs/PolynomialTrajectory4D/4d68d15524ede489eecd674bb6dc3ee8]. Dropping connection.

I found that in waypoint_navigator_node a publisher was created with message type PolynomialTrajectory4D. However, in trajectory_sampler_node a subscriber to the same topic path_segments has different message type of PolynomialTrajectory

Probably this requires a fix, no?

Issues on step catkin build mav_trajectory_generation_ros

Hi,
I am trying to install the repo but I faced an issue while trying:

catkin build mav_trajectory_generation_ros

I left it for 2 hours and it never finished, image attached:
image

It just kept blinking but nothing happend.

Thank you in advance.

Linear optimization bottlenecks

Hi all!
I am performing several tests with this code, and it feels like there is some optimization to do in it.
The reasoning behind this is because I want to calculate optimal segment times as fast as possible. So far, I made my own backtracking gradient descent (which depends only on the linear solver) and compared results with what is obtained from nlopt (without inequality constraints).
In my tests, I am getting to the solution in about 25%-50% faster than the time that nlopt does (using the same relative tolerance). See figure (number of points here means number of waypoints):

soltimeoptsegments

However, I feel like there is room for improvement in the linear solver, which would consequently improve the backtracking gradient descent method.

In order to find the bottlenecks for the linear solver, I started putting timers everywhere.
My initial assumption was that the bottleneck would be in the matrix inversions. I was wrong.
In order to make the bottlenecks evident, I solved the linear problem with 1500 waypoints on a quad-core machine (3.50GHz).
Total calculation time: 5.65s
Time spent on the function setupConstraintReorderingMatrix(): 2.51s
Time spent on the function updateSegmentsFromCompactConstraints(): 2.83s
Time spent on all the rest: 0.31s

At this point, it seems to me that I could only improve the solver by changing these functions. However, I got lost in nested code trying to understand everything.

If I understand it well, the setupConstraintReorderingMatrix() function is there to be able to further exploit the structure of the problem by having to invert a smaller matrix dp = -Rpp^-1 * Rpf * df
However, I don't think that the trade-off is paying off. Sparse matrix inversions can be quite fast, specially with use of the sparsesuite methods. My bet would be that it would be better to solve the full problem with lagrange multipliers and everything, and still be more efficient.
For instance, I just ran the problem above with 10.000 points. The time to calculate setupConstraintReorderingMatrix() was 220s, while the time to solve dp = -Rpp^-1 * Rpf * df (Rpp is 10.000x10.000) was 0.24s.

As for the function updateSegmentsFromCompactConstraints(), it seems to be responsible to populate the segments that goes into the output. I don't really understand why it is inefficient. Any ideas?

I am not sure what to ask here in this issue, but are you developers still active regarding this repository? It seems to me that I would have to dig deep to improve these two functions, but maybe you have better ideas on how to do it, given your experience with the code.

Documentation neglects to mention certain installation dependencies

When building according to the instructions, two issues pop up: missing cmake references to catkin_simple and yaml_cpp_catkin. There are existing issues (#79 and #102 ) that reference these problems, but both issues are closed with the simple solution to clone requisite repositories into the src directory of the catkin workspace.
However, this quintessential fix is not referenced anywhere in the Readme or the wiki. Anyone who installs this repo has to go down the same rabbithole of issue reports, and it would be convenient to mention in the Readme that you have to manually clone extra repositories. (I have provided a patch to do that)

Also convenient: have them pulled automatically, see PR #104

Segment Cost computation

Hello,

Thank you for open open sourcing your implementation.

In polynomial_optimization_linear_impl,
the segment cost is computed as 0.5 * c^T * Q * c

Can you please explain reason for multiplication factor of 0.5 ?

Thanks

Integration with PX4

Hi,

What do you think is the best way to use this trajectory with Pixhawk? PX4 currently does not have trajectory tracker. Sending position setpoints from the trajectory to PX4 position controller produces very bad trajectory tracking results. Do I have to do a geometric controller to fully use the advantages of the trajectory?

Thanks.

tiny questions

hello I am a beginner of trajectory generation problem and want to catch up with you guys. When I read your codes I got some small questions:
1.whats the meaning of BaseCofficients, why did you calculate these parameters and also for Candidates? What are they used for? Could you give me a favor in details?
Thanks in advanced!

Installation on Noetic

I am having trouble installing using ROS Noetic, will this functionality be available in the future?

$ sudo apt-get install python-wstool python-catkin-tools ros-noetic-cmake-modules libyaml-cpp-dev
Reading package lists... Done
Building dependency tree       
Reading state information... Done
Package python-wstool is not available, but is referred to by another package.
This may mean that the package is missing, has been obsoleted, or
is only available from another source
However the following packages replace it:
  python3-wstool

Package python-catkin-tools is not available, but is referred to by another package.
This may mean that the package is missing, has been obsoleted, or
is only available from another source

E: Package 'python-wstool' has no installation candidate
E: Package 'python-catkin-tools' has no installation candidate

wstool update fail

Hello! I have a problem with installation of packages. I setup catkin workspace and followed by installation instruction. I executed following commands:
cd src
wstool init
wstool set --git mav_trajectory_generation [email protected]:ethz-asl/mav_trajectory_generation.git -y
When I go to the next step and execute wstool update I get following error:

Prepare updating [email protected]:ethz-asl/mav_trajectory_generation.git (version None) to /home/evgenii/catkin_ws/src/mav_trajectory_gene
Failed to detect git presence at /home/evgenii/catkin_ws/src/mav_trajectory_generation.
(d)elete and replace, (a)bort, (b)ackup and replace, (s)kip: b
[mav_trajectory_generation] Fetching [email protected]:ethz-asl/mav_trajectory_generation.git (version None) to /home/evgenii/catkin_ws/src
[mav_trajectory_generation] Backing up /home/evgenii/catkin_ws/src/mav_trajectory_generation to /home/evgenii/catkin_ws/src/mav_trajecto-16-27
Cloning into '/home/evgenii/catkin_ws/src/mav_trajectory_generation'...
Warning: Permanently added the RSA host key for IP address '192.30.253.112' to the list of known hosts.
Permission denied (publickey).
fatal: Could not read from remote repository.

Please make sure you have the correct access rights
and the repository exists.
Exception caught during install: Error processing 'mav_trajectory_generation' : [mav_trajectory_generation] Checkout of [email protected]:eration.git version None into /home/evgenii/catkin_ws/src/mav_trajectory_generation failed.

ERROR in config: Error processing 'mav_trajectory_generation' : [mav_trajectory_generation] Checkout of [email protected]:ethz-asl/mav_trajon None into /home/evgenii/catkin_ws/src/mav_trajectory_generation failed.

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.