whoenig / uav_trajectories

Helper scripts and programs for trajectories

uav_trajectories

Helper scripts and programs for trajectories to be used on UAVs

Requirements

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. When using this work academically, follow their instructions on how to cite their work. We use the provided library in that package, but do not require ROS for execution.

Building

Tested on Ubuntu 20.04. Install additional dependencies using:

sudo apt install libeigen3-dev libboost-program-options-dev libboost-filesystem-dev libnlopt-cxx-dev libgoogle-glog-dev

And on Fedora 33 with installed dependencies:

sudo dnf install NLopt-devel glog-devel eigen3-devel boost-devel

Clone and build this repository:

git clone --recursive https://github.com/whoenig/uav_trajectories.git
mkdir uav_trajectories/build
cd uav_trajectories/build
cmake ..
make

Docker image

Instead of building the code locally, the Dockerfile can be used to containerize the application. This is useful if you don't want to install dependencies in your local environment or if you are using OSX.

To build the image use

docker build . -t gentrajectory

Use it by mapping your local file system into the container. In this example the current dir is mapped to /module. The genTrajectory application is running by default in the container and all you need to add is the arguments

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

Polynomial Trajectories

Generate Trajectory Given Waypoints

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. All waypoints will be visited in order and the time of arrival at a waypoint is computed automatically.

Example:

./genTrajectory -i examples/waypoints1.csv --v_max 1.0 --a_max 1.0 -o traj1.csv

Generate Trajectory Given Time-Position Pairs

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. All derivatives are 0 at the beginning and the end.

Example:

python3 ../examples/circle_with_time.py
python3 ../scripts/generate_trajectory.py timed_waypoints_circle0.csv circle0.csv --pieces 5

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.

This scripts also supports yaw. Example for an in-place yaw rotation:

python3 ../examples/yaw_with_time.py
python3 ../scripts/generate_trajectory.py timed_waypoints_yaw.csv yaw0.csv --pieces 5
python3 ../scripts/plot_trajectory.py yaw0.csv

Visualize Trajectory

A python script can be used to visualize a trajectory csv-file (3D plot, velocity, acceleration, angular velocity, yaw).

Example:

python3 ../scripts/plot_trajectory.py traj1.csv

Convert Trajectory to Bezier

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

Example:

python3 ../scripts/convert_to_bezier.py traj1.csv bezier1.csv

uav_trajectories's Issues

Time allocation in trajectory design

Hi Whoenig, when I was looking source code of genTrajectory.cpp. I found that the time allocation of each segment you choose is 6.5:

uav_trajectories/src/genTrajectory.cpp

Line 109 in 278d92d

const double magic_fabian_constant = 6.5; // A tuning parameter.

Can I ask how you get this "magic" time allocation?

Circle trajectory for less than 5 crazyflies

Hi @whoenig I wanted to ask how should I modify the circle_with_time script to accommodate only 2 or 3 Crazyflies doing circles. (Current script is for 5 CFs)

Also, once I get the trajectory which crazyflie_examples (in Crazyswarm1.0 or Crazyswarm2.0) script should I modify and where to modify in that script to do the planned trajectory?

How to translate the trajectory I want to design into .csv format

Hi all. I just began to study the quadrotor motion planning. I would like to design a trajectory for my quadrotor to fly. I have a question: how could I express the trajectory I want to design using waypoints, which means how could translate my trajectory into .csv format?

What is in waypoints1.csv?

Hello @whoenig ,

Thank you for creating this useful repository. I am trying to create waypoints for the drone. I am having difficulty understanding the waypoints1.csv file in the example folder. I see that there are three columns. The first column consists of zeros, and there are some numbers in the second and third columns. I am confused about what is in the second and third columns. I have gone through the previous isse How to translate the trajectory I want to design into .csv format, particularly this discussion. It seems that I just need to provide some x,y,z coordinates. However, I am confused about which column represents x, y and z. From the visualization is that first column is x, second column is y and third column is z. Please correct me if I am wrong. Thank you so much for your response.

Drone swarm formation path-planning for collision avoidance.

Hi,

I'm working on a project that involves a large number of drones that are supposed to takeoff from an initial grid, and create a set of formations one after another, at the end of which the drones land at their initial starting positions, how can I use this to plan the trajectories for each of the drones in the formation such that collision avoidance is guaranteed. There are no static obstacles in the simulation, the only obstacles are the drones themselves.

Thanks!
P.S. The number of drones may range from 100 - 5000.

What made Trajectories capable to control more CF?

Hi

I some have waypoints generated from Blender

Here's a csv of the path shown above
sequence_000.zip
It has: Timestamp (100ms interval), x,y,z (absolute) and r, g, b

I managed to use Crazyflie's official python lib's fadeColor for RGB and send_position_setpoint for xyz

