From bd2b247da76db80acc25f12a307b1b6b89228143 Mon Sep 17 00:00:00 2001 From: guillermogilg99 Date: Tue, 3 Dec 2024 15:10:24 +0000 Subject: [PATCH] Last commit before implementing kinematic optimization --- .../ceres_constraint_dist_to_obstacle.hpp | 6 +- .../ceres_constraint_smoothness.hpp | 80 ++++++++++++ .../ceres_constraint_wp_equidistance.hpp | 17 +-- include/utils/CeresOpt.hpp | 3 + src/ROS/local_planner_ros_node.cpp | 119 ++++++++++++------ src/utils/CeresOpt.cpp | 50 +++++--- 6 files changed, 208 insertions(+), 67 deletions(-) create mode 100644 include/local_planner_optimizer/ceres_constraint_smoothness.hpp diff --git a/include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp b/include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp index ac194ed..69f6f42 100644 --- a/include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp +++ b/include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp @@ -45,12 +45,12 @@ 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) - residuals[0] = 1/dist_; + residuals[0] = weight_*1/dist_; else - residuals[0] = 100.0; + residuals[0] = weight_*100.0; if (jacobians != nullptr && jacobians[0] != nullptr) { - int cte = -1/(dist_*dist_); + 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); diff --git a/include/local_planner_optimizer/ceres_constraint_smoothness.hpp b/include/local_planner_optimizer/ceres_constraint_smoothness.hpp new file mode 100644 index 0000000..d67fe9b --- /dev/null +++ b/include/local_planner_optimizer/ceres_constraint_smoothness.hpp @@ -0,0 +1,80 @@ +#ifndef CERES_CONSTRAINTS_SMOOTHNESS +#define CERES_CONSTRAINTS_SMOOTHNESS + +#include +#include +#include +#include "utils/ros/ROSInterfaces.hpp" +#include "utils/SaveDataVariantToFile.hpp" +#include "utils/misc.hpp" +#include "utils/geometry_utils.hpp" +#include "utils/metrics.hpp" +#include + +#include +#include + +#include "Grid3D/local_grid3d.hpp" + +#include + +using ceres::AutoDiffCostFunction; +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solve; +using ceres::Solver; + +class SmoothnessFunctor { + +public: + SmoothnessFunctor(double weight): weight_(weight) {} + + template + bool operator()(const T* const stateWP1, const T* const stateWP2, const T* const stateWP3, T* residual) const { + + // Compute both vectors and the dot product + T vecAB[3] = {stateWP2[0]-stateWP1[0], stateWP2[1]-stateWP1[1], stateWP2[2]-stateWP1[2]}; + T vecBC[3] = {stateWP3[0]-stateWP2[0], stateWP3[1]-stateWP2[1], stateWP3[2]-stateWP2[2]}; + T dot_product = (vecBC[0] * vecAB[0]) + (vecBC[1] * vecAB[1]) + (vecBC[2] * vecAB[2]); + + // Compute vector norms + T arg1 = (vecAB[0] * vecAB[0]) + (vecAB[1] * vecAB[1]) + (vecAB[2] * vecAB[2]); + T arg2 = (vecBC[0] * vecBC[0]) + (vecBC[1] * vecBC[1]) + (vecBC[2] * vecBC[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 \ No newline at end of file diff --git a/include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp b/include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp index 9d57301..ef1f140 100644 --- a/include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp +++ b/include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp @@ -27,21 +27,24 @@ using ceres::Solver; class EquidistanceFunctor { public: - EquidistanceFunctor(double d_target, double weight): d_target_(d_target), weight_(weight) {} + EquidistanceFunctor(double weight): weight_(weight) {} template - bool operator()(const T* const stateWP1, const T* const stateWP2, T* residual) const { + bool operator()(const T* const stateWP1, const T* const stateWP2, const T* const stateWP3, T* residual) const { - T dx = stateWP2[0] - stateWP1[0]; - T dy = stateWP2[1] - stateWP1[1]; - T dz = stateWP2[2] - stateWP1[2]; + T dx1 = stateWP2[0] - stateWP1[0]; + T dy1 = stateWP2[1] - stateWP1[1]; + T dz1 = stateWP2[2] - stateWP1[2]; + T dx2 = stateWP3[0] - stateWP2[0]; + T dy2 = stateWP3[1] - stateWP2[1]; + T dz2 = stateWP3[2] - stateWP2[2]; - residual[0] = weight_* (dx * dx + dy * dy + dz * dz - T(d_target_)); + residual[0] = weight_* ((dx2 * dx2 + dy2 * dy2 + dz2 * dz2) - (dx1 * dx1 + dy1 * dy1 + dz1 * dz1)); return true; } - double d_target_, weight_; + double weight_; private: diff --git a/include/utils/CeresOpt.hpp b/include/utils/CeresOpt.hpp index 755a72a..deefb18 100644 --- a/include/utils/CeresOpt.hpp +++ b/include/utils/CeresOpt.hpp @@ -8,6 +8,7 @@ #include "utils/geometry_utils.hpp" #include "utils/metrics.hpp" #include +#include #include #include @@ -19,6 +20,8 @@ #include "local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp" #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" + using ceres::AutoDiffCostFunction; using ceres::NumericDiffCostFunction; diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp index a08cbd0..a9f78cc 100755 --- a/src/ROS/local_planner_ros_node.cpp +++ b/src/ROS/local_planner_ros_node.cpp @@ -41,6 +41,8 @@ #include #include +#define USING_PREPLANNING 1 + /** @@ -392,9 +394,19 @@ class HeuristicLocalPlannerROS // std::cout << "Closest waypoint: " << global_path_local[closest_index] << std::endl; // 2.3 - Find furthest next waypoint that is still inside the local map (the drone will treat this waypoint as the local goal + // 2.3b - Build a vector with the local part of the global path (that will be used as the first iteration of the optimizer when NOT using preplanning) bool points_in_range = true; int it = closest_index; Planners::utils::Vec3i local_goal; + Planners::utils::CoordinateList global_path_local_section; + if(USING_PREPLANNING==0){ + Planners::utils::Vec3i auxnewpoint; + auxnewpoint.x = drone_local_x; + auxnewpoint.y = drone_local_y; + auxnewpoint.z = drone_local_z; + global_path_local_section.push_back(auxnewpoint); + } + while (it <= (global_path_local.size() - 1) && points_in_range) { @@ -405,6 +417,13 @@ class HeuristicLocalPlannerROS local_goal.x = global_path_local[it].x; local_goal.y = global_path_local[it].y; local_goal.z = global_path_local[it].z; + if(USING_PREPLANNING==0){ + Planners::utils::Vec3i auxnewpoint; + auxnewpoint.x = global_path_local[it].x; + auxnewpoint.y = global_path_local[it].y; + auxnewpoint.z = global_path_local[it].z; + global_path_local_section.push_back(auxnewpoint); + } it++; } else @@ -419,52 +438,74 @@ class HeuristicLocalPlannerROS // std::cout << "Local goal found: " << local_goal << std::endl; // 3 - Check if local goal accessible. If not, find closest point - - // 4 - Use path planner to find local waypoints - Planners::utils::Vec3i discrete_start, discrete_goal; - discrete_start.x = drone_local_x; - discrete_start.y = drone_local_y; - discrete_start.z = drone_local_z; - discrete_goal.x = local_goal.x; - discrete_goal.y = local_goal.y; - discrete_goal.z = local_goal.z; - - std::cout << "Discrete local start: " << discrete_start << " | Discrete local goal: " << discrete_goal << std::endl; - - if( algorithm_->detectCollision(discrete_start) ){ - std::cout << discrete_start << ": Start not valid" << std::endl; - } - if( algorithm_->detectCollision(discrete_goal) ){ - std::cout << discrete_goal << ": Goal not valid" << std::endl; - } - //std::cout << "LOCAL PATH CALCULATED SUCCESSFULLY" << std::endl; - //Planners::utils::CoordinateList local_path = std::get(local_path_data["path"]); - //std::cout << "Local path: " << local_path << std::endl; + // 4 - Use path planner to find local waypoints (if planning before the Ceres optimizer) + int planning_solved = 0; + Planners::utils::CoordinateList local_path; + if(USING_PREPLANNING == 1){ + Planners::utils::Vec3i discrete_start, discrete_goal; + discrete_start.x = drone_local_x; + discrete_start.y = drone_local_y; + discrete_start.z = drone_local_z; + discrete_goal.x = local_goal.x; + discrete_goal.y = local_goal.y; + discrete_goal.z = local_goal.z; + + std::cout << "Discrete local start: " << discrete_start << " | Discrete local goal: " << discrete_goal << std::endl; + + if( algorithm_->detectCollision(discrete_start) ){ + std::cout << discrete_start << ": Start not valid" << std::endl; + } + if( algorithm_->detectCollision(discrete_goal) ){ + std::cout << discrete_goal << ": Goal not valid" << std::endl; + } - - auto local_path_data = algorithm_->findPath(discrete_start, discrete_goal, loaded_sdf_); - if( std::get(local_path_data["solved"]) ){ - Planners::utils::CoordinateList local_path; - local_path = std::get(local_path_data["path"]); - - // Trilinear params test - for(const auto& point: local_path){ - double x_test = static_cast(point.x) * m_local_grid3d_->m_resolution; - double y_test = static_cast(point.y) * m_local_grid3d_->m_resolution; - double z_test = static_cast(point.z) * m_local_grid3d_->m_resolution; - TrilinearParams p_test = m_local_grid3d_->computeDistInterpolation(x_test, y_test, z_test); - std::cout << "Params for x = " << x_test/m_local_grid3d_->m_resolution << ", y = " << y_test/m_local_grid3d_->m_resolution << ", z = " << z_test/m_local_grid3d_->m_resolution << ": a0=" << p_test.a0 << ", a1=" << p_test.a1 << ", a2=" << p_test.a2 << ", a3=" << p_test.a3 << ", a4=" << p_test.a4 << ", a5=" << p_test.a5 << ", a6=" << p_test.a6 << ", a7=" << p_test.a7 << std::endl; - - float test_dist = p_test.a0 + p_test.a1*x_test + p_test.a2*y_test + p_test.a3*z_test + p_test.a4*x_test*y_test + p_test.a5*x_test*z_test + p_test.a6*y_test*z_test + p_test.a7*x_test*y_test*z_test; - uint64_t test_index = point.x + point.y*m_local_grid3d_->m_gridStepY + point.z*m_local_grid3d_->m_gridStepZ; - std::cout << "Interpolated distance : " << test_dist << "Real distance : " << m_local_grid3d_->m_grid[test_index].dist << std::endl; + //std::cout << "LOCAL PATH CALCULATED SUCCESSFULLY" << std::endl; + //Planners::utils::CoordinateList local_path = std::get(local_path_data["path"]); + //std::cout << "Local path: " << local_path << std::endl; + + auto local_planning_start = std::chrono::high_resolution_clock::now(); + auto local_path_data = algorithm_->findPath(discrete_start, discrete_goal, loaded_sdf_); + auto local_planning_stop = std::chrono::high_resolution_clock::now(); + std::chrono::duration local_duration = local_planning_stop - local_planning_start; + printf("TIEMPO TOTAL DEL PLANIFICADOR LOCAL: %.2f ms\n", local_duration.count()); + + if(std::get(local_path_data["solved"])){ + local_path = std::get(local_path_data["path"]); + planning_solved = 1; } + } + + + // 5 - If solved (or if not planning before optimization), optimize the path + + if(planning_solved == 1 || USING_PREPLANNING == 0){ + + if(USING_PREPLANNING == 0) + local_path = global_path_local_section; + + // // Trilinear params test + // for(const auto& point: local_path){ + // double x_test = static_cast(point.x) * m_local_grid3d_->m_resolution; + // double y_test = static_cast(point.y) * m_local_grid3d_->m_resolution; + // double z_test = static_cast(point.z) * m_local_grid3d_->m_resolution; + // TrilinearParams p_test = m_local_grid3d_->computeDistInterpolation(x_test, y_test, z_test); + // std::cout << "Params for x = " << x_test/m_local_grid3d_->m_resolution << ", y = " << y_test/m_local_grid3d_->m_resolution << ", z = " << z_test/m_local_grid3d_->m_resolution << ": a0=" << p_test.a0 << ", a1=" << p_test.a1 << ", a2=" << p_test.a2 << ", a3=" << p_test.a3 << ", a4=" << p_test.a4 << ", a5=" << p_test.a5 << ", a6=" << p_test.a6 << ", a7=" << p_test.a7 << std::endl; + + // float test_dist = p_test.a0 + p_test.a1*x_test + p_test.a2*y_test + p_test.a3*z_test + p_test.a4*x_test*y_test + p_test.a5*x_test*z_test + p_test.a6*y_test*z_test + p_test.a7*x_test*y_test*z_test; + // uint64_t test_index = point.x + point.y*m_local_grid3d_->m_gridStepY + point.z*m_local_grid3d_->m_gridStepZ; + // std::cout << "Interpolated distance : " << test_dist << "Real distance : " << m_local_grid3d_->m_grid[test_index].dist << std::endl; + // } // -----------------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_); + auto ceres_stop = std::chrono::high_resolution_clock::now(); + std::chrono::duration ceres_duration = ceres_stop - ceres_start; + printf("TIEMPO TOTAL DEL OPTIMIZADOR: %.2f ms\n", ceres_duration.count()); + std::cout << "Value of dist: " << m_local_grid3d_->m_grid[(m_local_grid3d_->m_gridSize-1)/2].dist << std::endl; - + //Convert the local path to GLOBAL COORDINATES and push them into the markers Planners::utils::Vec3i local_origin = {origen_local_x, origen_local_y, origen_local_z}; diff --git a/src/utils/CeresOpt.cpp b/src/utils/CeresOpt.cpp index f84b6e9..b10df8d 100644 --- a/src/utils/CeresOpt.cpp +++ b/src/utils/CeresOpt.cpp @@ -31,34 +31,41 @@ namespace Ceresopt // Declare Ceres optimization problem ceres::Problem problem; - // Equidistance function target distance squared + // // Equidistance function target distance squared - double dist_target = 0; - for (int i=0; i < (num_wp-1); i++){ - double tdx = wp_state_vector[i+1].parameter[0] - wp_state_vector[i].parameter[0]; - double tdy = wp_state_vector[i+1].parameter[1] - wp_state_vector[i].parameter[1]; - double tdz = wp_state_vector[i+1].parameter[2] - wp_state_vector[i].parameter[2]; + // double dist_target = 0; + // for (int i=0; i < (num_wp-1); i++){ + // double tdx = wp_state_vector[i+1].parameter[0] - wp_state_vector[i].parameter[0]; + // double tdy = wp_state_vector[i+1].parameter[1] - wp_state_vector[i].parameter[1]; + // double tdz = wp_state_vector[i+1].parameter[2] - wp_state_vector[i].parameter[2]; - dist_target += tdx * tdx + tdy * tdy + tdz * tdz; - } - dist_target = dist_target/(num_wp-1); + // dist_target += tdx * tdx + tdy * tdy + tdz * tdz; + // } + // dist_target = dist_target/(num_wp-1); // Cost function weights double weight_equidistance = 1.0; double weight_path_length = 1.0; - double weight_esdf = 1.0; + double weight_esdf = 10.0; + double weight_smoothness = 1.0; // Define cost functions - for (int i = 0; i < wp_state_vector.size() - 1; i++){ - - // 1 - Equidistance cost function - ceres::CostFunction* equidistance_function = new AutoDiffCostFunction - (new EquidistanceFunctor(dist_target, weight_equidistance)); - problem.AddResidualBlock(equidistance_function, nullptr, wp_state_vector[i].parameter, wp_state_vector[i+1].parameter); + // 1 - Equidistance cost function + Smoothness cost function + for (int i = 0; i < wp_state_vector.size() - 2; i++) + { + ceres::CostFunction* equidistance_function = new AutoDiffCostFunction + (new EquidistanceFunctor(weight_equidistance)); + ceres::CostFunction* smoothness_function = new AutoDiffCostFunction + (new SmoothnessFunctor(weight_smoothness)); + problem.AddResidualBlock(equidistance_function, nullptr, wp_state_vector[i].parameter, wp_state_vector[i+1].parameter, wp_state_vector[i+2].parameter); + problem.AddResidualBlock(smoothness_function, nullptr, wp_state_vector[i].parameter, wp_state_vector[i+1].parameter, wp_state_vector[i+2].parameter); + } - // 2 - Path length cost function + // 2 - Path length cost function + for (int i = 0; i < wp_state_vector.size() - 1; i++) + { ceres::CostFunction* path_length_function = new AutoDiffCostFunction (new PathLengthFunctor(weight_path_length));; problem.AddResidualBlock(path_length_function, nullptr, wp_state_vector[i].parameter, wp_state_vector[i+1].parameter); @@ -82,9 +89,16 @@ namespace Ceresopt options.linear_solver_type = ceres::DENSE_QR; options.minimizer_progress_to_stdout = true; options.max_num_iterations = 100; - + options.num_threads = 12; + ceres::Solver::Summary summary; + + auto start_opt = std::chrono::high_resolution_clock::now(); ceres::Solve(options, &problem, &summary); + auto end_opt = std::chrono::high_resolution_clock::now(); + std::chrono::duration opt_duration = end_opt - start_opt; + printf("TIEMPO DE OPTIMIZACIÓN: %.2f ms\n", opt_duration.count()); + // std::cout << summary.FullReport() << "\n";