├── .github └── workflows │ └── cmake.yml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── Dockerfile ├── LICENSE ├── README.md ├── examples ├── circle_with_time.py ├── takeoff.csv ├── waypoints1.csv └── yaw_with_time.py ├── scripts ├── auto_yaw_trajectory.py ├── convert_to_bezier.py ├── gen_traj.py ├── generate_trajectory.py ├── plot_trajectory.py └── uav_trajectory.py └── src └── genTrajectory.cpp /.github/workflows/cmake.yml: -------------------------------------------------------------------------------- 1 | name: CMake 2 | 3 | on: [push, pull_request] 4 | 5 | env: 6 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 7 | BUILD_TYPE: Release 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - uses: actions/checkout@v2 15 | with: 16 | # recursively checkout all submodules 17 | submodules: 'recursive' 18 | 19 | - name: install dependencies 20 | run: | 21 | sudo apt update 22 | sudo apt install libeigen3-dev libboost-program-options-dev libboost-filesystem-dev libnlopt-cxx-dev libgoogle-glog-dev 23 | 24 | - name: Configure CMake 25 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 26 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 27 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} 28 | 29 | - name: Build 30 | # Build your program with the given configuration 31 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 32 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | __pycache__/ 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "mav_trajectory_generation"] 2 | path = mav_trajectory_generation 3 | url = https://github.com/whoenig/mav_trajectory_generation.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | project(uavTrajectoryHelpers) 4 | 5 | # Enable C++14 and warnings 6 | set(CMAKE_CXX_STANDARD 14) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_CXX_EXTENSIONS OFF) 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 10 | 11 | find_package(Eigen3 REQUIRED NO_MODULE) 12 | find_package(Boost 1.58 REQUIRED COMPONENTS program_options filesystem) 13 | find_package(PkgConfig REQUIRED) 14 | pkg_check_modules(NLPOPT REQUIRED nlopt nlopt_cxx) 15 | 16 | # Additional include folders 17 | include_directories( 18 | mav_trajectory_generation/mav_trajectory_generation/include 19 | ${EIGEN3_INCLUDE_DIR} 20 | ) 21 | 22 | add_library(mav_trajectory_generation 23 | mav_trajectory_generation/mav_trajectory_generation/src/motion_defines.cpp 24 | mav_trajectory_generation/mav_trajectory_generation/src/polynomial.cpp 25 | mav_trajectory_generation/mav_trajectory_generation/src/rpoly.cpp 26 | mav_trajectory_generation/mav_trajectory_generation/src/segment.cpp 27 | mav_trajectory_generation/mav_trajectory_generation/src/timing.cpp 28 | mav_trajectory_generation/mav_trajectory_generation/src/trajectory.cpp 29 | mav_trajectory_generation/mav_trajectory_generation/src/vertex.cpp 30 | ) 31 | 32 | add_executable(genTrajectory 33 | src/genTrajectory.cpp 34 | ) 35 | target_link_libraries(genTrajectory 36 | ${Boost_LIBRARIES} 37 | mav_trajectory_generation 38 | glog 39 | nlopt 40 | ) 41 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | from ubuntu:22.04 2 | 3 | RUN apt-get update && apt-get install -y git build-essential libnlopt-dev libnlopt-cxx-dev libgoogle-glog-dev cmake libeigen3-dev libboost-.*-dev 4 | 5 | ADD ./ /uav_trajectories/ 6 | 7 | RUN mkdir /uav_trajectories/build 8 | WORKDIR /uav_trajectories/build 9 | 10 | RUN cmake .. 11 | RUN make 12 | 13 | ENTRYPOINT [ "/uav_trajectories/build/genTrajectory" ] 14 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 whoenig 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # uav_trajectories 2 | Helper scripts and programs for trajectories to be used on UAVs 3 | 4 | # Requirements 5 | 6 | Parts of this software are based on [mav_trajectory_generation]: https://github.com/ethz-asl/mav_trajectory_generation, a software package developed at ETH Zurich, implementing a trajectory optimization approach developed at MIT. 7 | When using this work academically, follow their instructions on how to cite their work. 8 | We use the provided library in that package, but do not require ROS for execution. 9 | 10 | # Building 11 | 12 | Tested on Ubuntu 20.04. Install additional dependencies using: 13 | 14 | ``` 15 | sudo apt install libeigen3-dev libboost-program-options-dev libboost-filesystem-dev libnlopt-cxx-dev libgoogle-glog-dev 16 | ``` 17 | 18 | And on Fedora 33 with installed dependencies: 19 | ``` 20 | sudo dnf install NLopt-devel glog-devel eigen3-devel boost-devel 21 | ``` 22 | 23 | Clone and build this repository: 24 | 25 | ``` 26 | git clone --recursive https://github.com/whoenig/uav_trajectories.git 27 | mkdir uav_trajectories/build 28 | cd uav_trajectories/build 29 | cmake .. 30 | make 31 | ``` 32 | 33 | ## Docker image 34 | 35 | Instead of building the code locally, the Dockerfile can be used to containerize the application. 36 | This is useful if you don't want to install dependencies in your local environment or if you are 37 | using OSX.\ 38 | Before building the docker image 39 | ``` 40 | git submodule update --init --recursive 41 | ``` 42 | then, to build the image use 43 | 44 | ``` 45 | docker build . -t gentrajectory 46 | ``` 47 | 48 | Use it by mapping your local file system into the container. In this example the current dir 49 | is mapped to ```/module```. The genTrajectory application is running by default in the container 50 | and all you need to add is the arguments 51 | 52 | ```docker run --rm -it -v $PWD:/module gentrajectory --input /module/examples/waypoints1.csv --v_max 1.0 --a_max 1.0 -o /module/traj1.csv``` 53 | 54 | ## Polynomial Trajectories 55 | 56 | ### Generate Trajectory Given Waypoints 57 | 58 | This program takes a sequence of waypoints and dynamic quadrotor limits as inputs, and produces a smooth trajectory (with 0 derivatives at the beginning and end) that can be executed safely. 59 | All waypoints will be visited in order and the time of arrival at a waypoint is computed automatically. 60 | 61 | Example: 62 | 63 | ``` 64 | ./genTrajectory -i examples/waypoints1.csv --v_max 1.0 --a_max 1.0 -o traj1.csv 65 | ``` 66 | 67 | ### Generate Trajectory Given Time-Position Pairs 68 | 69 | This python script fits an 8th order polynomial through the given time/position pair. This can be useful to define a choreography of multiple UAVs. 70 | All derivatives are 0 at the beginning and the end. 71 | 72 | Example: 73 | 74 | ``` 75 | python3 ../examples/circle_with_time.py 76 | python3 ../scripts/generate_trajectory.py timed_waypoints_circle0.csv circle0.csv --pieces 5 77 | ``` 78 | 79 | Here, the first script generates timed waypoints and writes them into csv files. The second script fits 5 pieces of an 8th-order spline through the generated waypoints. 80 | 81 | This scripts also supports yaw. Example for an in-place yaw rotation: 82 | 83 | ``` 84 | python3 ../examples/yaw_with_time.py 85 | python3 ../scripts/generate_trajectory.py timed_waypoints_yaw.csv yaw0.csv --pieces 5 86 | python3 ../scripts/plot_trajectory.py yaw0.csv 87 | ``` 88 | 89 | ### Visualize Trajectory 90 | 91 | A python script can be used to visualize a trajectory csv-file (3D plot, velocity, acceleration, angular velocity, yaw). 92 | 93 | Example: 94 | 95 | ``` 96 | python3 ../scripts/plot_trajectory.py traj1.csv 97 | ``` 98 | 99 | ### Generate PDF and CSV 100 | A pyhton scirpt can be used to transform the generated csv files with coefficients of the polynomials to (x,y,z) position vector and its derivatives (up to the snap) in a csv format and plot the corresponding values. 101 | 102 | ``` 103 | python3 scripts/gen_traj.py --traj circle_0.csv --output circle_traj.csv --dt 0.01 --stretchtime 1.0 104 | ``` 105 | 106 | ### Convert Trajectory to Bezier 107 | 108 | This python scripts takes the trajectory generated by genTrajectory, and converts it to a bezier defined between times [0,1]. You should evaluate this bezier for a given time with f(t/duration). 109 | 110 | Example: 111 | 112 | ``` 113 | python3 ../scripts/convert_to_bezier.py traj1.csv bezier1.csv 114 | ``` 115 | -------------------------------------------------------------------------------- /examples/circle_with_time.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | numRobots = 5 4 | 5 | r = 0.3 6 | height = 0.7 7 | w = 2 * np.pi / numRobots 8 | T = 2 * 2 * np.pi / w 9 | 10 | # horizontal circles 11 | for i in range(0, numRobots): 12 | phase = 2 * np.pi / numRobots * i 13 | 14 | with open("timed_waypoints_circle{}.csv".format(i), "w") as f: 15 | f.write("t,x,y,z,yaw\n") 16 | 17 | for t in np.linspace(0, T, 100): 18 | f.write("{},{},{},{},{}\n".format(t, r * np.cos(w * t + phase), r * np.sin(w * t + phase), height, 0)) 19 | -------------------------------------------------------------------------------- /examples/takeoff.csv: -------------------------------------------------------------------------------- 1 | 0.0,0.0,0.0 2 | 0.0,0.0,1.0 3 | -------------------------------------------------------------------------------- /examples/waypoints1.csv: -------------------------------------------------------------------------------- 1 | 0.0,0.453548997641,1.4156037569 2 | 0.0,0.0507996380329,1.73595356941 3 | 0.0,0.0245594959706,1.40918636322 4 | 0.0,-0.406359314919,1.73907721043 5 | 0.0,-0.329923599958,1.54326617718 6 | 0.0,-0.600749194622,1.50608336926 7 | 0.0,-0.716766893864,1.59969651699 8 | 0.0,-0.494434058666,1.60480451584 9 | 0.0,-0.348284929991,1.41735374928 10 | 0.0,-0.810315489769,1.50478100777 11 | 0.0,-0.979983448982,1.60467255116 12 | 0.0,-0.792673826218,1.4386472702 13 | 0.0,-0.851812183857,1.3940089941 14 | 0.0,-1.28274035454,1.61001288891 15 | 0.0,-1.07801425457,1.44105386734 16 | 0.0,-1.13254892826,1.39263415337 17 | 0.0,-1.55885064602,1.60734117031 18 | 0.0,-1.56996059418,1.61550962925 19 | -------------------------------------------------------------------------------- /examples/yaw_with_time.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | omega = 10 #rad/sec 4 | T = 10 # sec 5 | 6 | with open("timed_waypoints_yaw.csv", "w") as f: 7 | f.write("t,x,y,z,yaw\n") 8 | 9 | for t in np.linspace(0, T, 100): 10 | f.write("{},{},{},{},{}\n".format(t, 0, 0, 0, omega*t)) 11 | -------------------------------------------------------------------------------- /scripts/auto_yaw_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from mpl_toolkits.mplot3d import Axes3D 6 | import matplotlib.gridspec as gridspec 7 | import argparse 8 | # import scipy.interpolate 9 | import scipy.optimize 10 | 11 | import uav_trajectory 12 | 13 | def func(coefficients, tss, yawss): 14 | result = 0 15 | for ts, yaws, i in zip(tss, yawss, range(0, len(tss))): 16 | yaws_output = np.polyval(coefficients[i*8:(i+1)*8], ts) 17 | result += np.sum((yaws - yaws_output) ** 2) 18 | # print(yaws_output) 19 | return result 20 | 21 | def func_eq_constraint_val(coefficients, i, tss, yawss): 22 | result = 0 23 | end_val = np.polyval(coefficients[(i-1)*8:i*8], tss[i-1][-1]) 24 | start_val = np.polyval(coefficients[i*8:(i+1)*8], tss[i][0]) 25 | # print(i, end_val, start_val) 26 | return end_val - start_val 27 | 28 | def func_eq_constraint_der(coefficients, i, tss, yawss): 29 | result = 0 30 | last_der = np.polyder(coefficients[(i-1)*8:i*8]) 31 | this_der = np.polyder(coefficients[i*8:(i+1)*8]) 32 | 33 | end_val = np.polyval(last_der, tss[i-1][-1]) 34 | start_val = np.polyval(this_der, tss[i][0]) 35 | return end_val - start_val 36 | 37 | def func_eq_constraint_der_value(coefficients, i, t, desired_value): 38 | result = 0 39 | der = np.polyder(coefficients[i*8:(i+1)*8]) 40 | 41 | value = np.polyval(der, t) 42 | return value - desired_value 43 | 44 | # def func_eq_constraint(coefficients, tss, yawss): 45 | # result = 0 46 | # last_derivative = None 47 | # for ts, yaws, i in zip(tss, yawss, range(0, len(tss))): 48 | # derivative = np.polyder(coefficients[i*8:(i+1)*8]) 49 | # if last_derivative is not None: 50 | # result += np.polyval(derivative, 0) - last_derivative 51 | # last_derivative = np.polyval(derivative, tss[-1]) 52 | 53 | 54 | # # apply coefficients to trajectory 55 | # for i,p in enumerate(traj.polynomials): 56 | # p.pyaw.p = coefficients[i*8:(i+1)*8] 57 | # # evaluate at each timestep and compute the sum of squared differences 58 | # result = 0 59 | # for t,yaw in zip(ts,yaws): 60 | # e = traj.eval(t) 61 | # result += (e.yaw - yaw) ** 2 62 | # return result 63 | 64 | if __name__ == "__main__": 65 | parser = argparse.ArgumentParser() 66 | parser.add_argument("trajectory", type=str, help="CSV file containing trajectory") 67 | parser.add_argument("output", type=str, help="CSV file containing trajectory with updated yaw") 68 | parser.add_argument("--num", type=int, default=20, help="number of sampled points per trajectory segment") 69 | args = parser.parse_args() 70 | 71 | traj = uav_trajectory.Trajectory() 72 | traj.loadcsv(args.trajectory) 73 | 74 | tss = [] 75 | yawss = [] 76 | for p in traj.polynomials: 77 | ts = np.linspace(0, p.duration, args.num) #np.arange(0, p.duration, args.dt) 78 | evals = np.empty((len(ts), 15)) 79 | for t, i in zip(ts, range(0, len(ts))): 80 | e = p.eval(t) 81 | evals[i, 0:3] = e.pos 82 | evals[i, 3:6] = e.vel 83 | evals[i, 6:9] = e.acc 84 | evals[i, 9:12] = e.omega 85 | evals[i, 12] = e.yaw 86 | evals[i, 13] = e.roll 87 | evals[i, 14] = e.pitch 88 | 89 | yaws = np.arctan2(evals[:,4], evals[:,3]) 90 | tss.append(ts) 91 | yawss.append(yaws) 92 | 93 | x0 = np.zeros(len(traj.polynomials) * 8) 94 | print(x0) 95 | 96 | constraints = [] 97 | for i in range(1, len(tss)): 98 | constraints.append({'type': 'eq', 'fun': func_eq_constraint_val, 'args': (i, tss, yawss)}) 99 | constraints.append({'type': 'eq', 'fun': func_eq_constraint_der, 'args': (i, tss, yawss)}) 100 | 101 | # zero derivative at the beginning and end 102 | constraints.append({'type': 'eq', 'fun': func_eq_constraint_der_value, 'args': (0, tss[0][0], 0)}) 103 | constraints.append({'type': 'eq', 'fun': func_eq_constraint_der_value, 'args': (len(tss)-1, tss[-1][-1], 0)}) 104 | 105 | 106 | res = scipy.optimize.minimize(func, x0, (tss, yawss), method="SLSQP", options={"maxiter": 1000}, 107 | constraints=constraints 108 | ) 109 | print(res) 110 | 111 | for i,p in enumerate(traj.polynomials): 112 | result = res.x[i*8:(i+1)*8] 113 | p.pyaw.p = np.array(result[::-1]) 114 | 115 | traj.savecsv(args.output) 116 | 117 | # current_t = 0.0 118 | # for p in traj.polynomials: 119 | # ts = np.arange(0, p.duration, args.dt) 120 | # evals = np.empty((len(ts), 15)) 121 | # for t, i in zip(ts, range(0, len(ts))): 122 | # e = p.eval(t) 123 | # evals[i, 0:3] = e.pos 124 | # evals[i, 3:6] = e.vel 125 | # evals[i, 6:9] = e.acc 126 | # evals[i, 9:12] = e.omega 127 | # evals[i, 12] = e.yaw 128 | # evals[i, 13] = e.roll 129 | # evals[i, 14] = e.pitch 130 | 131 | # # velocity = np.linalg.norm(evals[:,3:6], axis=1) 132 | # yaw = np.arctan2(evals[:,4], evals[:,3]) 133 | # print(yaw) 134 | # # yaw[0] = 0 135 | # # yaw[-1] = 0 136 | # result = np.polyfit(ts, yaw, deg=7) 137 | # # p.pyaw.p = np.append(result[::-1], [0,0,0]) 138 | # p.pyaw.p = np.array(result[::-1]) 139 | # # print(result) 140 | # # result = scipy.interpolate.UnivariateSpline(ts, yaw, k=5) 141 | # # print(result(ts)) 142 | 143 | # # p.pyaw.p = np.append(result.get_coeffs(), [0, 0]) 144 | # # print(p.pyaw.p) 145 | # # print(result.get_coeffs()) 146 | # traj.savecsv(args.output) 147 | 148 | 149 | 150 | 151 | 152 | # print(yaw) 153 | 154 | 155 | 156 | -------------------------------------------------------------------------------- /scripts/convert_to_bezier.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | import csv 4 | import math 5 | from numpy.linalg import inv 6 | import argparse 7 | 8 | parser = argparse.ArgumentParser(description="convert traj to bezier") 9 | parser.add_argument("inputfile", help="input file destination") 10 | parser.add_argument("outputfile", help="output file destination") 11 | args = parser.parse_args() 12 | 13 | trajfile = args.inputfile 14 | outputfile = args.outputfile 15 | 16 | 17 | def comb(a, b): 18 | f = math.factorial 19 | return f(a) / f(b) / f(a-b) 20 | 21 | 22 | trajstr = [] 23 | 24 | with open(trajfile, 'r') as csvfile: 25 | reader = csv.reader(csvfile, delimiter=',') 26 | for row in reader: 27 | trajstr.append(row) 28 | 29 | trajstr = trajstr[1:] 30 | traj = [] 31 | for l in trajstr: 32 | tr = [] 33 | for p in l[:25]: 34 | tr.append(float(p)) 35 | traj.append(tr) 36 | 37 | 38 | matrix = [] 39 | for k in range(0, 8): 40 | matrixrow = [] 41 | for i in range(0, 8): 42 | if i > k: 43 | matrixrow.append(0) 44 | else: 45 | eff = comb(7-i, k-i) * pow(-1, k-i) * comb(7,i) 46 | matrixrow.append(eff) 47 | matrix.append(matrixrow) 48 | 49 | 50 | 51 | matrixnp = np.array(matrix) 52 | invmtr = inv(matrixnp) 53 | 54 | out = open(outputfile, "w") 55 | 56 | out.write("#duration,p0x,p0y,p0z,...\n") 57 | 58 | for trj in traj: 59 | duration = trj[0] 60 | multip = 1 61 | for i in range(8): 62 | trj[1+i] *= multip 63 | trj[9+i] *= multip 64 | trj[17+i] *= multip 65 | multip *= duration 66 | 67 | xnp = np.transpose(np.array(trj[1:9])) 68 | ynp = np.transpose(np.array(trj[9:17])) 69 | znp = np.transpose(np.array(trj[17:])) 70 | xctrl = np.matmul(invmtr, xnp).tolist() 71 | yctrl = np.matmul(invmtr, ynp).tolist() 72 | zctrl = np.matmul(invmtr, znp).tolist() 73 | pstr = "" 74 | pstr += str(duration) + "," 75 | for i in range(7): 76 | pstr += str(xctrl[i]) + "," 77 | pstr += str(yctrl[i]) + "," 78 | pstr += str(zctrl[i]) + "," 79 | pstr += str(xctrl[-1]) + "," 80 | pstr += str(yctrl[-1]) + "," 81 | pstr += str(zctrl[-1]) 82 | out.write(pstr + "\n") 83 | out.close() 84 | -------------------------------------------------------------------------------- /scripts/gen_traj.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits.mplot3d import Axes3D 4 | import matplotlib.gridspec as gridspec 5 | import argparse 6 | import os 7 | from matplotlib.backends.backend_pdf import PdfPages 8 | import uav_trajectory 9 | 10 | if __name__ == "__main__": 11 | parser = argparse.ArgumentParser() 12 | parser.add_argument("--traj", type=str, help="CSV file containing polynomials") 13 | parser.add_argument("--output", type=str, help="CSV file containing pos, vel, acc, jerk, snap") 14 | parser.add_argument("--dt", default=0.01, type=float, help="CSV file containing polynomials") 15 | parser.add_argument("--stretchtime", default=1.0, type=float, help="stretch time factor (smaller means faster)") 16 | args = parser.parse_args() 17 | 18 | traj = uav_trajectory.Trajectory() 19 | traj.loadcsv(args.traj) 20 | 21 | 22 | traj.stretchtime(args.stretchtime) 23 | 24 | 25 | ts = np.arange(0, traj.duration, args.dt) 26 | # t, pos, vel, acc, jerk, snap: 1 + 15 27 | evals = np.empty((len(ts), 15 + 1)) # Additional column for 't' 28 | with open(args.output, "w") as f: 29 | for t, i in zip(ts, range(0, len(ts))): 30 | e = traj.eval(t) 31 | evals[i, 0] = t 32 | # pos[2] = 0 33 | evals[i, 1:4] = e.pos 34 | evals[i, 4:7] = e.vel 35 | evals[i, 7:10] = e.acc 36 | evals[i, 10:13] = e.jerk 37 | evals[i, 13:16] = e.snap 38 | header = "t,posx,posy,posz,velx,vely,velz,accx,accy,accz,jerkx,jerky,jerkz,snapx,snapy,snapz" 39 | 40 | evals_with_header = np.vstack((header.split(','), evals)) 41 | np.savetxt(f, evals_with_header, delimiter=",", fmt='%s') 42 | 43 | 44 | 45 | # Extract position, velocity, and acceleration, jerk, snap data from evals 46 | pos = evals[:, 1:4] 47 | vel = evals[:, 4:7] 48 | acc = evals[:, 7:10] 49 | jerk = evals[:, 10:13] 50 | snap = evals[:, 13:16] 51 | 52 | # Time steps (using dt from arguments) 53 | time_steps = ts 54 | 55 | # Create a PDF file to save the plots 56 | pdf_filename = os.path.splitext(args.output)[0] + '.pdf' 57 | with PdfPages(pdf_filename) as pdf: 58 | axes = ["x", "y", "z"] 59 | # Page 1: pos 60 | fig, ax = plt.subplots(3, 1, figsize=(8, 12)) 61 | for i in range(3): 62 | ax[i].plot(time_steps, pos[:, i], label=f'pos[{i}]', color='b') 63 | ax[i].set_xlabel('Time (s)') 64 | ax[i].set_ylabel(f' {axes[i]}') 65 | ax[i].legend() 66 | ax[i].grid(True) 67 | fig.suptitle('Position') 68 | pdf.savefig(fig) 69 | plt.close(fig) 70 | 71 | # Page 2: vel 72 | fig, ax = plt.subplots(3, 1, figsize=(8, 12)) 73 | for i in range(3): 74 | ax[i].plot(time_steps, vel[:, i], label=f'vel[{i}]', color='b') 75 | ax[i].set_xlabel('Time (s)') 76 | ax[i].set_ylabel(f' {axes[i]}') 77 | ax[i].legend() 78 | ax[i].grid(True) 79 | fig.suptitle('Velocity') 80 | pdf.savefig(fig) 81 | plt.close(fig) 82 | 83 | # Page 3: acc 84 | fig, ax = plt.subplots(3, 1, figsize=(8, 12)) 85 | for i in range(3): 86 | ax[i].plot(time_steps, acc[:, i], label=f'acc[{i}]', color='b') 87 | ax[i].set_xlabel('Time (s)') 88 | ax[i].set_ylabel(f' {axes[i]}') 89 | ax[i].legend() 90 | ax[i].grid(True) 91 | fig.suptitle('Acceleration') 92 | pdf.savefig(fig) 93 | plt.close(fig) 94 | 95 | 96 | # Page 4: jerk 97 | fig, ax = plt.subplots(3, 1, figsize=(8, 12)) 98 | for i in range(3): 99 | ax[i].plot(time_steps, jerk[:, i], label=f'jerk[{i}]', color='b') 100 | ax[i].set_xlabel('Time (s)') 101 | ax[i].set_ylabel(f' {axes[i]}') 102 | ax[i].legend() 103 | ax[i].grid(True) 104 | fig.suptitle('Jerk') 105 | pdf.savefig(fig) 106 | plt.close(fig) 107 | 108 | # Page 5: snap 109 | fig, ax = plt.subplots(3, 1, figsize=(8, 12)) 110 | for i in range(3): 111 | ax[i].plot(time_steps, snap[:, i], label=f'snap[{i}]', color='b') 112 | ax[i].set_xlabel('Time (s)') 113 | ax[i].set_ylabel(f' {axes[i]}') 114 | ax[i].legend() 115 | ax[i].grid(True) 116 | fig.suptitle('Snap') 117 | pdf.savefig(fig) 118 | plt.close(fig) 119 | 120 | -------------------------------------------------------------------------------- /scripts/generate_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from mpl_toolkits.mplot3d import Axes3D 6 | import matplotlib.gridspec as gridspec 7 | import argparse 8 | # import scipy.interpolate 9 | import scipy.optimize 10 | 11 | import uav_trajectory 12 | 13 | # computes the difference between current interpolation and desired values 14 | def func(coefficients, times, values, piece_length): 15 | result = 0 16 | i = 0 17 | for t, value in zip(times, values): 18 | if t > (i+1) * piece_length: 19 | i = i + 1 20 | estimate = np.polyval(coefficients[i*8:(i+1)*8], t - i * piece_length) 21 | # print(coefficients[i*8:(i+1)*8], t - i * piece_length, estimate) 22 | result += (value - estimate) ** 2 #np.sum((values - estimates) ** 2) 23 | # print(coefficients, result) 24 | return result 25 | 26 | # constraint to match values between spline pieces 27 | # def func_eq_constraint_val(coefficients, i, piece_length): 28 | # result = 0 29 | # end_val = np.polyval(coefficients[(i-1)*8:i*8], piece_length) 30 | # start_val = np.polyval(coefficients[i*8:(i+1)*8], 0) 31 | # return end_val - start_val 32 | 33 | def func_eq_constraint_der(coefficients, i, piece_length, order): 34 | result = 0 35 | last_der = np.polyder(coefficients[(i-1)*8:i*8], order) 36 | this_der = np.polyder(coefficients[i*8:(i+1)*8], order) 37 | 38 | end_val = np.polyval(last_der, piece_length) 39 | start_val = np.polyval(this_der, 0) 40 | return end_val - start_val 41 | 42 | def func_eq_constraint_der_value(coefficients, i, t, desired_value, order): 43 | result = 0 44 | der = np.polyder(coefficients[i*8:(i+1)*8], order) 45 | 46 | value = np.polyval(der, t) 47 | return value - desired_value 48 | 49 | # def func_eq_constraint(coefficients, tss, yawss): 50 | # result = 0 51 | # last_derivative = None 52 | # for ts, yaws, i in zip(tss, yawss, range(0, len(tss))): 53 | # derivative = np.polyder(coefficients[i*8:(i+1)*8]) 54 | # if last_derivative is not None: 55 | # result += np.polyval(derivative, 0) - last_derivative 56 | # last_derivative = np.polyval(derivative, tss[-1]) 57 | 58 | 59 | # # apply coefficients to trajectory 60 | # for i,p in enumerate(traj.polynomials): 61 | # p.pyaw.p = coefficients[i*8:(i+1)*8] 62 | # # evaluate at each timestep and compute the sum of squared differences 63 | # result = 0 64 | # for t,yaw in zip(ts,yaws): 65 | # e = traj.eval(t) 66 | # result += (e.yaw - yaw) ** 2 67 | # return result 68 | 69 | def generate_trajectory(data, num_pieces): 70 | piece_length = data[-1,0] / num_pieces 71 | 72 | x0 = np.zeros(num_pieces * 8) 73 | 74 | constraints = [] 75 | # piecewise values and derivatives have to match 76 | for i in range(1, num_pieces): 77 | for order in range(0, 4): 78 | constraints.append({'type': 'eq', 'fun': func_eq_constraint_der, 'args': (i, piece_length, order)}) 79 | 80 | # zero derivative at the beginning and end 81 | for order in range(1, 3): 82 | constraints.append({'type': 'eq', 'fun': func_eq_constraint_der_value, 'args': (0, 0, 0, order)}) 83 | constraints.append({'type': 'eq', 'fun': func_eq_constraint_der_value, 'args': (num_pieces-1, piece_length, 0, order)}) 84 | 85 | 86 | resX = scipy.optimize.minimize(func, x0, (data[:,0], data[:,1], piece_length), method="SLSQP", options={"maxiter": 1000}, 87 | constraints=constraints 88 | ) 89 | resY = scipy.optimize.minimize(func, x0, (data[:,0], data[:,2], piece_length), method="SLSQP", options={"maxiter": 1000}, 90 | constraints=constraints 91 | ) 92 | resZ = scipy.optimize.minimize(func, x0, (data[:,0], data[:,3], piece_length), method="SLSQP", options={"maxiter": 1000}, 93 | constraints=constraints 94 | ) 95 | 96 | resYaw = scipy.optimize.minimize(func, x0, (data[:,0], data[:,4], piece_length), method="SLSQP", options={"maxiter": 1000}, 97 | constraints=constraints 98 | ) 99 | 100 | traj = uav_trajectory.Trajectory() 101 | traj.polynomials = [uav_trajectory.Polynomial4D( 102 | piece_length, 103 | np.array(resX.x[i*8:(i+1)*8][::-1]), 104 | np.array(resY.x[i*8:(i+1)*8][::-1]), 105 | np.array(resZ.x[i*8:(i+1)*8][::-1]), 106 | np.array(resYaw.x[i*8:(i+1)*8][::-1])) for i in range(0, num_pieces)] 107 | 108 | traj.duration = data[-1,0] 109 | return traj 110 | 111 | 112 | if __name__ == "__main__": 113 | parser = argparse.ArgumentParser() 114 | parser.add_argument("input", type=str, help="CSV file containing time waypoints") 115 | parser.add_argument("output", type=str, help="CSV file containing trajectory with updated yaw") 116 | parser.add_argument("--pieces", type=int, default=5, help="number of pieces") 117 | args = parser.parse_args() 118 | 119 | data = np.loadtxt(args.input, delimiter=',', skiprows=1) 120 | traj = generate_trajectory(data, args.pieces) 121 | traj.savecsv(args.output) 122 | -------------------------------------------------------------------------------- /scripts/plot_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from mpl_toolkits.mplot3d import Axes3D 6 | import matplotlib.gridspec as gridspec 7 | import argparse 8 | 9 | import uav_trajectory 10 | 11 | if __name__ == "__main__": 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument("trajectory", type=str, help="CSV file containing trajectory") 14 | parser.add_argument("--stretchtime", type=float, help="stretch time factor (smaller means faster)") 15 | args = parser.parse_args() 16 | 17 | traj = uav_trajectory.Trajectory() 18 | traj.loadcsv(args.trajectory) 19 | 20 | if args.stretchtime: 21 | traj.stretchtime(args.stretchtime) 22 | 23 | ts = np.arange(0, traj.duration, 0.01) 24 | evals = np.empty((len(ts), 15)) 25 | for t, i in zip(ts, range(0, len(ts))): 26 | e = traj.eval(t) 27 | evals[i, 0:3] = e.pos 28 | evals[i, 3:6] = e.vel 29 | evals[i, 6:9] = e.acc 30 | evals[i, 9:12] = e.omega 31 | evals[i, 12] = e.yaw 32 | evals[i, 13] = e.roll 33 | evals[i, 14] = e.pitch 34 | 35 | velocity = np.linalg.norm(evals[:,3:6], axis=1) 36 | acceleration = np.linalg.norm(evals[:,6:9], axis=1) 37 | omega = np.linalg.norm(evals[:,9:12], axis=1) 38 | 39 | # print stats 40 | print("max speed (m/s): ", np.max(velocity)) 41 | print("max acceleration (m/s^2): ", np.max(acceleration)) 42 | print("max omega (rad/s): ", np.max(omega)) 43 | print("max roll (deg): ", np.max(np.degrees(evals[:,13]))) 44 | print("max pitch (deg): ", np.max(np.degrees(evals[:,14]))) 45 | 46 | # Create 3x1 sub plots 47 | gs = gridspec.GridSpec(6, 1) 48 | fig = plt.figure() 49 | 50 | ax = plt.subplot(gs[0:2, 0], projection='3d') # row 0 51 | ax.plot(evals[:,0], evals[:,1], evals[:,2]) 52 | 53 | ax = plt.subplot(gs[2, 0]) # row 2 54 | ax.plot(ts, velocity) 55 | ax.set_ylabel("velocity [m/s]") 56 | 57 | ax = plt.subplot(gs[3, 0]) # row 3 58 | ax.plot(ts, acceleration) 59 | ax.set_ylabel("acceleration [m/s^2]") 60 | 61 | ax = plt.subplot(gs[4, 0]) # row 4 62 | ax.plot(ts, omega) 63 | ax.set_ylabel("omega [rad/s]") 64 | 65 | ax = plt.subplot(gs[5, 0]) # row 5 66 | ax.plot(ts, np.degrees(evals[:,12])) 67 | ax.set_ylabel("yaw [deg]") 68 | 69 | # ax = plt.subplot(gs[6, 0]) # row 5 70 | # ax.plot(ts, np.degrees(evals[:,13])) 71 | # ax.set_ylabel("roll [deg]") 72 | 73 | # ax = plt.subplot(gs[7, 0]) # row 5 74 | # ax.plot(ts, np.degrees(evals[:,14])) 75 | # ax.set_ylabel("pitch [deg]") 76 | 77 | plt.show() 78 | 79 | -------------------------------------------------------------------------------- /scripts/uav_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | 4 | def normalize(v): 5 | norm = np.linalg.norm(v) 6 | assert norm > 0 7 | return v / norm 8 | 9 | 10 | class Polynomial: 11 | def __init__(self, p): 12 | self.p = p 13 | 14 | def stretchtime(self, factor): 15 | recip = 1.0 / factor; 16 | scale = recip 17 | for i in range(1, len(self.p)): 18 | self.p[i] *= scale 19 | scale *= recip 20 | 21 | # evaluate a polynomial using horner's rule 22 | def eval(self, t): 23 | assert t >= 0 24 | x = 0.0 25 | for i in range(0, len(self.p)): 26 | x = x * t + self.p[len(self.p) - 1 - i] 27 | return x 28 | 29 | # compute and return derivative 30 | def derivative(self): 31 | return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)]) 32 | 33 | 34 | class TrajectoryOutput: 35 | def __init__(self): 36 | self.pos = None # position [m] 37 | self.vel = None # velocity [m/s] 38 | self.acc = None # acceleration [m/s^2] 39 | self.jerk = None 40 | self.snap = None 41 | self.omega = None # angular velocity [rad/s] 42 | self.yaw = None # yaw angle [rad] 43 | self.roll = None # required roll angle [rad] 44 | self.pitch = None # required pitch angle [rad] 45 | 46 | 47 | # 4d single polynomial piece for x-y-z-yaw, includes duration. 48 | class Polynomial4D: 49 | def __init__(self, duration, px, py, pz, pyaw): 50 | self.duration = duration 51 | self.px = Polynomial(px) 52 | self.py = Polynomial(py) 53 | self.pz = Polynomial(pz) 54 | self.pyaw = Polynomial(pyaw) 55 | 56 | # compute and return derivative 57 | def derivative(self): 58 | return Polynomial4D( 59 | self.duration, 60 | self.px.derivative().p, 61 | self.py.derivative().p, 62 | self.pz.derivative().p, 63 | self.pyaw.derivative().p) 64 | 65 | def stretchtime(self, factor): 66 | self.duration *= factor 67 | self.px.stretchtime(factor) 68 | self.py.stretchtime(factor) 69 | self.pz.stretchtime(factor) 70 | self.pyaw.stretchtime(factor) 71 | 72 | # see Daniel Mellinger, Vijay Kumar: 73 | # Minimum snap trajectory generation and control for quadrotors. ICRA 2011: 2520-2525 74 | # section III. DIFFERENTIAL FLATNESS 75 | def eval(self, t): 76 | result = TrajectoryOutput() 77 | # flat variables 78 | result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)]) 79 | result.yaw = self.pyaw.eval(t) 80 | 81 | # 1st derivative 82 | derivative = self.derivative() 83 | result.vel = np.array([derivative.px.eval(t), derivative.py.eval(t), derivative.pz.eval(t)]) 84 | dyaw = derivative.pyaw.eval(t) 85 | 86 | # 2nd derivative 87 | derivative2 = derivative.derivative() 88 | result.acc = np.array([derivative2.px.eval(t), derivative2.py.eval(t), derivative2.pz.eval(t)]) 89 | 90 | # 3rd derivative 91 | derivative3 = derivative2.derivative() 92 | jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)]) 93 | result.jerk = jerk 94 | 95 | derivative4 = derivative3.derivative() 96 | result.snap = np.array([derivative4.px.eval(t), derivative4.py.eval(t), derivative4.pz.eval(t)]) 97 | thrust = result.acc + np.array([0, 0, 9.81]) # add gravity 98 | 99 | z_body = normalize(thrust) 100 | x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0]) 101 | y_body = normalize(np.cross(z_body, x_world)) 102 | x_body = np.cross(y_body, z_body) 103 | 104 | jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body) 105 | h_w = jerk_orth_zbody / np.linalg.norm(thrust) 106 | 107 | result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw]) 108 | 109 | # compute required roll/pitch angles 110 | result.pitch = np.arcsin(-x_body[2]) 111 | result.roll = np.arctan2(y_body[2], z_body[2]) 112 | 113 | return result 114 | 115 | 116 | class Trajectory: 117 | def __init__(self): 118 | self.polynomials = None 119 | self.duration = None 120 | 121 | def loadcsv(self, filename): 122 | data = np.loadtxt(filename, delimiter=",", skiprows=1, usecols=range(33), ndmin=2) 123 | self.polynomials = [Polynomial4D(row[0], row[1:9], row[9:17], row[17:25], row[25:33]) for row in data] 124 | self.duration = np.sum(data[:,0]) 125 | 126 | def savecsv(self, filename): 127 | data = np.empty((len(self.polynomials), 8*4+1)) 128 | for i, p in enumerate(self.polynomials): 129 | data[i,0] = p.duration 130 | data[i,1:9] = p.px.p 131 | data[i,9:17] = p.py.p 132 | data[i,17:25] = p.pz.p 133 | data[i,25:33] = p.pyaw.p 134 | np.savetxt(filename, data, fmt="%.6f", delimiter=",", header="duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7") 135 | 136 | def stretchtime(self, factor): 137 | for p in self.polynomials: 138 | p.stretchtime(factor) 139 | self.duration *= factor 140 | 141 | def eval(self, t): 142 | assert t >= 0 143 | assert t <= self.duration 144 | 145 | current_t = 0.0 146 | for p in self.polynomials: 147 | if t < current_t + p.duration: 148 | return p.eval(t - current_t) 149 | current_t = current_t + p.duration 150 | -------------------------------------------------------------------------------- /src/genTrajectory.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | 11 | using namespace Eigen; 12 | 13 | // see https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix 14 | template 15 | M load_csv (const std::string & path) { 16 | std::ifstream indata; 17 | indata.open(path); 18 | std::string line; 19 | std::vector values; 20 | uint rows = 0; 21 | while (std::getline(indata, line)) { 22 | std::stringstream lineStream(line); 23 | std::string cell; 24 | while (std::getline(lineStream, cell, ',')) { 25 | values.push_back(std::stod(cell)); 26 | } 27 | ++rows; 28 | } 29 | return Map>(values.data(), rows, values.size()/rows); 30 | } 31 | 32 | int main(int argc, char **argv) 33 | { 34 | std::string inputFile; 35 | std::string outputFile; 36 | double v_max; 37 | double a_max; 38 | 39 | namespace po = boost::program_options; 40 | 41 | po::options_description desc("Allowed options"); 42 | desc.add_options() 43 | ("help", "produce help message") 44 | ("input,i", po::value(&inputFile)->required(), "path to list of waypoints") 45 | ("output,o", po::value(&outputFile)->required(), "path to output files") 46 | ("v_max", po::value(&v_max)->default_value(1.0), "maximum velocity [m/s]") 47 | ("a_max", po::value(&a_max)->default_value(1.0), "maximum velocity [m/s^2]") 48 | ; 49 | 50 | try 51 | { 52 | po::variables_map vm; 53 | po::store(po::parse_command_line(argc, argv, desc), vm); 54 | po::notify(vm); 55 | 56 | if (vm.count("help")) { 57 | std::cout << desc << "\n"; 58 | return 0; 59 | } 60 | } 61 | catch(po::error& e) 62 | { 63 | std::cerr << e.what() << std::endl << std::endl; 64 | std::cerr << desc << std::endl; 65 | return 1; 66 | } 67 | 68 | if (!boost::filesystem::exists(inputFile)) { 69 | std::cerr << "No such file: " << inputFile << std::endl; 70 | return 1; 71 | } 72 | 73 | mav_trajectory_generation::Vertex::Vector vertices; 74 | const int dimension = 3; 75 | const int derivative_to_optimize = mav_trajectory_generation::derivative_order::JERK; 76 | 77 | // create vertices with their constraints 78 | auto input = load_csv >(inputFile); 79 | 80 | if (input.rows() < 2) { 81 | std::cerr << "Not enough datapoints given!" << std::endl; 82 | return 1; 83 | } 84 | 85 | if (input.rows() == 2) { 86 | mav_trajectory_generation::Vertex v1(dimension); 87 | v1.makeStartOrEnd(input.row(0), derivative_to_optimize); 88 | vertices.push_back(v1); 89 | 90 | mav_trajectory_generation::Vertex v2(dimension); 91 | auto middle = (input.row(1) - input.row(0)) * 0.5 + input.row(0); 92 | v2.addConstraint(mav_trajectory_generation::derivative_order::POSITION, middle); 93 | vertices.push_back(v2); 94 | 95 | mav_trajectory_generation::Vertex v3(dimension); 96 | v3.makeStartOrEnd(input.row(1), derivative_to_optimize); 97 | vertices.push_back(v3); 98 | } else { 99 | // at least 3 points given 100 | for (int row = 0; row < input.rows(); ++row) { 101 | // std::cout << input.row(row) << std::endl; 102 | mav_trajectory_generation::Vertex vertex(dimension); 103 | if (row == 0 || row == input.rows() - 1) { 104 | vertex.makeStartOrEnd(input.row(row), derivative_to_optimize); 105 | } else { 106 | vertex.addConstraint(mav_trajectory_generation::derivative_order::POSITION, input.row(row)); 107 | } 108 | vertices.push_back(vertex); 109 | } 110 | } 111 | 112 | // compute segment times 113 | std::vector segment_times; 114 | const double magic_fabian_constant = 6.5; // A tuning parameter. 115 | segment_times = estimateSegmentTimes(vertices, v_max, a_max, magic_fabian_constant); 116 | 117 | // solve 118 | const int N = 8; 119 | mav_trajectory_generation::Segment::Vector segments; 120 | 121 | #if SOLVE_LINEAR 122 | mav_trajectory_generation::PolynomialOptimization opt(dimension); 123 | opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); 124 | opt.solveLinear(); 125 | 126 | // Obtain the polynomial segments. 127 | opt.getSegments(&segments); 128 | #else 129 | mav_trajectory_generation::NonlinearOptimizationParameters parameters; 130 | parameters.max_iterations = 1000; 131 | parameters.f_rel = 0.05; 132 | parameters.x_rel = 0.1; 133 | parameters.time_penalty = 500.0; 134 | parameters.initial_stepsize_rel = 0.1; 135 | parameters.inequality_constraint_tolerance = 0.1; 136 | 137 | mav_trajectory_generation::PolynomialOptimizationNonLinear opt(dimension, parameters, false); 138 | opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); 139 | opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max); 140 | opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max); 141 | opt.optimize(); 142 | 143 | // Obtain the polynomial segments. 144 | opt.getPolynomialOptimizationRef().getSegments(&segments); 145 | #endif 146 | 147 | std::ofstream output(outputFile); 148 | 149 | output << "Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7" << std::endl; 150 | Eigen::IOFormat csv_fmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "", ""); 151 | 152 | for (const auto& segment : segments) { 153 | output << segment.getTime() << ","; 154 | for (const auto& polynomial : segment.getPolynomialsRef()) { 155 | Eigen::VectorXd coefficients = polynomial.getCoefficients(); 156 | output << coefficients.format(csv_fmt) << ","; 157 | } 158 | output << "0,0,0,0,0,0,0,0" << std::endl; 159 | } 160 | 161 | return 0; 162 | } 163 | --------------------------------------------------------------------------------