With 1 radio, it works pretty well for 1 drone, however with more it flies a bit more wobbly and will not work for more than 4 drones (bandwidth issue)

Currently trying to use more radios (didn't help in first attempt)

I was told Crazyswarm has better communication efficiency, is it the
the pycrazyswarm in particular https://crazyswarm.readthedocs.io/en/latest/api.html
or is it Trajectories the same as the latest official lib? (in terms of efficiency/performance)

To better understand the problem, in what way could Crazyswarm handle 15+ CFs per radio compared to official python lib that could do only 3-4 per radio?

From my limited understanding, instead of blasting coordinates at every 100ms or faster, you pre-program certain trajectories (which we have no fine control over the time)
And activate them on-demand, thus greatly lowering the frequency / amount of communication required.

Or are there some fancy optimisation in the protocol level?

Could the goTo or go_to command also achieve 15+ CF per radio?
send_position_setpoint at 10hz can only go up to 3-4 CF per radio

CMake Issue - nlopt.hpp

[ 80%] Built target mav_trajectory_generation [ 90%] Building CXX object CMakeFiles/genTrajectory.dir/src/genTrajectory.cpp.o In file included from /home/smartlab/Desktop/github_files/uav_trajectories/src/genTrajectory.cpp:8: /home/smartlab/Desktop/github_files/uav_trajectories/mav_trajectory_generation/mav_trajectory_generation/include/mav_trajectory_generation/polynomial_optimization_nonlinear.h:25:10: fatal error: nlopt.hpp: No such file or directory 25 | #include <nlopt.hpp> | ^~~~~~~~~~~ compilation terminated. make[2]: *** [CMakeFiles/genTrajectory.dir/build.make:63: CMakeFiles/genTrajectory.dir/src/genTrajectory.cpp.o] Error 1 make[1]: *** [CMakeFiles/Makefile2:78: CMakeFiles/genTrajectory.dir/all] Error 2 make: *** [Makefile:84: all] Error 2

I am running Ubuntu 20.04.

Generated trajectories have discontinuities

Dear Wolfgang,

Thank you for making this nice tool available - it really makes it a lot easier to generate trajectories compared to using ETH's mav_trajectory_generation directly!

One problem, however, that I quite frequently encounter is that the generated trajectories are really "messed up": sometimes there are discontinuities so that the trajectory looks nothing like one would expect given the waypoints, other times the velocity jumps or goes to very high values. To me it looks like numerical instabilities but since my examples are so simple (see below) and obviously people are successfully using this tool for much more complicated trajectories it might also be that I'm somehow misusing the tool?

Examples for the weird behavior: (all examples use the command ./genTrajectory -i waypoints.csv -o trajectory.csv --v_max 2 --a_max 2.0)

A simple square in the x-y-plane:

0.0, 0.0, 0.0
10.0, 0.0, 0.0
10.0, 10.0, 0.0
0.0, 10.0, 0.0
0.0, 0.0, 0.0

results in:

A straight line with an additional waypoint in the middle:

0.0, 0.0, 0.0
10.0, 0.0, 0.0
20.0, 0.0, 0.0

Here the positions look fine but there are pretty crazy jumps in the velocity:

I also tested with several much more complex real-world trajectories with dozens of waypoints and the behavior was very much the same, sadly.

Do you have any idea what could be the problem here?

Thank you very much for your help!
Michael

Help in generating the trajectory in run time based on getting target points and obstacles around

Hello,

This is a great repository and very helpful. Thank you so much for making it public. I need help in using this repository to generate trajectory in runtime and not pre-programmed flight paths. Basically, I would like to extract information about target points from sensors along with obstacles along the way, and I need help utilizing this repository to generate the trajectory within the code. I tried to study [generate_trajectory.py](https://github.com/whoenig/uav_trajectories/blob/main/scripts/generate_trajectory.py), but I guess it requires waypoints. Also, my code is in Python. Thank you for your guidance.

Trajectory throws an error when two waypoints are the same

I tried the UAV-trajectory generation tool, but whenever I have two waypoints with the same values in succession (like a pause during flight) , I get the error message below. How do I generate trajectories that gives room for pauses during flight?

WARNING: Logging before InitGoogleLogging() is written to STDERR
F0425 11:04:29.853758 22964 polynomial_optimization_linear_impl.h:287] Check failed: segment_time > 0 (0 vs. 0) Segment times need to be greater than zero
*** Check failure stack trace: ***
Aborted (core dumped)

Timed Trajectories for Multi-UAVs

Hi @whoenig

In response to: #1 (comment)

It took me a while to understand, but I realize my approach was wrong

Because even in Blender by itself, I'm finding it hard (very hard) to plan the paths in a timed manner. (maybe due to my lack of experience with Blender)

---

I suppose the current concept is: (Please correct me if I'm wrong)

What is a Trajectory:
Given a sequence of Waypoints (coordinates) that a drone needs to go, what's the smoothest way (high order polynomial) to get to them, while meeting UAV physical limits.
Current commit also assumes velocity of zero at the initial and final Waypoint of the Trajectory.

Between each waypoint is a 7th order polynomial, expressing X, Y, X and Yaw
A Trajectory is a set of 7th order polynomials with a duration of how long each polynomial should be executed. (A Trajectory is a piecewise polynomial)
The sum of each polynomial duration is the total duration of the Trajectory

With multiple UAVs, we have the concept of "Formations".
A set of UAVs with their respective initial Waypoint is the initial Formation. Vice versa, the final Waypoint is the final Formation in a set of UAVs

How fast a drone transforms from a Formation to another can be adjusted using the Timescale of the Trajectory.
If it is desired for all drones to arrive their final Waypoint at the same time, Timescale for each drone may need to be adjusted accordingly.

---

We need a more granular Timescale

Currently there is no fine control over timing, only the duration of a whole Trajectory using Timescale.
(I'm still not really sure how timescale works, seems like it's related to poly4d_stretchtime() in crazyflie firmware)

Good control on timing is useful for collision avoidance

A whole Trajectory can be sped up or slowed down to reduce chances of a collision (however that will affect the arrival time to the final Waypoint

These are the ways I have thought of to gain granular control over Timescale:

1. Split a Trajectory into two Subtrajctories. Slow down the first Subtrajectory, and then speed up the other Subtrajectory. This way we can avoid a collision, while being able to arrive to the final Waypoint at desired time
   But in this repo, the drone will stop between Subtrajectories as velocities are assumed to be zero at final and initial Waypoint
   (we need to fully stop a drone, instead of smoothly slowing down and speeding up)
   To fix this, we may need a velocity vector for initial and final waypoint (not just magnitude).

2. A separate timescale for each polynomial, instead of a global one which we have now.
   From what I understood in source code, this can easily be achieved, and should have rather good results. This way we can slowdown a polynomial and speed up another, to achieve the same total Trajectory duration

3. An advanced method is to introduce a Timescale polynomial, this way, we can smoothly adjust how fast or slow certain parts of a Trajectory should be.
   (I'm really not sure how the math can be done)

That away we'll have nearly full control over the motion, especially in the case of multiple UAVs,

---

TODO:

I'm going to implement num 2 and submit a PR(timescale for each polynomial), as the maths seems to already be implemented, just needs to change the way the Timescale variable is used

Even though it's not recommended to use num 1, it may still be useful to be able to set a velocity vector for initial and final waypoints.

For num 3 (Timescale polynomial), after your suggestions / tips, I may try to implement this given that it's a good approach

Also, there already is a duration variable in the output of this repo, wouldn't it make sense to optimize/solve the duration of each polynomial, to be able to arrive each waypoint at a the desired time? Then it can potentially generate timed trajectories for collision-free points generated from libMultiRobotPlanning

Find a way to project the trajectory into 2-D plot

Hi whoenig, I am using your tool to design the minimum snap trajectory. I was thinking, is there a way to visualizate the trajectory in its 2-D plane. In your python script command: "python3 ../scripts/plot_trajectory.py traj1.csv" can visualization the 3-D trajectory I design. I would loke to get the 2-D plan's visual, like X-Y, X-Z, Y-Z. I can use this 2-D plane ploe to do the comparation. Thanks!

WARNING: Logging before InitGoogleLogging() is written to STDERR

Hi, I'm using the uav_trajectories but I've gotten an error while trying to convert some waypoints into trajectories using https://github.com/whoenig/uav_trajectories#generate-trajectory-given-waypoints , the error is:

WARNING: Logging before InitGoogleLogging() is written to STDERR
E0225 15:11:11.154989  8484 polynomial_optimization_nonlinear_impl.h:234] error while running nlopt: bounds 172 fail 0.1 <= 0.084653 <= 0.169306

Image attached:

Waypoints file attached:
ras.csv

Question about build

Environment：
OS = Ubuntu 20.04
ROS = Noetic
Python = 3.8.10

---

Hello. I have a question about the build.
I execute the following command.

sudo apt install libnlopt-dev libgoogle-glog-dev
git clone --recursive https://github.com/whoenig/uav_trajectories.git
mkdir uav_trajectories/build
cd uav_trajectories/build
cmake .. 
make

but I get the following error and cannot proceed.
Is the uav_trajectory only available for Ubuntu 16.04?
Also, if you don't mind, could you please point out what I am doing wrong?

---

Error message 1
I got this when I first ran make.

Error message 2
I build nlopt.hpp which the cause of the error, separately and add uav_trajectories/mav_trajectory_generation/mav_trajectory_generation/include/mav_trajectory_generation/ directory.
After that, I ran make and the following message came up.