{"id":13761900,"url":"https://github.com/ethz-asl/mav_trajectory_generation","last_synced_at":"2025-04-13T02:11:19.602Z","repository":{"id":47292957,"uuid":"76038638","full_name":"ethz-asl/mav_trajectory_generation","owner":"ethz-asl","description":"Polynomial trajectory generation and optimization, especially for rotary-wing MAVs.","archived":false,"fork":false,"pushed_at":"2023-05-20T10:49:20.000Z","size":1374,"stargazers_count":585,"open_issues_count":21,"forks_count":226,"subscribers_count":63,"default_branch":"master","last_synced_at":"2025-04-04T04:10:01.441Z","etag":null,"topics":[],"latest_commit_sha":null,"homepage":null,"language":"C++","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"apache-2.0","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/ethz-asl.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null}},"created_at":"2016-12-09T13:40:46.000Z","updated_at":"2025-04-03T13:39:44.000Z","dependencies_parsed_at":"2022-08-19T13:32:19.856Z","dependency_job_id":"879f26d6-0b59-483f-a73f-aeded17628ff","html_url":"https://github.com/ethz-asl/mav_trajectory_generation","commit_stats":null,"previous_names":[],"tags_count":0,"template":false,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ethz-asl%2Fmav_trajectory_generation","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ethz-asl%2Fmav_trajectory_generation/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ethz-asl%2Fmav_trajectory_generation/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ethz-asl%2Fmav_trajectory_generation/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/ethz-asl","download_url":"https://codeload.github.com/ethz-asl/mav_trajectory_generation/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":248654090,"owners_count":21140236,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":[],"created_at":"2024-08-03T14:00:31.384Z","updated_at":"2025-04-13T02:11:19.577Z","avatar_url":"https://github.com/ethz-asl.png","language":"C++","funding_links":[],"categories":["Repositories"],"sub_categories":["External Repositories"],"readme":"# mav_trajectory_generation\nThis repository contains tools for polynomial trajectory generation and optimization based on methods described in [1].\nThese techniques are especially suitable for rotary-wing micro aerial vehicles (MAVs).\nThis README provides a brief overview of our trajectory generation utilities with some examples.\n\n**Authors**: Markus Achtelik, Michael Burri, Helen Oleynikova, Rik Bähnemann, Marija Popović  \n**Maintainer**: Rik Bähnemann, brik@ethz.ch  \n**Affiliation**: Autonomous Systems Lab, ETH Zurich  \n\n## Bibliography\nThis 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):  \n[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.\n```\n@incollection{richter2016polynomial,\n  title={Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments},\n  author={Richter, Charles and Bry, Adam and Roy, Nicholas},\n  booktitle={Robotics Research},\n  pages={649--666},\n  year={2016},\n  publisher={Springer}\n}\n```\n\nFurthermore, the nonlinear optimization features our own extensions, described in:  \n\nMichael 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.\n```\n@inproceedings{burri2015real-time,\n  author={Burri, Michael and Oleynikova, Helen and  and Achtelik, Markus W. and Siegwart, Roland},\n  booktitle={Intelligent Robots and Systems (IROS 2015), 2015 IEEE/RSJ International Conference on},\n  title={Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Unknown Environments},\n  year={2015},\n  month={Sept}\n}\n```\n\n## Installation Instructions (Ubuntu)\nTo install this package with [ROS Indigo](http://wiki.ros.org/indigo/Installation/Ubuntu) or [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu):\n\n1. Install additional system dependencies (swap indigo for kinetic or melodic as necessary):\n\n** Note: ROS melodic requires libyaml-cpp-dev and does not build with yaml_cpp_catkin in your catkin workspace!\n\n```\nsudo apt-get install python-wstool python-catkin-tools ros-indigo-cmake-modules libyaml-cpp-dev\n```\n\n2. Set up a catkin workspace (if not already done):\n\n```\nmkdir -p ~/catkin_ws/src\ncd ~/catkin_ws\ncatkin init\ncatkin config --extend /opt/ros/indigo\ncatkin config --cmake-args -DCMAKE_BUILD_TYPE=Release\ncatkin config --merge-devel\n```\n\n3. Install the repository and its dependencies (with rosinstall):\n\n```\ncd src\nwstool init\nwstool set --git mav_trajectory_generation git@github.com:ethz-asl/mav_trajectory_generation.git -y\nwstool update\nwstool merge mav_trajectory_generation/install/mav_trajectory_generation_https.rosinstall\nwstool update -j8\necho \"source ~/catkin_ws/devel/setup.bash\" \u003e\u003e ~/.bashrc\nsource /opt/ros/indigo/setup.bash\n```\n\nIn case you have your SSH keys for github set up, feel free to use the ssh rosinstall instead:\n```\nwstool merge mav_trajectory_generation/install/mav_trajectory_generation_ssh.rosinstall\n```\n\n4. Use [catkin_build](http://catkin-tools.readthedocs.io/en/latest/verbs/catkin_build.html) to build the repository:\n\n```\ncatkin build mav_trajectory_generation_ros\n```\n\n\n## Basics\nA **vertex** describes the properties of a support point of a **polynomial** path. Pairs of vertices are connected together to form **segments**.\nEach vertex has a set of constraints: the values of position derivatives that must be matched during optimization procedures.\nTypically, only the positions are specified for vertices along a path, while start and end vertices have all derivatives of position set to zero.\n```\n  x----------x-----------------x\nvertex            segment\n```\nIn the case of multi-dimensional vertices, the derivative constraints exist in all dimensions, with possibly different values.\n\n## Linear Optimization\nIn 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].\n\nNecessary includes:\n```c++\n#include \u003cmav_trajectory_generation/polynomial_optimization_linear.h\u003e\n```\n\n1. Create a list of three (x,y,z) vertices to fly through, e.g. (0,0,1) -\u003e (1,2,3) -\u003e (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).\n\n```c++\nmav_trajectory_generation::Vertex::Vector vertices;\nconst int dimension = 3;\nconst int derivative_to_optimize = mav_trajectory_generation::derivative_order::SNAP;\nmav_trajectory_generation::Vertex start(dimension), middle(dimension), end(dimension);\n```\n\n2. Add constraints to the vertices.\n\n```c++\nstart.makeStartOrEnd(Eigen::Vector3d(0,0,1), derivative_to_optimize);\nvertices.push_back(start);\n\nmiddle.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(1,2,3));\nvertices.push_back(middle);\n\nend.makeStartOrEnd(Eigen::Vector3d(2,1,5), derivative_to_optimize);\nvertices.push_back(end);\n```\n\n3. Compute the segment times.\n\n```c++\nstd::vector\u003cdouble\u003e segment_times;\nconst double v_max = 2.0;\nconst double a_max = 2.0;\nsegment_times = estimateSegmentTimes(vertices, v_max, a_max);\n```\n\n4. 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.\n\n```c++\nconst int N = 10;\nmav_trajectory_generation::PolynomialOptimization\u003cN\u003e opt(dimension);\nopt.setupFromVertices(vertices, segment_times, derivative_to_optimize);\nopt.solveLinear();\n```\n\n5. Obtain the polynomial segments.\n\n```c++\nmav_trajectory_generation::Segment::Vector segments;\nopt.getSegments(\u0026segments);\n```\n\n## Nonlinear Optimization\nIn 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.\n\nNecessary includes:\n\n```c++\n#include \u003cmav_trajectory_generation/polynomial_optimization_nonlinear.h\u003e\n```\n\n1. Set up the problem by following Steps 1-3 in the **Linear Optimization** section.\n\n2. Set the parameters for nonlinear optimization. Below is an example, but the default parameters should be reasonable enough to use without fine-tuning.\n\n```c++\nNonlinearOptimizationParameters parameters;\nparameters.max_iterations = 1000;\nparameters.f_rel = 0.05;\nparameters.x_rel = 0.1;\nparameters.time_penalty = 500.0;\nparameters.initial_stepsize_rel = 0.1;\nparameters.inequality_constraint_tolerance = 0.1;\n```\n\n3. 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.\n\n```c++\nconst int N = 10;\nPolynomialOptimizationNonLinear\u003cN\u003e opt(dimension, parameters, false);\nopt.setupFromVertices(vertices, segment_times, derivative_to_optimize);\nopt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max);                                opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max);\nopt.optimize();\n```\n\n4. Obtain the polynomial segments.\n\n```c++\nmav_trajectory_generation::Segment::Vector segments;\nopt.getPolynomialOptimizationRef().getSegments(\u0026segments);\n```\n## Creating Trajectories\nIn this section, we consider how to use our trajectory optimization results. We first need to convert our optimization object into the Trajectory class:\n\n```c++\n#include \u003cmav_trajectory_generation/trajectory.h\u003e\n\nmav_trajectory_generation::Trajectory trajectory;\nopt.getTrajectory(\u0026trajectory);\n```\n\nWe can also create new trajectories by splitting (getting a trajectory with a single dimension) or compositing (getting a trajectory by appending another trajectory:\n\n```c++\n// Splitting:\nmav_trajectory_generation::Trajectory x_trajectory = trajectory.getTrajectoryWithSingleDimension(1);\n\n// Compositing:\nmav_trajectory_generation::Trajectory trajectory_with_yaw; trajectory.getTrajectoryWithAppendedDimension(yaw_trajectory, \u0026trajectory_with_yaw);\n```\n\n## Sampling Trajectories\nIn this section, we consider methods of evaluating the trajectory at particular instances of time. There are two methods of doing this.\n\n1. By evaluating directly from the Trajectory class.\n\n```c++\n// Single sample:\ndouble sampling_time = 2.0;\nint derivative_order = mav_trajectory_generation::derivative_order::POSITION;\nEigen::VectorXd sample = trajectory.evaluate(sampling_time, derivative_order);\n\n// Sample range:\ndouble t_start = 2.0;\ndouble t_end = 10.0;\ndouble dt = 0.01;\nstd::vector\u003cEigen::VectorXd\u003e result;\nstd::vector\u003cdouble\u003e sampling_times; // Optional.\ntrajectory.evaluateRange(t_start, t_end, dt, derivative_order, \u0026result, \u0026sampling_times);\n```\n\n2. 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).\n\n```c++\n#include \u003cmav_trajectory_generation_ros/trajectory_sampling.h\u003e\n\nmav_msgs::EigenTrajectoryPoint state;\nmav_msgs::EigenTrajectoryPoint::Vector states;\n\n// Single sample:\ndouble sampling_time = 2.0;\nbool success = mav_trajectory_generation::sampleTrajectoryAtTime(trajectory, sample_time, \u0026state);\n\n// Sample range:\ndouble t_start = 2.0;\ndouble duration = 10.0;\ndouble dt = 0.01;\nsuccess = mav_trajectory_generation::sampleTrajectoryInRange(trajectory, t_start, duration, dt, \u0026states);\n\n// Whole trajectory:\ndouble sampling_interval = 0.01;\nsuccess = mav_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_interval, \u0026states);\n```\n\n## Visualizing Trajectories\nIn this section, we describe how to visualize trajectories in [rviz](http://wiki.ros.org/rviz).\n\n\u003cp align=\"center\"\u003e\u003cimg src=\"http://i68.tinypic.com/2cp5oxs.png\" height=\"350\"/\u003e\u003c/p\u003e\n\nFor a simple visualization:\n\n```c++\n#include \u003cmav_trajectory_generation_ros/ros_visualization.h\u003e\n\nvisualization_msgs::MarkerArray markers;\ndouble distance = 1.0; // Distance by which to seperate additional markers. Set 0.0 to disable.\nstd::string frame_id = \"world\";\n\n// From Trajectory class:\nmav_trajectory_generation::drawMavTrajectory(trajectory, distance, frame_id, \u0026markers);\n\n// From mav_msgs::EigenTrajectoryPoint::Vector states:\nmav_trajectory_generation::drawMavSampledTrajectory(states, distance, frame_id, \u0026markers)\n```\n\nFor a visualization including an additional marker at a set distance (e.g. hexacopter marker):\n\n\n```c++\nmav_visualization::HexacopterMarker hex(simple);\n\n// From Trajectory class:\nmav_trajectory_generation::drawMavTrajectoryWithMavMarker(trajectory, distance, frame_id, hex \u0026markers);\n\n// From mav_msgs::EigenTrajectoryPoint::Vector states:\nmav_trajectory_generation::drawMavSampledTrajectoryWithMavMarker(states, distance, frame_id, hex, \u0026markers)\n```\n\n## Checking Input Feasibility\nThe package contains three implementations to check generated trajectories for\ninput feasibility. The checks are based on the rigid-body model assumption and\nflat state characteristics presented in [Mellinger2011](http://www-personal.acfr.usyd.edu.au/spns/cdm/papers/Mellinger.pdf).\n\n```\n@inproceedings{mellinger2011minimum,\n  title={Minimum snap trajectory generation and control for quadrotors},\n  author={Mellinger, Daniel and Kumar, Vijay},\n  booktitle={Robotics and Automation (ICRA), 2011 IEEE International Conference on},\n  pages={2520--2525},\n  year={2011},\n  organization={IEEE}\n}\n```\n\nThe trajectories are checked for low and high thrust, high velocities, high roll\nand pitch rates, high yaw rates and high yaw angular accelerations.\n\n`FeasibilitySampling` implements a naive sampling-based check.\n`FeasibilityRecursive` implements a slightly adapted recursive feasibility test\npresented in [Müller2015](http://flyingmachinearena.org/wp-content/publications/2015/mueTRO15.pdf).\n```\n@article{mueller2015computationally,\n  title={A computationally efficient motion primitive for quadrocopter trajectory generation},\n  author={Mueller, Mark W and Hehn, Markus and D'Andrea, Raffaello},\n  journal={IEEE Transactions on Robotics},\n  volume={31},\n  number={6},\n  pages={1294--1310},\n  year={2015},\n  publisher={IEEE}\n}\n```\nWe adapted [RapidQuadrocopterTrajectories](https://github.com/markwmuller/RapidQuadrocopterTrajectories) to\ncheck arbitrary polynomial order trajectories for yaw and velocity feasibility.\n\n`FeasibilityAnalytic` analytically checks the magnitudes except for the roll and\npitch rates, where it runs the recursive test (recommended: low in computation\ntime, no false positives).\n\n### Example\n```c++\n// Create input constraints.\ntypedef InputConstraintType ICT;\nInputConstraints input_constraints;\ninput_constraints.addConstraint(ICT::kFMin, 0.5 * 9.81); // minimum acceleration in [m/s/s].\ninput_constraints.addConstraint(ICT::kFMax, 1.5 * 9.81); // maximum acceleration in [m/s/s].\ninput_constraints.addConstraint(ICT::kVMax, 3.5); // maximum velocity in [m/s].\ninput_constraints.addConstraint(ICT::kOmegaXYMax, M_PI / 2.0); // maximum roll/pitch rates in [rad/s].\ninput_constraints.addConstraint(ICT::kOmegaZMax, M_PI / 2.0); // maximum yaw rates in [rad/s].\ninput_constraints.addConstraint(ICT::kOmegaZDotMax, M_PI); // maximum yaw acceleration in [rad/s/s].\n\n// Create feasibility object of choice (FeasibilityAnalytic,\n// FeasibilitySampling, FeasibilityRecursive).\nFeasibilityAnalytic feasibility_check(input_constraints);\nfeasibility_check.settings_.setMinSectionTimeS(0.01);\n\n// Check feasibility.\nSegment dummy_segment;\nInputFeasibilityResult result =\n    feasibility_check.checkInputFeasibility(dummy_segment);\nstd::cout \u003c\u003c \"The segment input is \" \u003c\u003c getInputFeasibilityResultName(result);\n\u003c\u003c \".\" \u003c\u003c std::endl;\n```\n\n### Benchmarking\nBoth recursive and analytic checks are comparably fast.\nThe recursive check may have a couple more false negatives, i.e., segments, that\ncan not be determined feasible although they are. But this is neglectable.\nThe sampling based check is both slow and may have false positives, i.e.,\nconsider segments feasible although they are not. We do not recommend using\nthis.\n\nHere are the computational results over 1000 random segments with different\nparameter settings:\n\u003cp align=\"center\"\u003e\u003cimg src=\"https://cloud.githubusercontent.com/assets/11293852/26199226/903dea9a-3bc9-11e7-945e-91e5a119e63f.png\" /\u003e\u003c/p\u003e\n\n## Checking Half-Space Feasibility\nThe package also contains a method to check if a trajectory or segment is inside\nan arbitrary set of half spaces based on [RapidQuadrocopterTrajectories](https://github.com/markwmuller/RapidQuadrocopterTrajectories).\nThis is useful to check if a segment is inside a box or above ground.\n\nExample ground plane feasibility:\n```c++\n// Create feasibility check.\nFeasibilityBase feasibility_check;\n// Create ground plane.\nEigen::Vector3d point(0.0, 0.0, 0.0);\nEigen::Vector3d normal(0.0, 0.0, 1.0);\nfeasibility_check.half_plane_constraints_.emplace_back(point, normal);\n// Check feasibility.\nSegment dummy_segment;\nif(!feasibility_check.checkHalfPlaneFeasibility(segment)) {\n  std::cout \u003c\u003c \"The segment is in collision with the ground plane.\" \u003c\u003c std::endl;\n}\n```\n\nExample box feasibility:\n```c++\n// Create feasibility check.\nFeasibilityBase feasibility_check;\n// Create box constraints.\nEigen::Vector3d box_center(0.0, 0.0, 0.0);\nEigen::Vector3d box_size(1.0, 1.0, 1.0);\nfeasibility_check.half_plane_constraints_ =\n    HalfPlane::createBoundingBox(box_center, box_size);\n// Check feasibility.\nSegment dummy_segment;\nif(!feasibility_check.checkHalfPlaneFeasibility(segment)) {\n  std::cout \u003c\u003c \"The segment is not inside the box.\" \u003c\u003c std::endl;\n}\n```\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fethz-asl%2Fmav_trajectory_generation","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fethz-asl%2Fmav_trajectory_generation","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fethz-asl%2Fmav_trajectory_generation/lists"}