Added velocity to the state vector. Added 3 velocity-related optimiza…

…tion ceres functions. WEIGHTS NOT YET CALIBRATED / UNTESTED

include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp

@@ -24,7 +24,7 @@ using ceres::Problem;
 using ceres::Solve;
 using ceres::Solver;
 
-class ObstacleDistanceCostFunctor : public SizedCostFunction<1, 3>
+class ObstacleDistanceCostFunctor : public SizedCostFunction<1, 6>
     {
     public:
         ObstacleDistanceCostFunctor(Local_Grid3d* grid_, double weight): g_3D(grid_), weight_(weight)
@@ -44,7 +44,7 @@ class ObstacleDistanceCostFunctor : public SizedCostFunction<1, 3>
             {
                 TrilinearParams p = g_3D->computeDistInterpolation(x, y, z);
                 dist_ = p.a0 + p.a1*x + p.a2*y + p.a3*z + p.a4*x*y + p.a5*x*z + p.a6*y*z + p.a7*x*y*z;
-                if(fabsf(dist_) > 0.01)
+                if(dist_ > 0.01)
                     residuals[0] = weight_*1/dist_;
                 else
                     residuals[0] = weight_*100.0;
@@ -53,19 +53,25 @@ class ObstacleDistanceCostFunctor : public SizedCostFunction<1, 3>
                     int cte = -weight_/(dist_*dist_);
                     jacobians[0][0] = cte*(p.a1 + p.a4*y + p.a5*z + p.a7*y*z);
                     jacobians[0][1] = cte*(p.a2 + p.a4*x + p.a6*z + p.a7*x*z);
-                    jacobians[0][2] = cte*(p.a3 + p.a5*x + p.a6*y + p.a7*x*y); 
-                }			
-            }
-            else // This might give an error
-            {
-                std::cout << "Ceres found point outside map" << std::endl;
-                if (jacobians != nullptr && jacobians[0] != nullptr)
+                    jacobians[0][2] = cte*(p.a3 + p.a5*x + p.a6*y + p.a7*x*y);
+                    jacobians[0][3] = 0.0;
+                    jacobians[0][4] = 0.0;
+                    jacobians[0][5] = 0.0; 
+                }
+                }
+                else // This might give an error
                 {
-                    jacobians[0][0] = 0.0;
-                    jacobians[0][1] = 0.0;
-                    jacobians[0][2] = 0.0;
+                    std::cout << "Ceres found point outside map" << std::endl;
+                    if (jacobians != nullptr && jacobians[0] != nullptr)
+                    {
+                        jacobians[0][0] = 0.0;
+                        jacobians[0][1] = 0.0;
+                        jacobians[0][2] = 0.0;
+                        jacobians[0][3] = 0.0;
+                        jacobians[0][4] = 0.0;
+                        jacobians[0][5] = 0.0;
+                    }
                 }
-            }
             return true;
         }
 

include/local_planner_optimizer/ceres_constraint_min_acceleration.hpp

