Merge develop into master

.github/workflows/ci-tests.yml

@@ -69,8 +69,8 @@ jobs:
         comment-on-alert: true
         # Mention @rhysd in the commit comment
         alert-comment-cc-users: '@bradygm'
-        alert-threshold: 200%
-        # comment-always: true
+        alert-threshold: 150%
+        comment-always: true
     # Upload the updated cache file for the next job by actions/cache
 
     - name: Run unit tests

README.md

@@ -2,7 +2,7 @@
 # Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification 
 
 This repository contains code for the paper
-**<a href="https://arxiv.org/abs/2306.11845">"Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification"</a>**  by *<a href="https://sagars2.com">Sagar Sachdev\*</a>, <a href="https://bradymoon.com">Brady Moon\*</a>, <a href="https://theairlab.org/team/junbiny/">Junbin Yuan</a>, and <a href="https://www.ri.cmu.edu/ri-faculty/sebastian-scherer/">Sebastian Scherer</a> (\* equal contribution)*.
+**<a href="https://arxiv.org/abs/2306.11845">"Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification"</a>**  by *<a href="https://bradymoon.com">Brady Moon\*</a>, <a href="https://sagars2.com">Sagar Sachdev\*</a>, <a href="https://theairlab.org/team/junbiny/">Junbin Yuan</a>, and <a href="https://www.ri.cmu.edu/ri-faculty/sebastian-scherer/">Sebastian Scherer</a> (\* equal contribution)*.
 
 This codebase includes both a solver for trochoidal paths when there is wind as well as also solving Dubins paths when there is no wind. The Dubins path solutions use the work <a href="http://dx.doi.org/10.1016/S0921-8890(00)00127-5">"Classification of the Dubins set"</a> as well as the correction proposed in the work  <a href="https://www.research-collection.ethz.ch/handle/20.500.11850/615185">"Circling Back: Dubins set Classification Revisited."</a>
 
@@ -11,27 +11,7 @@ This codebase includes both a solver for trochoidal paths when there is wind as
 </p>
 
 ## Brief Overview
- Time-optimal path planning in high winds for a
-turning rate constrained UAV is a challenging problem to solve
-and is important for deployment and field operations. Previous
-works have used trochoidal path segments, which consist of
-straight and maximum-rate turn segments, as optimal extremal
-paths in uniform wind conditions. Current methods iterate
-over all candidate trochoidal trajectory types and choose the
-time-optimal one; however, this can be computationally slow.
-As such, a method to narrow down the candidate trochoidal
-trajectory types before computing the trajectories would reduce
-the computation time. We thus introduce a geometric
-approach to reduce the candidate trochoidal trajectory types by
-framing the problem in the air-relative frame and bounding the
-solution within a subset of candidate trajectories. This method
-reduces overall computation by around 37% compared to pre-
-existing methods in Bang-Straight-Bang trajectories, freeing
-up computation for other onboard processes and can lead to
-significant total computational reductions when solving many
-trochoidal paths. When used within the framework of a global
-path planner, faster state expansions help find solutions faster or
-compute higher-quality paths.
+Time-optimal path planning in high winds for a turning-rate constrained UAV is a challenging problem to solve and is important for deployment and field operations. Previous works have used trochoidal path segments comprising straight and maximum-rate turn segments, as optimal extremal paths in uniform wind conditions. Current methods iterate over all candidate trochoidal trajectory types and select the one that is time-optimal; however, this exhaustive search can be computationally slow. In this paper, we introduce a method to decrease the computation time. This is achieved by reducing the number of candidate trochoidal trajectory types by framing the problem in the air-relative frame and bounding the solution within a subset of candidate trajectories. Our method reduces overall computation by 37.4% compared to pre-existing methods in Bang-Straight-Bang trajectories, freeing up computation for other onboard processes and can lead to significant total computational reductions when solving many trochoidal paths. When used within the framework of a global path planner, faster state expansions help find solutions faster or compute higher-quality paths. We also release our open-source codebase as a C++ package.
 
 
 ## Prerequisites
