{"id":19526615,"url":"https://github.com/zju-fast-lab/am_traj","last_synced_at":"2025-04-26T10:31:24.971Z","repository":{"id":84897516,"uuid":"242753529","full_name":"ZJU-FAST-Lab/am_traj","owner":"ZJU-FAST-Lab","description":"Alternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight","archived":false,"fork":false,"pushed_at":"2021-06-10T18:08:30.000Z","size":70636,"stargazers_count":185,"open_issues_count":0,"forks_count":46,"subscribers_count":11,"default_branch":"master","last_synced_at":"2024-11-07T14:02:57.427Z","etag":null,"topics":["motion-planning","quadrotor","robotics","trajectory-optimization"],"latest_commit_sha":null,"homepage":null,"language":"C++","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"gpl-3.0","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/ZJU-FAST-Lab.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,"roadmap":null,"authors":null}},"created_at":"2020-02-24T14:12:21.000Z","updated_at":"2024-09-13T08:15:53.000Z","dependencies_parsed_at":"2023-03-15T20:00:32.923Z","dependency_job_id":null,"html_url":"https://github.com/ZJU-FAST-Lab/am_traj","commit_stats":null,"previous_names":[],"tags_count":2,"template":false,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ZJU-FAST-Lab%2Fam_traj","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ZJU-FAST-Lab%2Fam_traj/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ZJU-FAST-Lab%2Fam_traj/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/ZJU-FAST-Lab%2Fam_traj/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/ZJU-FAST-Lab","download_url":"https://codeload.github.com/ZJU-FAST-Lab/am_traj/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":250972784,"owners_count":21516426,"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":["motion-planning","quadrotor","robotics","trajectory-optimization"],"created_at":"2024-11-11T01:10:56.222Z","updated_at":"2025-04-26T10:31:19.954Z","avatar_url":"https://github.com/ZJU-FAST-Lab.png","language":"C++","funding_links":[],"categories":[],"sub_categories":[],"readme":"# AM-Traj\n\nAlternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight\n\n# Deprecated !!!\n\nThis repo is deprecated.\n\nFor __local-planning__, it is encouraged to use our [__EGO-Planner__](https://github.com/ZJU-FAST-Lab/ego-planner). \n\nFor __general-purpose multicopter trajectory optimization__, it is encouraged to check our solver [__GCOPTER__](https://github.com/ZJU-FAST-Lab/GCOPTER) if released.\n\n## 0. About\n\n__AM-Traj__ is a __C++11__ [__header-only__](https://en.wikipedia.org/wiki/Header-only) library for generating large-scale piecewise polynomial trajectories for aggressive autonomous flights, with highlights on its superior computational efficiency and simultaneous spatial-temporal optimality. Besides, an extremely fast feasibility checker is designed for various kinds of constraints. All components in this framework leverage the algebraic convenience of polynomial trajectory optimization problem, thus our method is capable of computing a spatial-temporal optimal trajectory with 60 pieces within 5ms, i.e., 150Hz at least. You just need to include \"am_traj.hpp\" and \"root_finder.hpp\" in your code. __Please use the up-to-date master branch which may have a better performance than the one in our paper.__\n\n__Author__: [Zhepei Wang](https://zhepeiwang.github.io/) and [Fei Gao](https://ustfei.com/) from the [ZJU Fast Lab](http://zju-fast.com/).\n\n__Related Papers__:\n\n- [__Alternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight__](https://ieeexplore.ieee.org/document/9121729), Zhepei Wang, Xin Zhou, Chao Xu, Jian Chu, and Fei Gao, IEEE Robotics and Automation Letters (RA-L).\n- [__Detailed Proofs of Alternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight__](https://arxiv.org/abs/2002.09254), Zhepei Wang, Xin Zhou, Chao Xu, and Fei Gao, the supplementary material.\n\n  If you use our code for your academic research, please cite the first related paper.\n\n__Video Links__: [youtube](https://youtu.be/H89ALyWA2NI) or [bilibili](https://www.bilibili.com/video/av91537651)\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/cp.gif\" width = \"533\" height = \"300\"/\u003e\n\u003c/p\u003e\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/sv.gif\" width = \"427\" height = \"240\"/\u003e\n  \u003cimg src=\"misc/fpv.gif\" width = \"427\" height = \"240\"/\u003e\n\u003c/p\u003e\n\n## 1. Features\n\n- Only two headers are required for usage. The first one is \"am_traj.hpp\", which contains a polynomial trajectory optimizer. The second one is \"root_finder.hpp\", which originates from a self-developed [toolkit](https://github.com/ZJU-FAST-Lab/Root-Finder).\n\n- No complex third-party dependencies except [STL](https://en.wikipedia.org/wiki/Standard_Template_Library) and [Eigen](https://en.wikipedia.org/wiki/Eigen_(C%2B%2B_library)).\n\n- The library is able to generate __large-scale__ trajectories with __optimal coefficients__ and __optimal time allocations__ in __real-time__, without using general purpose [NLP](https://en.wikipedia.org/wiki/Nonlinear_programming) solvers. Both the unconstrained case as well as the constrained case are considered.\n\n- The library provides an efficient formulation for various feasibility checkers. Only algebraic operations are involved, which make our feasbility check for some high-order (\u003e4) constraints even __faster than traditional check by closed-form solutions__ for low-order constraints (\u003c=4). Therefore, the scalability makes it no longer painful to do feasibility check for high-order polynomial trajectories.\n\n- The library implements an extremely fast spatial trajectory solver when time allocation is prefixed and constraints are not considered. It uses a Banded LU Factorization which is much more efficient here than a general Sparse LU Factorization. The former has linear time complexity while the latter may get stuck in its \"analyzePattern\". Therefore, our lib __only requires 4 (or 18) seconds__ to generate an __Unconstrained Spatial (or Spatial-Temporal) Optimal Traj with 1000000 Segments__!!!\n\n## 2. Interface\n\nOnly three functions below are needed.\n\n### __Constructor__\n\n    AmTraj(double wT, double wA, double wJ, double mVr, double mAr, int mIts, double eps)\n\n  Inputs:\n\n    wT: Weight for the time regularization\n    wA: Weight for the integrated squared norm of acceleration\n    wJ: Weight for the integrated squared norm of jerk\n    mVr: Maximum velocity rate\n    mAr: Maximum acceleration rate\n    mIts: Maximum number of iterations in optimization\n    eps: Relative tolerance\n\n  Outputs:\n\n    An instance of AmTraj: An object used for trajectory generation\n\n  Example:\n\n    AmTraj amTrajOpt(1024.0, 32.0, 1.0, 7.0, 3.5, 32, 0.02);\n\n### __Unconstrained Spatial-Temporal Optimization__\n\n    Trajectory AmTraj::genOptimalTrajDT(const std::vector\u003cEigen::Vector3d\u003e \u0026wayPs, Eigen::Vector3d iniVel, Eigen::Vector3d iniAcc, Eigen::Vector3d finVel, Eigen::Vector3d finAcc) const\n\n  Inputs:\n\n    wayPs: Fixed waypoints for a trajectory\n    iniVel: Trajectory velocity at the first waypoint\n    iniAcc: Trajectory acceleration at the first waypoint\n    finVel: Trajectory velocity at the last waypoint\n    finAcc: Trajectory acceleration at the last waypoint\n\n  Outputs:\n\n    An instance of Trajectory: A trajectory with optimal coefficient matrices and optimal durations, on which constraints of maximum velocity/acceleration rate are not guaranteed\n  \n  Example:\n\n    Eigen::Vector3d iV(2.0, 1.0, 0.0), fV(0.0, 0.0, 0.0)\n    Eigen::Vector3d iA(0.0, 0.0, 2.0), fA(0.0, 0.0, 0.0)\n    std::vector\u003cEigen::Vector3d\u003e wPs;\n    wPs.emplace_back(0.0, 0.0, 0.0);\n    wPs.emplace_back(4.0, 2.0, 1.0);\n    wPs.emplace_back(9.0, 7.0, 5.0);\n    wPs.emplace_back(1.0, 3.0, 2.0);\n    Trajectory traj = amTrajOpt.genOptimalTrajDT(wPs, iV, iA, fV, fA);\n\n### __Constrained Spatial-Temporal Optimization__\n\n    Trajectory AmTraj::genOptimalTrajDTC(const std::vector\u003cEigen::Vector3d\u003e \u0026wayPs, Eigen::Vector3d iniVel, Eigen::Vector3d iniAcc, Eigen::Vector3d finVel, Eigen::Vector3d finAcc) const\n\n  Inputs:\n\n    wayPs: Fixed waypoints for a trajectory\n    iniVel: Trajectory velocity at the first waypoint\n    iniAcc: Trajectory acceleration at the first waypoint\n    finVel: Trajectory velocity at the last waypoint\n    finAcc: Trajectory acceleration at the last waypoint\n\n  Outputs:\n\n    An instance of Trajectory: A trajectory with optimal coefficient matrices and best time allocation, whose maximum velocity/acceleration rate are guaranteed to satisfy constraints\n  \n  Example:\n\n    Eigen::Vector3d iV(2.0, 1.0, 0.0), fV(0.0, 0.0, 0.0)\n    Eigen::Vector3d iA(0.0, 0.0, 2.0), fA(0.0, 0.0, 0.0)\n    std::vector\u003cEigen::Vector3d\u003e wPs;\n    wPs.emplace_back(0.0, 0.0, 0.0);\n    wPs.emplace_back(4.0, 2.0, 1.0);\n    wPs.emplace_back(9.0, 7.0, 5.0);\n    wPs.emplace_back(1.0, 3.0, 2.0);\n    Trajectory traj = amTrajOpt.genOptimalTrajDTC(wPs, iV, iA, fV, fA);\n\n### __The Others__\n\n  There are many useful functions in the header files. Use it by following their comments.\n\n## 3. Performance\n\n### 3.0 Performance over different feasibility checkers\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/comparefc.png\" width = \"496\" height = \"300\"/\u003e\n\u003c/p\u003e\n\nWe compare our feasibility check method with [Mueller's recursive bound check](https://doi.org/10.1109/TRO.2015.2479878), [Burri's analytical extrema check](https://doi.org/10.1109/IROS.2015.7353622), as well as the widely used sampling-based check. In each case, 1000 trajectory pieces are randomly generated along with maximum velocity rate constraints to estimate average time consumption. As is shown in above figure, our method outperforms all other methods in computation speed because of its resolution independence and scalability with higher polynomial orders. \n\n### 3.1 Performance over different trajectory optimizers\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/compareto.png\" width = \"700\" height = \"300\"/\u003e\n\u003c/p\u003e\n\nThe benchmark is done as follows: We generate a sequence of waypoints by random walk, of which the step is uniformly distributed over [-3.0m, 8.0m] for each axis. The maximum speed and acceleration rates are set to 5.0m/s and 3.5m/s^2, respectively. The derivatives on the first and the last waypoints are set to zero. The objective function is set as wT=512.0, wA=0.0, wJ=1.0. For a given number of pieces, each method is applied to 1000 sequences of waypoints. The cost is then normalized by the cost of unconstrained case of our method. All comparisons are conducted on an Intel Core i7-8700 CPU under Linux environment.\n\nWe compare our method with [Richter's method](https://doi.org/10.1177/0278364914558129) with time allocation optimized by [NLopt](https://nlopt.readthedocs.io/en/latest/) (QP + NLOPT) and [Mellinger's method](https://doi.org/10.1109/ICRA.2011.5980409) with durations properly scaled using [Liu's method](https://ieeexplore.ieee.org/document/7839930/) (BGD + Time Scaling). As is shown in the figure above, our optimizer has the fastest speed and the lowest cost when constraints are taken into consideration.\n\n## 4. Examples\n\nTwo examples are provided in this repository.\n\n### 4.0 Example 0\n\n  Example 0 contains a simple comparison between the constrained case of our method against conventional method which uses heuristic time allocation along with time scaling. The lap time, maximum velocity/acceleration rate, cost function value, and visualization are provided.\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/visual.png\" width = \"463\" height = \"300\"/\u003e\n\u003c/p\u003e\n\n- Building\n\n  To build this example, first make sure you have [ROS](http://wiki.ros.org/melodic/Installation/Ubuntu) properly installed. Desktop-full install is recommended. Second, clone this repository into the __src__ directory of your ROS workspace. Rename it am_traj then __move the example1 subdirectory to somewhere else__, because we only consider the building of example0 here. Use the following command to install Eigen if they are not ready.\n  \n      sudo apt install libeigen3-dev\n  \n  Now cd into your current ROS workspace\n\n      catkin_make\n\n- Running\n\n      source devel/setup.bash\n      rospack profile\n      roslaunch example0 example0.launch\n\n  Now you can see the example 0 is running with visualization showing in rviz.\n\n- Parameters Setting\n\n  It is strongly recommended that modifying parameters after roughly going through our related papers. The parameters for objective should satisfy the corresponding assumption. For example, zero weight for time is illegal. Although our method is quite efficient when initial guess is far from optimum, it is still a first-order method with no Hessian info leveraged. Technically speaking, the best convergence rate for first-order method under smooth nonconvex consumption is sublinear, which means higher precision requires much more iterations. Therefore, we suggest that the relative tol should be set as 0.02~0.001. Pratically, time allocations [1.00, 2.00, 4.00, 0.30] and [1.01, 2.02, 4.04, 0.303] won't make much difference for real-time usage.\n\n### 4.1 Example 1\n\n  Example 1 contains a complete global planner simulation procedure. In this example, we adopt an RRT* based waypoints selector proposed by [Richter et al.](https://doi.org/10.1177/0278364914558129). Based on the selector, we use our library to generate a optimal feasible trajectory. The whole planner is able to do global traejctory generation in real time.\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/example1viz.gif\" width = \"534\" height = \"300\"/\u003e\n\u003c/p\u003e\n\n- Building\n\n  To build this example, first make sure you have ROS and Eigen properly installed as described in previous example. Moreover, some parts of the simulation require GCC 7. Therefore, if your GCC version is below the required version, the following commands help you install it without changing your default compiler.\n  \n      sudo apt-get install -y gcc-7 g++-7 \u0026\u0026 sleep 0.2;\n      sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-5 60 --slave /usr/bin/g++ g++ /usr/bin/g++-5 \u0026\u0026 sleep 0.2;\n      sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 50 --slave /usr/bin/g++ g++ /usr/bin/g++-7;\n  \n  Second, use the following commands to install [PyGame](https://www.pygame.org/) and [OMPL](https://ompl.kavrakilab.org/).\n\n      sudo apt install python-pygame\n      cd ~/Desktop/\n      wget https://github.com/ompl/ompl/archive/1.4.2.zip\n      unzip 1.4.2.zip\n      cd ompl-1.4.2\n      mkdir build\n      cd build\n      cmake ..\n      make\n      sudo make install\n  \n  Third, clone this repository into the __src__ directory of your ROS workspace. Rename it am_traj. Now cd into your current ROS workspace.\n\n      catkin_make\n\n- Running\n  \n  Type following commands to start all nodes first.\n\n      source devel/setup.bash\n      rospack profile\n      roslaunch example1 example1.launch\n  \n  Open a new terminal in your current ROS workspace and then\n\n      source devel/setup.bash\n      python src/am_traj/example1/key2joy.py\n\n  Now you can see an rviz window and a pygame window, which are shown below.\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/twowindows.png\" width = \"533\" height = \"300\"/\u003e\n\u003c/p\u003e\n\n  First, we should keep the pygame window active by click it once. Then, press N on your keyboard to activate manual mode. Press W/A/S/D and Up/Down/Left/Right on your keyboard to control the white quadrotor's height/yaw and horizontal movement respectively. Please navigate the quadrotor to a safe region. After that, press M on your keyboary to enter auto mode such that global planner is activated.\n\n  To navigate the quadrotor to any region safely, you should click once the following button on tools bar of rviz.\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/navgbutton.png\" width = \"232\" height = \"74\"/\u003e\n\u003c/p\u003e\n\n  Once the button is clicked, you can move your mouse arrow to any free region you want. Click the place and hold it, then a green arrow appears as is shown in the following figure. You can change the direction of the green arrow. It is worth noting that, the angle between this arrow and positive part of x-axis (red axis) decides the relative height you want the quadrotor to keep at this region. The arrow in figure below gives 60 degrees which means the desired height is 60/180*MapHeight.\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/click.png\" width = \"280\" height = \"210\"/\u003e\n\u003c/p\u003e\n\n  Now unhold the click, the global planner will finds you an appropriate trajectory instantly. The quadrotor then tracks the trajectory.\n\n\u003cp align=\"center\"\u003e\n  \u003cimg src=\"misc/track.png\" width = \"533\" height = \"300\"/\u003e\n\u003c/p\u003e\n\n- Conclusion\n\n  In most cases, the planner in this example is far faster than sampling-based kinodynamic planner, which may take several seconds to find an relative good trajectory for quadrotors. However, the RRT* used for waypoints selection can still take a relative long time (0.05 ~ 0.10 s) in comparison with our trajectory optimizer (about 0.5 ms for unconstrained case and 2.0 ms for constrained case). What's more, the waypoint selector does not consider the dynamics. That's to say, it is possible that wierd waypoints may be selected, when initial speed is large or obstacles are too dense. We claim that this is the __inherent property__ of the adopted waypoint selector.\n\n## 5. Misc\n\n- The library implements a specific order of polynomial trajectory optimization. If higher or lower order is required, it is recommended to refer to our related papers, where the framework used in the library are described and analyzed more flexibly.\n\n- The self-developed toolkit \"root_finder.hpp\" used in this library is an portable substitution to the suggested modern univariate polynomial real-roots solver in our related paper. It is more efficient in our scene than the wide used [__TOMS493: Jenkins–Traub Algorithm__](https://en.wikipedia.org/wiki/Jenkins%E2%80%93Traub_algorithm). For detailed performance, please refer to the corresponding [repository](https://github.com/ZJU-FAST-Lab/Root-Finder). If efficiency is much more important to you, we suggest an closed-source solver [RS-ANewDsc](http://anewdsc.mpi-inf.mpg.de/) from Max Planck Institute for Informatics.\n\n- Currently, we only implement feasibilty checker for dynamic constraints. If only your constraints can be expressed in multivariate polynomial of trajectory or its higher derivatives. You can transform it into compatible format and utilize some functions in this library to accomplish simple yet solid check for it.\n\n- The library will support optimizing waypoints in the future, following the general framework in our paper. However, the spatial constraints depend on specific planning method. Therefore, feel free to modify all codes in the library if you need to optimize waypoints.\n\n- Thanks [Helen Oleynikova](https://github.com/helenol) et al. a lot for their [open-source project](https://github.com/ethz-asl/mav_trajectory_generation). In our benchmark, some modules in compared methods are modified from their projects.\n\n## 6. Licence\nThe source code is released under [GPLv3](http://www.gnu.org/licenses/) license.\n\n## 7. Maintaince\nFor any technical issues, please contact Zhepei WANG (\u003cwangzhepei@live.com\u003e) or Fei GAO (\u003cfgaoaa@zju.edu.cn\u003e).\n\nFor commercial inquiries, please contact Fei GAO (\u003cfgaoaa@zju.edu.cn\u003e).\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fzju-fast-lab%2Fam_traj","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fzju-fast-lab%2Fam_traj","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fzju-fast-lab%2Fam_traj/lists"}