@@ -0,0 +1,85 @@
+#ifndef CERES_CONSTRAINTS_MIN_ACCELERATION
+#define CERES_CONSTRAINTS_MIN_ACCELERATION
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include "utils/ros/ROSInterfaces.hpp"
+#include "utils/SaveDataVariantToFile.hpp"
+#include "utils/misc.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/metrics.hpp"
+#include <ros/ros.h>
+
+#include <heuristic_planners/Vec3i.h>
+#include <heuristic_planners/CoordinateList.h>
+
+#include "Grid3D/local_grid3d.hpp"
+
+#include <ceres/ceres.h>
+
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+
+class MinAccelerationFunctor { // Minimiza la variación de la dirección de la velocidad (que mide la aceleración a módulo de la velocidad constante) ponderado con la distancia del desplazamiento entre WP
+
+public:
+    MinAccelerationFunctor(double weight): weight_(weight) {}
+
+    template <typename T>
+    bool operator()(const T* const stateWP1, const T* const stateWP2, T* residual) const {
+
+        // Compute both vectors and the dot product
+        T vel_start[3] = {stateWP1[3], stateWP1[4], stateWP1[5]};
+        T vel_end[3] = {stateWP2[3], stateWP2[4], stateWP2[5]};
+        T dot_product = (vel_end[0] * vel_start[0]) + (vel_end[1] * vel_start[1]) + (vel_end[2] * vel_start[2]);
+
+        // Compute vector norms
+		T arg1 = (vel_start[0] * vel_start[0]) + (vel_start[1] * vel_start[1]) + (vel_start[2] * vel_start[2]);
+		T arg2 = (vel_end[0] * vel_end[0]) + (vel_end[1] * vel_end[1]) + (vel_end[2] * vel_end[2]);
+		T norm_vector1, norm_vector2, cos_angle;
+
+		if (arg1 < 0.0001 && arg1 > -0.0001)
+			norm_vector1 = T{0.0};
+		else
+			norm_vector1 = ceres::sqrt(arg1);
+
+		if (arg2 < 0.0001 && arg2 > -0.0001)
+			norm_vector2 = T{0.0};
+		else
+			norm_vector2 = ceres::sqrt(arg2);
+
+        // Compute cos(angle)
+        if (norm_vector1 < 0.0001 || norm_vector2 < 0.0001)
+            cos_angle = T{0.0};
+        else
+            cos_angle = dot_product/(norm_vector1 * norm_vector2);
+
+        // Calculate residual
+        T min_expected_cos = T{-1.0};
+        T max_expected_cos = T{1.0};
+        T min_cos_residual = T{20.0};
+        T max_cos_residual = T{0.0}; // We want the angle to be 0 -> Residual is 0 when cos(angle) = 1
+
+        // Calculate position variation
+        T displacement[3] = {stateWP2[0] - stateWP1[0], stateWP2[1] - stateWP1[1], stateWP2[2] - stateWP1[2]};
+        T displacement_mod = ceres::sqrt(displacement[0] * displacement[0] + displacement[1] * displacement[1] + displacement[2] * displacement[2]);
+        if (displacement_mod < 0.0001)
+            displacement_mod = T{0.0001};
+
+        residual[0] = weight_ * (min_cos_residual + ((cos_angle - min_expected_cos) * (max_cos_residual - min_cos_residual) / (max_expected_cos - min_expected_cos))) / displacement_mod;
+
+        return true;
+    }
+
+    double weight_;
+
+private:
+
+
+};
+
+#endif

include/local_planner_optimizer/ceres_constraint_pos_vel_coherence.hpp

@@ -0,0 +1,79 @@
+#ifndef CERES_CONSTRAINTS_POS_VEL_COHERENCE
+#define CERES_CONSTRAINTS_POS_VEL_COHERENCE
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include "utils/ros/ROSInterfaces.hpp"
+#include "utils/SaveDataVariantToFile.hpp"
+#include "utils/misc.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/metrics.hpp"
+#include <ros/ros.h>
+
+#include <heuristic_planners/Vec3i.h>
+#include <heuristic_planners/CoordinateList.h>
+
+#include "Grid3D/local_grid3d.hpp"
+
+#include <ceres/ceres.h>
+
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+
+class PosVelCoherenceFunctor {
+
+public:
+    PosVelCoherenceFunctor(double weight): weight_(weight) {}
+
+    template <typename T>
+    bool operator()(const T* const stateWP1, const T* const stateWP2, T* residual) const {
+
+        // Compute both vectors and the dot product
+        T vel_total[3] = {stateWP1[3] + stateWP2[3], stateWP1[4] + stateWP2[4], stateWP1[5] + stateWP2[5]};
+        T pos_dif[3] = {stateWP2[0] - stateWP1[0], stateWP2[1] - stateWP1[1], stateWP2[2] - stateWP1[2]};
+        T dot_product = (vel_total[0] * pos_dif[0]) + (vel_total[1] * pos_dif[1]) + (vel_total[2] * pos_dif[2]);
+
+        // Compute vector norms
+		T arg1 = (vel_total[0] * vel_total[0]) + (vel_total[1] * vel_total[1]) + (vel_total[2] * vel_total[2]);
+		T arg2 = (pos_dif[0] * pos_dif[0]) + (pos_dif[1] * pos_dif[1]) + (pos_dif[2] * pos_dif[2]);
+		T norm_vector1, norm_vector2, cos_angle;
+
+		if (arg1 < 0.0001 && arg1 > -0.0001)
+			norm_vector1 = T{0.0};
+		else
+			norm_vector1 = ceres::sqrt(arg1);
+
+		if (arg2 < 0.0001 && arg2 > -0.0001)
+			norm_vector2 = T{0.0};
+		else
+			norm_vector2 = ceres::sqrt(arg2);
+
+        // Compute cos(angle)
+        if (norm_vector1 < 0.0001 || norm_vector2 < 0.0001)
+            cos_angle = T{0.0};
+        else
+            cos_angle = dot_product/(norm_vector1 * norm_vector2);
+
+        // Calculate residual
+        T min_expected_cos = T{-1.0};
+        T max_expected_cos = T{1.0};
+        T min_cos_residual = T{20.0};
+        T max_cos_residual = T{0.0}; // We want the angle to be 0 -> Residual is 0 when cos(angle) = 1
+
+        residual[0] = weight_ * (min_cos_residual + ((cos_angle - min_expected_cos) * (max_cos_residual - min_cos_residual) / (max_expected_cos - min_expected_cos)));
+
+        return true;
+    }
+
+    double weight_;
+
+private:
+
+
+};
+
+#endif