@@ -107,9 +87,9 @@ bool valid = trochoids::get_trochoid_path(start_state, goal_state, trochoid_path
 ## Citation
 If you find this work useful, please cite our paper:
 ```
-@article{sachdev2023timeoptimal,
+@article{moon2023timeoptimal,
         title={Time-Optimal Path Planning in a Constant Wind for Uncrewed Aerial Vehicles using Dubins Set Classification}, 
-        author={Sagar Sachdev and Brady Moon and Junbin Yuan and Sebastian Scherer},
+        author={Brady Moon and Sagar Sachdev and Junbin Yuan and Sebastian Scherer},
         year={2023},
         eprint={2306.11845},
         archivePrefix={arXiv},

include/trochoids/DubinsStateSpace.h

@@ -38,7 +38,7 @@
 #ifndef TROCHOIDS_DUBINSSTATESPACE_H
 #define TROCHOIDS_DUBINSSTATESPACE_H
 
-#include <boost/math/constants/constants.hpp>
+#include <cassert>
 #include <iostream>
 #include <math.h>
 #include <vector>

src/DubinsStateSpace.cpp

@@ -41,7 +41,7 @@
 
 namespace Dubins
 {
-    const double twopi = 2. * boost::math::constants::pi<double>();
+    const double twopi = 2. * M_PI;
     const double DUBINS_EPS = 1e-6;
     const double DUBINS_ZERO = -1e-7;
 
@@ -236,15 +236,22 @@ int Dubins::find_quadrant(double angle)
         return 4;  // fourth quadrant
 }
 
+/**
+ * @brief Uses the Dubins set classification for solving a Dubins path
+ *
+ * @param state1 DubinsState of the starting pose
+ * @param state2 DubinsState of the ending pose
+ * @return DubinsPath
+ */
 Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(const DubinsState state1,
                                                                               const DubinsState state2) const
 {
     double x1 = state1.x, y1 = state1.y, th1 = state1.theta;
     double x2 = state2.x, y2 = state2.y, th2 = state2.theta;
     double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx);
     double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
-    // if (d > (sqrt(4 - pow((abs(cos(alpha)) + abs(cos(beta))),2)) + abs(sin(alpha)) + abs(sin(beta))))
-    if (d > 4)
+    if (d > (std::abs(std::sin(alpha)) + std::abs(std::sin(beta)) +
+                std::sqrt(4 - std::pow(std::cos(alpha) + std::cos(beta), 2))))
     {
         int init_quadrant = find_quadrant(alpha);
         int final_quadrant = find_quadrant(beta);
@@ -271,10 +278,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
             DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
             DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta);
             double S13 = dubinsrsr.length_[0] - M_PI;
-            double S12 = dubinsrsr.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[2] - M_PI);
 
             if (S13 <= 0)
             {
+                double S12 = dubinsrsr.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[2] - M_PI);
                 if (S12 > 0)
                 {
                     return dubinsrsl;
@@ -320,10 +327,10 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
             DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
             DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta);
             double S31 = dubinslsl.length_[2] - M_PI;
-            double S21 = dubinslsl.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[0] - M_PI);
 
             if (S31 <= 0)
             {
+                double S21 = dubinslsl.length_[1] - dubinsrsl.length_[1] - 2*(dubinsrsl.length_[0] - M_PI);
                 if (S21 > 0)
                 {
                     return dubinsrsl;
@@ -355,13 +362,9 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
             {
                 return dubinsrsr;
             }
-            else if (S24 > 0)
-            {
-                return dubinsRSL(d, alpha, beta);
-            }
             else
             {
-                return Dubins::dubins(d, alpha, beta);
+                return dubinsRSL(d, alpha, beta);
             }
         }
         else if (init_quadrant == 3 && final_quadrant == 1)
@@ -373,24 +376,20 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
             {
                 return dubinslsl;
             }
-            else if (S31 > 0)
-            {
-                return dubinsLSR(d, alpha, beta);
-            }
             else
             {
-                return Dubins::dubins(d, alpha, beta);
+                return dubinsLSR(d, alpha, beta);
             }
         }
         else if (init_quadrant == 3 && final_quadrant == 4)
         {
             DubinsPath dubinslsr = dubinsLSR(d, alpha, beta);
             DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
             double S24 = dubinsrsr.length_[2] - M_PI;
-            double S34 = dubinsrsr.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[0] - M_PI);
 
             if (S24 <= 0)
             {
+                double S34 = dubinsrsr.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[0] - M_PI);
                 if (S34 > 0)
                 {
                     return dubinslsr;
@@ -422,24 +421,20 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
             {
                 return dubinslsl;
             }
-            else if (S42 > 0)
-            {
-                return dubinsRSL(d, alpha, beta);
-            }
             else
             {
-                return Dubins::dubins(d, alpha, beta);
+                return dubinsRSL(d, alpha, beta);
             }
         }
         else if (init_quadrant == 4 && final_quadrant == 3)
         {
             DubinsPath dubinslsr = dubinsLSR(d, alpha, beta);
             DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
             double S42 = dubinslsl.length_[0] - M_PI;
-            double S43 = dubinslsl.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[2] - M_PI);
 
             if (S42 <= 0)
             {
+                double S43 = dubinslsl.length_[1] - dubinslsr.length_[1] - 2*(dubinslsr.length_[2] - M_PI);
                 if (S43 > 0)
                 {
                     return dubinslsr;
@@ -485,18 +480,14 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
         }
         else if (init_quadrant == 2 && final_quadrant == 2)
         {
-            DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
             DubinsPath dubinsrsl = dubinsRSL(d, alpha, beta);
-            DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
-
-            double plsl = dubinslsl.length_[1];
             double prsl = dubinsrsl.length_[1];
-            double trsl = dubinsrsl.length_[0];
-            double prsr = dubinsrsr.length_[1];
-            double qrsl = dubinsrsl.length_[2];
-
+
             if (alpha > beta)
             {
+                DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
+                double trsl = dubinsrsl.length_[0];
+                double plsl = dubinslsl.length_[1];
                 double S122 = plsl - prsl - 2*(trsl - M_PI);
                 if (S122 < 0)
                 {
@@ -511,8 +502,11 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
                     return Dubins::dubins(d, alpha, beta);
                 }
             }
-            else if (alpha < beta)
+            else
             {
+                DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
+                double prsr = dubinsrsr.length_[1];
+                double qrsl = dubinsrsl.length_[2];
                 double S222 = prsr - prsl - 2*(qrsl - M_PI);
                 if (S222 < 0)
                 {
@@ -527,24 +521,17 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
                     return Dubins::dubins(d, alpha, beta);
                 }
             }
-            else
-            {
-                return Dubins::dubins(d, alpha, beta);
-            }
         }
         else if (init_quadrant == 3 && final_quadrant == 3)
         {
-            DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
             DubinsPath dubinslsr = dubinsLSR(d, alpha, beta);
-            DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
-
-            double prsr = dubinsrsr.length_[1];
             double plsr = dubinslsr.length_[1];
-            double tlsr = dubinslsr.length_[0];
-            double plsl = dubinslsl.length_[1];
-            double qlsr = dubinslsr.length_[2];
+
             if (alpha < beta)
             {
+                DubinsPath dubinsrsr = dubinsRSR(d, alpha, beta);
+                double prsr = dubinsrsr.length_[1];
+                double tlsr = dubinslsr.length_[0];
                 double S133 = prsr - plsr - 2*(tlsr - M_PI);
                 if (S133 < 0)
                 {
@@ -559,8 +546,11 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
                     return Dubins::dubins(d, alpha, beta);
                 }
             }
-            else if (alpha > beta)
+            else
             {
+                DubinsPath dubinslsl = dubinsLSL(d, alpha, beta);
+                double plsl = dubinslsl.length_[1];
+                double qlsr = dubinslsr.length_[2];
                 double S233 = plsl - plsr - 2*(qlsr - M_PI);
                 if (S233 < 0)
                 {
@@ -575,10 +565,6 @@ Dubins::DubinsStateSpace::DubinsPath Dubins::DubinsStateSpace::dubins_matrix(con
                     return Dubins::dubins(d, alpha, beta);
                 }
             }
-            else
-            {
-                return Dubins::dubins(d, alpha, beta);
-            }
         }
         else if (init_quadrant == 4 && final_quadrant == 1)
         {

src/trochoids.cpp

@@ -371,7 +371,7 @@ std::vector<std::pair<double, double>> trochoids::Trochoid::trochoid_classificat
             }
         }
         // Compute the quadrant between the decision points
-        if (std::isnan(d_between_angle))  // No decision points
+        if (std::isnan(d_between_angle))  // No decision points or at last decision point
         {
             bool within_four_r = check_within_four_r(decision_points[decision_points.size()-1], x0, y0, xf, yf);
             if (within_four_r)

test/all_benchmarks.cpp

@@ -445,5 +445,100 @@ static void BM_GetTrochoid_with_classification(benchmark::State& state) {
 }
 BENCHMARK(BM_GetTrochoid_with_classification);
 
+static void BM_Dubins_Random_Exhaustive(benchmark::State& state) {
+    std::random_device rd;
+    std::mt19937 gen = std::mt19937(rd());
+    std::uniform_real_distribution<> disRange(-1000, 1000);
+    std::uniform_real_distribution<> disPhi(0.0, 2.0 * M_PI);
+
+    double max_kappa = 0.01;
+
+    Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);
+
+    for (auto _ : state){
+        benchmark::DoNotOptimize(dubins_path_object.dubins({disRange(gen), disRange(gen), disPhi(gen)}, {disRange(gen), disRange(gen), disPhi(gen)}));
+        benchmark::ClobberMemory();
+    }
+
+}
+BENCHMARK(BM_Dubins_Random_Exhaustive);
+
+static void BM_Dubins_Random_Classification(benchmark::State& state) {
+    std::random_device rd;
+    std::mt19937 gen = std::mt19937(rd());
+    std::uniform_real_distribution<> disRange(-1000, 1000);
+    std::uniform_real_distribution<> disPhi(0.0, 2.0 * M_PI);
+
+    double max_kappa = 0.01;
+
+    Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);
+
+    for (auto _ : state){
+        benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({disRange(gen), disRange(gen), disPhi(gen)}, {disRange(gen), disRange(gen), disPhi(gen)}));
+        benchmark::ClobberMemory();
+    }
+
+}
+BENCHMARK(BM_Dubins_Random_Classification);
+
+static void BM_Dubins_a12(benchmark::State& state) {
+    double max_kappa = 0.01;
+
+    Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);
+
+    for (auto _ : state){
+        benchmark::DoNotOptimize(dubins_path_object.dubins({0, 0, 0.5}, {800, 0, 2.4}));
+        benchmark::ClobberMemory();
+    }
+
+}
+BENCHMARK(BM_Dubins_a12);
+
+static void BM_Dubins_a12_random(benchmark::State& state) {
+    std::random_device rd;
+    std::mt19937 gen = std::mt19937(rd());
+    std::uniform_real_distribution<> disPhi1(0.0, M_PI_2);
+    std::uniform_real_distribution<> disPhi2(M_PI_2, M_PI);
+    double max_kappa = 0.01;
+
+    Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);
+
+    for (auto _ : state){
+        benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({0, 0, disPhi1(gen)}, {800, 0, disPhi2(gen)}));
+        benchmark::ClobberMemory();
+    }
+
+}
+BENCHMARK(BM_Dubins_a12_random);
+
+static void BM_Dubins_a22(benchmark::State& state) {
+    double max_kappa = 0.01;
+
+    Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);
+
+    for (auto _ : state){
+        benchmark::DoNotOptimize(dubins_path_object.dubins({0, 0, 2.8}, {800, 0, 2.4}));
+        benchmark::ClobberMemory();
+    }
+
+}
+BENCHMARK(BM_Dubins_a22);
+
+static void BM_Dubins_a22_random(benchmark::State& state) {
+    std::random_device rd;
+    std::mt19937 gen = std::mt19937(rd());
+    std::uniform_real_distribution<> disPhi2(M_PI_2, M_PI);
+    double max_kappa = 0.01;
+
+    Dubins::DubinsStateSpace dubins_path_object(1/max_kappa);
+
+    for (auto _ : state){
+        benchmark::DoNotOptimize(dubins_path_object.dubins_matrix({0, 0, disPhi2(gen)}, {800, 0, disPhi2(gen)}));
+        benchmark::ClobberMemory();
+    }
+
+}
+BENCHMARK(BM_Dubins_a22_random);
+
 
 BENCHMARK_MAIN();