Skip to content

Commit

Permalink
Last commit before implementing kinematic optimization
Browse files Browse the repository at this point in the history
  • Loading branch information
@guillermogilg99
guillermogilg99 committed Dec 3, 2024
1 parent 7d01fea commit bd2b247
Show file tree
Hide file tree
Showing 6 changed files with 208 additions and 67 deletions.
6 changes: 3 additions & 3 deletions include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
80 changes: 80 additions & 0 deletions include/local_planner_optimizer/ceres_constraint_smoothness.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
#ifndef CERES_CONSTRAINTS_SMOOTHNESS
#define CERES_CONSTRAINTS_SMOOTHNESS

#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 SmoothnessFunctor {

public:
SmoothnessFunctor(double weight): weight_(weight) {}

template <typename T>
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
17 changes: 10 additions & 7 deletions include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <typename T>
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:

Expand Down
3 changes: 3 additions & 0 deletions include/utils/CeresOpt.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include "utils/geometry_utils.hpp"
#include "utils/metrics.hpp"
#include <ros/ros.h>
#include <chrono>

#include <heuristic_planners/Vec3i.h>
#include <heuristic_planners/CoordinateList.h>
Expand All @@ -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;
Expand Down
119 changes: 80 additions & 39 deletions src/ROS/local_planner_ros_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@
#include <heuristic_planners/Vec3i.h>
#include <heuristic_planners/CoordinateList.h>

#define USING_PREPLANNING 1



/**
Expand Down Expand Up @@ -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)
{
Expand All @@ -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
Expand All @@ -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<Planners::utils::CoordinateList>(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<bool>(local_path_data["solved"]) ){
Planners::utils::CoordinateList local_path;
local_path = std::get<Planners::utils::CoordinateList>(local_path_data["path"]);

// Trilinear params test
for(const auto& point: local_path){
double x_test = static_cast<double>(point.x) * m_local_grid3d_->m_resolution;
double y_test = static_cast<double>(point.y) * m_local_grid3d_->m_resolution;
double z_test = static_cast<double>(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<Planners::utils::CoordinateList>(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<double, std::milli> local_duration = local_planning_stop - local_planning_start;
printf("TIEMPO TOTAL DEL PLANIFICADOR LOCAL: %.2f ms\n", local_duration.count());

if(std::get<bool>(local_path_data["solved"])){
local_path = std::get<Planners::utils::CoordinateList>(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<double>(point.x) * m_local_grid3d_->m_resolution;
// double y_test = static_cast<double>(point.y) * m_local_grid3d_->m_resolution;
// double z_test = static_cast<double>(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<double, std::milli> 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};
Expand Down
50 changes: 32 additions & 18 deletions src/utils/CeresOpt.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<EquidistanceFunctor, 1, 3, 3>
(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<EquidistanceFunctor, 1, 3, 3, 3>
(new EquidistanceFunctor(weight_equidistance));
ceres::CostFunction* smoothness_function = new AutoDiffCostFunction<SmoothnessFunctor, 1, 3, 3, 3>
(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<PathLengthFunctor, 1, 3, 3>
(new PathLengthFunctor(weight_path_length));;
problem.AddResidualBlock(path_length_function, nullptr, wp_state_vector[i].parameter, wp_state_vector[i+1].parameter);
Expand All @@ -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<double, std::milli> opt_duration = end_opt - start_opt;
printf("TIEMPO DE OPTIMIZACIÓN: %.2f ms\n", opt_duration.count());


// std::cout << summary.FullReport() << "\n";

Expand Down

0 comments on commit bd2b247

Please sign in to comment.