include/local_planner_optimizer/ceres_constraint_velocity_change.hpp

@@ -0,0 +1,53 @@
+#ifndef CERES_CONSTRAINTS_VELOCITY_CHANGE
+#define CERES_CONSTRAINTS_VELOCITY_CHANGE
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include "utils/ros/ROSInterfaces.hpp"
+#include "utils/SaveDataVariantToFile.hpp"
+#include "utils/misc.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/metrics.hpp"
+#include <ros/ros.h>
+
+#include <heuristic_planners/Vec3i.h>
+#include <heuristic_planners/CoordinateList.h>
+
+#include "Grid3D/local_grid3d.hpp"
+
+#include <ceres/ceres.h>
+
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+
+class VelocityChangeFunctor {
+
+public:
+    VelocityChangeFunctor(double weight, double desired_vel): weight_(weight), desired_vel_(desired_vel) {}
+
+    template <typename T>
+    bool operator()(const T* const stateWP1, T* residual) const {
+
+        T vx = stateWP1[3];
+        T vy = stateWP1[4];
+        T vz = stateWP1[5];
+
+
+        residual[0] = weight_* (T(desired_vel_) - ceres::sqrt(vx * vx + vy * vy + vz * vz)) * (T(desired_vel_)  - ceres::sqrt(vx * vx + vy * vy + vz * vz));
+
+        return true;
+    }
+
+    double weight_;
+    double desired_vel_;
+
+private:
+
+
+};
+
+#endif

include/utils/CeresOpt.hpp

@@ -21,6 +21,9 @@
 #include "local_planner_optimizer/ceres_constraint_wp_equidistance.hpp"
 #include "local_planner_optimizer/ceres_constraint_path_length.hpp"
 #include "local_planner_optimizer/ceres_constraint_smoothness.hpp"
+#include "local_planner_optimizer/ceres_constraint_velocity_change.hpp"
+#include "local_planner_optimizer/ceres_constraint_min_acceleration.hpp"
+#include "local_planner_optimizer/ceres_constraint_pos_vel_coherence.hpp"
 
 
 using ceres::AutoDiffCostFunction;
@@ -36,11 +39,12 @@ using ceres::LossFunction;
 using ceres::CauchyLoss;
 
 struct parameterBlockWP{
-	double parameter[3];
+	double parameter[6];
 };
 
 namespace Ceresopt{
-    Planners::utils::CoordinateList ceresOptimizer(Planners::utils::CoordinateList initial_path, Local_Grid3d &_grid);
+    std::vector<double> InitVelCalculator(std::vector<parameterBlockWP> wp_state_vector, float total_travel_time, int num_wp, float res);
+    Planners::utils::CoordinateList ceresOptimizer(Planners::utils::CoordinateList initial_path, Local_Grid3d &_grid, float resolution_);
 }
 
 

src/ROS/local_planner_ros_node.cpp

@@ -499,7 +499,7 @@ class HeuristicLocalPlannerROS
 
             // -----------------CERES OPTIMIZATION OF THE PATH-----------------
             auto ceres_start = std::chrono::high_resolution_clock::now();
-            Planners::utils::CoordinateList opt_local_path = Ceresopt::ceresOptimizer(local_path, *m_local_grid3d_);
+            Planners::utils::CoordinateList opt_local_path = Ceresopt::ceresOptimizer(local_path, *m_local_grid3d_, resolution_);
             auto ceres_stop = std::chrono::high_resolution_clock::now();
             std::chrono::duration<double, std::milli> ceres_duration = ceres_stop - ceres_start;
             printf("TIEMPO TOTAL DEL OPTIMIZADOR: %.2f ms\n", ceres_duration.count());