add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,207 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__OPTIONS_HPP_
|
||||
#define DEPRECATED__OPTIONS_HPP_
|
||||
|
||||
#include <string>
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct nav2_smac_planner::SmootherParams
|
||||
* @brief Parameters for the smoother cost function
|
||||
*/
|
||||
struct SmootherParams
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::SmootherParams
|
||||
*/
|
||||
SmootherParams()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get params from ROS parameter
|
||||
* @param node_ Ptr to node
|
||||
* @param name Name of plugin
|
||||
*/
|
||||
void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
|
||||
{
|
||||
std::string local_name = name + std::string(".smoother.smoother.");
|
||||
|
||||
// Smoother params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_curve", rclcpp::ParameterValue(1.5));
|
||||
node->get_parameter(local_name + "w_curve", curvature_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_cost", rclcpp::ParameterValue(0.0));
|
||||
node->get_parameter(local_name + "w_cost", costmap_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_dist", rclcpp::ParameterValue(0.0));
|
||||
node->get_parameter(local_name + "w_dist", distance_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0));
|
||||
node->get_parameter(local_name + "w_smooth", smooth_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "cost_scaling_factor", rclcpp::ParameterValue(10.0));
|
||||
node->get_parameter(local_name + "cost_scaling_factor", costmap_factor);
|
||||
}
|
||||
|
||||
double smooth_weight{0.0};
|
||||
double costmap_weight{0.0};
|
||||
double distance_weight{0.0};
|
||||
double curvature_weight{0.0};
|
||||
double max_curvature{0.0};
|
||||
double costmap_factor{0.0};
|
||||
double max_time;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct nav2_smac_planner::OptimizerParams
|
||||
* @brief Parameters for the ceres optimizer
|
||||
*/
|
||||
struct OptimizerParams
|
||||
{
|
||||
OptimizerParams()
|
||||
: debug(false),
|
||||
max_iterations(50),
|
||||
max_time(1e4),
|
||||
param_tol(1e-8),
|
||||
fn_tol(1e-6),
|
||||
gradient_tol(1e-10)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @struct AdvancedParams
|
||||
* @brief Advanced parameters for the ceres optimizer
|
||||
*/
|
||||
struct AdvancedParams
|
||||
{
|
||||
AdvancedParams()
|
||||
: min_line_search_step_size(1e-9),
|
||||
max_num_line_search_step_size_iterations(20),
|
||||
line_search_sufficient_function_decrease(1e-4),
|
||||
max_num_line_search_direction_restarts(20),
|
||||
max_line_search_step_contraction(1e-3),
|
||||
min_line_search_step_contraction(0.6),
|
||||
line_search_sufficient_curvature_decrease(0.9),
|
||||
max_line_search_step_expansion(10)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get advanced params from ROS parameter
|
||||
* @param node_ Ptr to node
|
||||
* @param name Name of plugin
|
||||
*/
|
||||
void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
|
||||
{
|
||||
std::string local_name = name + std::string(".smoother.optimizer.advanced.");
|
||||
|
||||
// Optimizer advanced params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "min_line_search_step_size",
|
||||
rclcpp::ParameterValue(1e-20));
|
||||
node->get_parameter(
|
||||
local_name + "min_line_search_step_size",
|
||||
min_line_search_step_size);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_num_line_search_step_size_iterations",
|
||||
rclcpp::ParameterValue(50));
|
||||
node->get_parameter(
|
||||
local_name + "max_num_line_search_step_size_iterations",
|
||||
max_num_line_search_step_size_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "line_search_sufficient_function_decrease",
|
||||
rclcpp::ParameterValue(1e-20));
|
||||
node->get_parameter(
|
||||
local_name + "line_search_sufficient_function_decrease",
|
||||
line_search_sufficient_function_decrease);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_num_line_search_direction_restarts",
|
||||
rclcpp::ParameterValue(10));
|
||||
node->get_parameter(
|
||||
local_name + "max_num_line_search_direction_restarts",
|
||||
max_num_line_search_direction_restarts);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_line_search_step_expansion",
|
||||
rclcpp::ParameterValue(50));
|
||||
node->get_parameter(
|
||||
local_name + "max_line_search_step_expansion",
|
||||
max_line_search_step_expansion);
|
||||
}
|
||||
|
||||
|
||||
double min_line_search_step_size; // Ceres default: 1e-9
|
||||
int max_num_line_search_step_size_iterations; // Ceres default: 20
|
||||
double line_search_sufficient_function_decrease; // Ceres default: 1e-4
|
||||
int max_num_line_search_direction_restarts; // Ceres default: 5
|
||||
|
||||
double max_line_search_step_contraction; // Ceres default: 1e-3
|
||||
double min_line_search_step_contraction; // Ceres default: 0.6
|
||||
double line_search_sufficient_curvature_decrease; // Ceres default: 0.9
|
||||
int max_line_search_step_expansion; // Ceres default: 10
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Get params from ROS parameter
|
||||
* @param node_ Ptr to node
|
||||
* @param name Name of plugin
|
||||
*/
|
||||
void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
|
||||
{
|
||||
std::string local_name = name + std::string(".smoother.optimizer.");
|
||||
|
||||
// Optimizer params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "param_tol", rclcpp::ParameterValue(1e-15));
|
||||
node->get_parameter(local_name + "param_tol", param_tol);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7));
|
||||
node->get_parameter(local_name + "fn_tol", fn_tol);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10));
|
||||
node->get_parameter(local_name + "gradient_tol", gradient_tol);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_iterations", rclcpp::ParameterValue(500));
|
||||
node->get_parameter(local_name + "max_iterations", max_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_time", rclcpp::ParameterValue(0.100));
|
||||
node->get_parameter(local_name + "max_time", max_time);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "debug_optimizer", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(local_name + "debug_optimizer", debug);
|
||||
|
||||
advanced.get(node, name);
|
||||
}
|
||||
|
||||
bool debug;
|
||||
int max_iterations; // Ceres default: 50
|
||||
double max_time; // Ceres default: 1e4
|
||||
|
||||
double param_tol; // Ceres default: 1e-8
|
||||
double fn_tol; // Ceres default: 1e-6
|
||||
double gradient_tol; // Ceres default: 1e-10
|
||||
|
||||
AdvancedParams advanced;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__OPTIONS_HPP_
|
||||
@@ -0,0 +1,146 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__SMOOTHER_HPP_
|
||||
#define DEPRECATED__SMOOTHER_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/smoother_cost_function.hpp"
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class nav2_smac_planner::Smoother
|
||||
* @brief A Conjugate Gradient 2D path smoother implementation
|
||||
*/
|
||||
class Smoother
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::Smoother
|
||||
*/
|
||||
Smoother() {}
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_smac_planner::Smoother
|
||||
*/
|
||||
~Smoother() {}
|
||||
|
||||
/**
|
||||
* @brief Initialization of the smoother
|
||||
* @param params OptimizerParam struct
|
||||
*/
|
||||
void initialize(const OptimizerParams params)
|
||||
{
|
||||
_debug = params.debug;
|
||||
|
||||
// General Params
|
||||
|
||||
// 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
|
||||
_options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
|
||||
_options.line_search_type = ceres::WOLFE;
|
||||
_options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
|
||||
_options.line_search_interpolation_type = ceres::CUBIC;
|
||||
|
||||
_options.max_num_iterations = params.max_iterations;
|
||||
_options.max_solver_time_in_seconds = params.max_time;
|
||||
|
||||
_options.function_tolerance = params.fn_tol;
|
||||
_options.gradient_tolerance = params.gradient_tol;
|
||||
_options.parameter_tolerance = params.param_tol;
|
||||
|
||||
_options.min_line_search_step_size = params.advanced.min_line_search_step_size;
|
||||
_options.max_num_line_search_step_size_iterations =
|
||||
params.advanced.max_num_line_search_step_size_iterations;
|
||||
_options.line_search_sufficient_function_decrease =
|
||||
params.advanced.line_search_sufficient_function_decrease;
|
||||
_options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
|
||||
_options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
|
||||
_options.max_num_line_search_direction_restarts =
|
||||
params.advanced.max_num_line_search_direction_restarts;
|
||||
_options.line_search_sufficient_curvature_decrease =
|
||||
params.advanced.line_search_sufficient_curvature_decrease;
|
||||
_options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
|
||||
|
||||
if (_debug) {
|
||||
_options.minimizer_progress_to_stdout = true;
|
||||
} else {
|
||||
_options.logging_type = ceres::SILENT;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Smoother method
|
||||
* @param path Reference to path
|
||||
* @param costmap Pointer to minimal costmap
|
||||
* @param smoother parameters weights
|
||||
* @return If smoothing was successful
|
||||
*/
|
||||
bool smooth(
|
||||
std::vector<Eigen::Vector2d> & path,
|
||||
nav2_costmap_2d::Costmap2D * costmap,
|
||||
const SmootherParams & params)
|
||||
{
|
||||
_options.max_solver_time_in_seconds = params.max_time;
|
||||
|
||||
#ifdef _MSC_VER
|
||||
std::vector<double> parameters_vec(path.size() * 2);
|
||||
double * parameters = parameters_vec.data();
|
||||
#else
|
||||
double parameters[path.size() * 2]; // NOLINT
|
||||
#endif
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
parameters[2 * i] = path[i][0];
|
||||
parameters[2 * i + 1] = path[i][1];
|
||||
}
|
||||
|
||||
ceres::GradientProblemSolver::Summary summary;
|
||||
ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params));
|
||||
ceres::Solve(_options, problem, parameters, &summary);
|
||||
|
||||
if (_debug) {
|
||||
std::cout << summary.FullReport() << '\n';
|
||||
}
|
||||
|
||||
if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
path[i][0] = parameters[2 * i];
|
||||
path[i][1] = parameters[2 * i + 1];
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
bool _debug;
|
||||
ceres::GradientProblemSolver::Options _options;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__SMOOTHER_HPP_
|
||||
@@ -0,0 +1,542 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
|
||||
#define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_smac_planner/options.hpp"
|
||||
|
||||
#define EPSILON 0.0001
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct nav2_smac_planner::UnconstrainedSmootherCostFunction
|
||||
* @brief Cost function for path smoothing with multiple terms
|
||||
* including curvature, smoothness, collision, and avoid obstacles.
|
||||
*/
|
||||
class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::UnconstrainedSmootherCostFunction
|
||||
* @param original_path Original unsmoothed path to smooth
|
||||
* @param costmap A costmap to get values for collision and obstacle avoidance
|
||||
*/
|
||||
UnconstrainedSmootherCostFunction(
|
||||
std::vector<Eigen::Vector2d> * original_path,
|
||||
nav2_costmap_2d::Costmap2D * costmap,
|
||||
const SmootherParams & params)
|
||||
: _original_path(original_path),
|
||||
_num_params(2 * original_path->size()),
|
||||
_costmap(costmap),
|
||||
_params(params)
|
||||
{
|
||||
// int height = costmap->getSizeInCellsX();
|
||||
// int width = costmap->getSizeInCellsY();
|
||||
// bool** binMap;
|
||||
// binMap = new bool*[width];
|
||||
|
||||
// for (int x = 0; x < width; x++) { binMap[x] = new bool[height]; }
|
||||
|
||||
// for (int x = 0; x < width; ++x) {
|
||||
// for (int y = 0; y < height; ++y) {
|
||||
// binMap[x][y] = costmap->getCost(x,y) >= 253 ? true : false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// voronoiDiagram.initializeMap(width, height, binMap);
|
||||
// voronoiDiagram.update();
|
||||
// voronoiDiagram.visualize();
|
||||
}
|
||||
|
||||
/**
|
||||
* @struct CurvatureComputations
|
||||
* @brief Cache common computations between the curvature terms to minimize recomputations
|
||||
*/
|
||||
struct CurvatureComputations
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::CurvatureComputations
|
||||
*/
|
||||
CurvatureComputations()
|
||||
{
|
||||
valid = true;
|
||||
}
|
||||
|
||||
bool valid;
|
||||
/**
|
||||
* @brief Check if result is valid for penalty
|
||||
* @return is valid (non-nan, non-inf, and turning angle > max)
|
||||
*/
|
||||
bool isValid()
|
||||
{
|
||||
return valid;
|
||||
}
|
||||
|
||||
Eigen::Vector2d delta_xi{0.0, 0.0};
|
||||
Eigen::Vector2d delta_xi_p{0.0, 0.0};
|
||||
double delta_xi_norm{0};
|
||||
double delta_xi_p_norm{0};
|
||||
double delta_phi_i{0};
|
||||
double turning_rad{0};
|
||||
double ki_minus_kmax{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Smoother cost function evaluation
|
||||
* @param parameters X,Y pairs of points
|
||||
* @param cost total cost of path
|
||||
* @param gradient of path at each X,Y pair from cost function derived analytically
|
||||
* @return if successful in computing values
|
||||
*/
|
||||
virtual bool Evaluate(
|
||||
const double * parameters,
|
||||
double * cost,
|
||||
double * gradient) const
|
||||
{
|
||||
Eigen::Vector2d xi;
|
||||
Eigen::Vector2d xi_p1;
|
||||
Eigen::Vector2d xi_m1;
|
||||
unsigned int x_index, y_index;
|
||||
cost[0] = 0.0;
|
||||
double cost_raw = 0.0;
|
||||
double grad_x_raw = 0.0;
|
||||
double grad_y_raw = 0.0;
|
||||
unsigned int mx, my;
|
||||
bool valid_coords = true;
|
||||
double costmap_cost = 0.0;
|
||||
|
||||
// cache some computations between the residual and jacobian
|
||||
CurvatureComputations curvature_params;
|
||||
|
||||
for (int i = 0; i != NumParameters() / 2; i++) {
|
||||
x_index = 2 * i;
|
||||
y_index = 2 * i + 1;
|
||||
gradient[x_index] = 0.0;
|
||||
gradient[y_index] = 0.0;
|
||||
if (i < 1 || i >= NumParameters() / 2 - 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]);
|
||||
xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]);
|
||||
xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]);
|
||||
|
||||
// compute cost
|
||||
addSmoothingResidual(_params.smooth_weight, xi, xi_p1, xi_m1, cost_raw);
|
||||
addCurvatureResidual(_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw);
|
||||
addDistanceResidual(_params.distance_weight, xi, _original_path->at(i), cost_raw);
|
||||
|
||||
if (valid_coords = _costmap->worldToMap(xi[0], xi[1], mx, my)) {
|
||||
costmap_cost = _costmap->getCost(mx, my);
|
||||
addCostResidual(_params.costmap_weight, costmap_cost, cost_raw, xi);
|
||||
}
|
||||
|
||||
if (gradient != NULL) {
|
||||
// compute gradient
|
||||
gradient[x_index] = 0.0;
|
||||
gradient[y_index] = 0.0;
|
||||
addSmoothingJacobian(_params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
|
||||
addCurvatureJacobian(
|
||||
_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params,
|
||||
grad_x_raw, grad_y_raw);
|
||||
addDistanceJacobian(
|
||||
_params.distance_weight, xi, _original_path->at(
|
||||
i), grad_x_raw, grad_y_raw);
|
||||
|
||||
if (valid_coords) {
|
||||
addCostJacobian(_params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw);
|
||||
}
|
||||
|
||||
gradient[x_index] = grad_x_raw;
|
||||
gradient[y_index] = grad_y_raw;
|
||||
}
|
||||
}
|
||||
|
||||
cost[0] = cost_raw;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get number of parameter blocks
|
||||
* @return Number of parameters in cost function
|
||||
*/
|
||||
virtual int NumParameters() const {return _num_params;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Cost function term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addSmoothingResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (
|
||||
pt_p.dot(pt_p) -
|
||||
4 * pt_p.dot(pt) +
|
||||
2 * pt_p.dot(pt_m) +
|
||||
4 * pt.dot(pt) -
|
||||
4 * pt.dot(pt_m) +
|
||||
pt_m.dot(pt_m)); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addSmoothingJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight *
|
||||
(-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative
|
||||
j1 += weight *
|
||||
(-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function term for maximum curved paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCurvatureResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & r) const
|
||||
{
|
||||
curvature_params.valid = true;
|
||||
curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
|
||||
curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
|
||||
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
|
||||
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
|
||||
|
||||
if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
|
||||
std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
|
||||
std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
|
||||
{
|
||||
// ensure we have non-nan values returned
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const double & delta_xi_by_xi_p =
|
||||
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
|
||||
double projection =
|
||||
curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
|
||||
if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
|
||||
projection = 1.0;
|
||||
}
|
||||
|
||||
curvature_params.delta_phi_i = std::acos(projection);
|
||||
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
|
||||
|
||||
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature;
|
||||
|
||||
if (curvature_params.ki_minus_kmax <= EPSILON) {
|
||||
// Quadratic penalty need not apply
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
r += weight *
|
||||
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for maximum curvature paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct with cached values to speed up Jacobian computation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCurvatureJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & /*pt_m*/,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
|
||||
-1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
|
||||
// const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
|
||||
auto neg_pt_plus = -1 * pt_p;
|
||||
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
|
||||
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
|
||||
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
|
||||
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
|
||||
|
||||
const double & u = 2 * curvature_params.ki_minus_kmax;
|
||||
const double & common_prefix =
|
||||
(1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
|
||||
const double & common_suffix = curvature_params.delta_phi_i /
|
||||
(curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
|
||||
|
||||
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
|
||||
curvature_params.delta_xi_norm;
|
||||
|
||||
const Eigen::Vector2d jacobian = u *
|
||||
(common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_im1 = u *
|
||||
(common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
|
||||
|
||||
// Old formulation we may require again.
|
||||
// j0 += weight *
|
||||
// (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
|
||||
// j1 += weight *
|
||||
// (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
|
||||
|
||||
j0 += weight * jacobian[0]; // xi x component of partial-derivative
|
||||
j1 += weight * jacobian[1]; // xi x component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for steering away changes in pose
|
||||
* @param weight Weight to apply to function
|
||||
* @param xi Point Xi for evaluation
|
||||
* @param xi_original original point Xi for evaluation
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addDistanceResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & xi,
|
||||
const Eigen::Vector2d & xi_original,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (xi - xi_original).dot(xi - xi_original); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for steering away changes in pose
|
||||
* @param weight Weight to apply to function
|
||||
* @param xi Point Xi for evaluation
|
||||
* @param xi_original original point Xi for evaluation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addDistanceJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & xi,
|
||||
const Eigen::Vector2d & xi_original,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight * 2 * (xi[0] - xi_original[0]); // xi y component of partial-derivative
|
||||
j1 += weight * 2 * (xi[1] - xi_original[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Cost function term for steering away from costs
|
||||
* @param weight Weight to apply to function
|
||||
* @param value Point Xi's cost'
|
||||
* @param params computed values to reduce overhead
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCostResidual(
|
||||
const double & weight,
|
||||
const double & value,
|
||||
double & r,
|
||||
Eigen::Vector2d & xi) const
|
||||
{
|
||||
if (value == FREE) {
|
||||
return;
|
||||
}
|
||||
|
||||
r += weight * value * value; // objective function value
|
||||
|
||||
|
||||
// float obsDst = voronoiDiagram.getDistance((int)xi[0], (int)xi[1]);
|
||||
|
||||
// if (abs(obsDst) > 0.3) {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// r += weight * (abs(obsDst) - 0.3) * (abs(obsDst) - 0.3);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for steering away from costs
|
||||
* @param weight Weight to apply to function
|
||||
* @param mx Point Xi's x coordinate in map frame
|
||||
* @param mx Point Xi's y coordinate in map frame
|
||||
* @param value Point Xi's cost'
|
||||
* @param params computed values to reduce overhead
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCostJacobian(
|
||||
const double & weight,
|
||||
const unsigned int & mx,
|
||||
const unsigned int & my,
|
||||
const double & value,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (value == FREE) {
|
||||
return;
|
||||
}
|
||||
|
||||
const Eigen::Vector2d grad = getCostmapGradient(mx, my);
|
||||
const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value;
|
||||
|
||||
j0 += common_prefix * grad[0]; // xi x component of partial-derivative
|
||||
j1 += common_prefix * grad[1]; // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Computing the gradient of the costmap using
|
||||
* the 2 point numerical differentiation method
|
||||
* @param mx Point Xi's x coordinate in map frame
|
||||
* @param mx Point Xi's y coordinate in map frame
|
||||
* @param params Params reference to store gradients
|
||||
*/
|
||||
inline Eigen::Vector2d getCostmapGradient(
|
||||
const unsigned int mx,
|
||||
const unsigned int my) const
|
||||
{
|
||||
// find unit vector that describes that direction
|
||||
// via 7 point taylor series approximation for gradient at Xi
|
||||
Eigen::Vector2d gradient;
|
||||
|
||||
double l_1 = 0.0;
|
||||
double l_2 = 0.0;
|
||||
double l_3 = 0.0;
|
||||
double r_1 = 0.0;
|
||||
double r_2 = 0.0;
|
||||
double r_3 = 0.0;
|
||||
|
||||
if (mx < _costmap->getSizeInCellsX()) {
|
||||
r_1 = static_cast<double>(_costmap->getCost(mx + 1, my));
|
||||
}
|
||||
if (mx + 1 < _costmap->getSizeInCellsX()) {
|
||||
r_2 = static_cast<double>(_costmap->getCost(mx + 2, my));
|
||||
}
|
||||
if (mx + 2 < _costmap->getSizeInCellsX()) {
|
||||
r_3 = static_cast<double>(_costmap->getCost(mx + 3, my));
|
||||
}
|
||||
|
||||
if (mx > 0) {
|
||||
l_1 = static_cast<double>(_costmap->getCost(mx - 1, my));
|
||||
}
|
||||
if (mx - 1 > 0) {
|
||||
l_2 = static_cast<double>(_costmap->getCost(mx - 2, my));
|
||||
}
|
||||
if (mx - 2 > 0) {
|
||||
l_3 = static_cast<double>(_costmap->getCost(mx - 3, my));
|
||||
}
|
||||
|
||||
gradient[1] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60;
|
||||
|
||||
if (my < _costmap->getSizeInCellsY()) {
|
||||
r_1 = static_cast<double>(_costmap->getCost(mx, my + 1));
|
||||
}
|
||||
if (my + 1 < _costmap->getSizeInCellsY()) {
|
||||
r_2 = static_cast<double>(_costmap->getCost(mx, my + 2));
|
||||
}
|
||||
if (my + 2 < _costmap->getSizeInCellsY()) {
|
||||
r_3 = static_cast<double>(_costmap->getCost(mx, my + 3));
|
||||
}
|
||||
|
||||
if (my > 0) {
|
||||
l_1 = static_cast<double>(_costmap->getCost(mx, my - 1));
|
||||
}
|
||||
if (my - 1 > 0) {
|
||||
l_2 = static_cast<double>(_costmap->getCost(mx, my - 2));
|
||||
}
|
||||
if (my - 2 > 0) {
|
||||
l_3 = static_cast<double>(_costmap->getCost(mx, my - 3));
|
||||
}
|
||||
|
||||
gradient[0] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60;
|
||||
|
||||
gradient.normalize();
|
||||
return gradient;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Computing the normalized orthogonal component of 2 vectors
|
||||
* @param a Vector
|
||||
* @param b Vector
|
||||
* @param norm a Vector's norm
|
||||
* @param norm b Vector's norm
|
||||
* @return Normalized vector of orthogonal components
|
||||
*/
|
||||
inline Eigen::Vector2d normalizedOrthogonalComplement(
|
||||
const Eigen::Vector2d & a,
|
||||
const Eigen::Vector2d & b,
|
||||
const double & a_norm,
|
||||
const double & b_norm) const
|
||||
{
|
||||
return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
|
||||
}
|
||||
|
||||
std::vector<Eigen::Vector2d> * _original_path{nullptr};
|
||||
int _num_params;
|
||||
nav2_costmap_2d::Costmap2D * _costmap{nullptr};
|
||||
SmootherParams _params;
|
||||
// DynamicVoronoi voronoiDiagram;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
|
||||
@@ -0,0 +1,213 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__UPSAMPLER_HPP_
|
||||
#define DEPRECATED__UPSAMPLER_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <algorithm>
|
||||
#include <utility>
|
||||
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/upsampler_cost_function.hpp"
|
||||
#include "nav2_smac_planner/upsampler_cost_function_nlls.hpp"
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class nav2_smac_planner::Upsampler
|
||||
* @brief A Conjugate Gradient 2D path upsampler implementation
|
||||
*/
|
||||
class Upsampler
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::Upsampler
|
||||
*/
|
||||
Upsampler() {}
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_smac_planner::Upsampler
|
||||
*/
|
||||
~Upsampler() {}
|
||||
|
||||
/**
|
||||
* @brief Initialization of the Upsampler
|
||||
*/
|
||||
void initialize(const OptimizerParams params)
|
||||
{
|
||||
_debug = params.debug;
|
||||
|
||||
// General Params
|
||||
|
||||
// 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
|
||||
_options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
|
||||
_options.line_search_type = ceres::WOLFE;
|
||||
_options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
|
||||
_options.line_search_interpolation_type = ceres::CUBIC;
|
||||
|
||||
_options.max_num_iterations = params.max_iterations; // 5000
|
||||
_options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO
|
||||
|
||||
_options.function_tolerance = params.fn_tol;
|
||||
_options.gradient_tolerance = params.gradient_tol;
|
||||
_options.parameter_tolerance = params.param_tol; // 1e-20;
|
||||
|
||||
_options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30;
|
||||
_options.max_num_line_search_step_size_iterations =
|
||||
params.advanced.max_num_line_search_step_size_iterations;
|
||||
_options.line_search_sufficient_function_decrease =
|
||||
params.advanced.line_search_sufficient_function_decrease; // 1e-30;
|
||||
_options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
|
||||
_options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
|
||||
_options.max_num_line_search_direction_restarts =
|
||||
params.advanced.max_num_line_search_direction_restarts;
|
||||
_options.line_search_sufficient_curvature_decrease =
|
||||
params.advanced.line_search_sufficient_curvature_decrease;
|
||||
_options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
|
||||
|
||||
if (_debug) {
|
||||
_options.minimizer_progress_to_stdout = true;
|
||||
} else {
|
||||
_options.logging_type = ceres::SILENT;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Upsampling method
|
||||
* @param path Reference to path
|
||||
* @param upsample parameters weights
|
||||
* @param upsample_ratio upsample ratio
|
||||
* @return If Upsampler was successful
|
||||
*/
|
||||
bool upsample(
|
||||
std::vector<Eigen::Vector2d> & path,
|
||||
const SmootherParams & params,
|
||||
const int & upsample_ratio)
|
||||
{
|
||||
_options.max_solver_time_in_seconds = params.max_time;
|
||||
|
||||
if (upsample_ratio != 2 && upsample_ratio != 4) {
|
||||
// invalid inputs
|
||||
return false;
|
||||
}
|
||||
|
||||
const int param_ratio = upsample_ratio * 2.0;
|
||||
const int total_size = 2 * (path.size() * upsample_ratio - upsample_ratio + 1);
|
||||
double parameters[total_size]; // NOLINT
|
||||
|
||||
// 20-4hz regularly, but dosnt work in faster cases
|
||||
// Linearly distribute initial poses for optimization
|
||||
// TODO(stevemacenski) generalize for 2x and 4x
|
||||
unsigned int next_pt;
|
||||
Eigen::Vector2d interpolated;
|
||||
std::vector<Eigen::Vector2d> temp_path;
|
||||
for (unsigned int pt = 0; pt != path.size() - 1; pt++) {
|
||||
next_pt = pt + 1;
|
||||
interpolated = (path[next_pt] + path[pt]) / 2.0;
|
||||
|
||||
parameters[param_ratio * pt] = path[pt][0];
|
||||
parameters[param_ratio * pt + 1] = path[pt][1];
|
||||
temp_path.push_back(path[pt]);
|
||||
|
||||
parameters[param_ratio * pt + 2] = interpolated[0];
|
||||
parameters[param_ratio * pt + 3] = interpolated[1];
|
||||
temp_path.push_back(interpolated);
|
||||
}
|
||||
|
||||
parameters[total_size - 2] = path.back()[0];
|
||||
parameters[total_size - 1] = path.back()[1];
|
||||
temp_path.push_back(path.back());
|
||||
|
||||
// Solve the upsampling problem
|
||||
ceres::GradientProblemSolver::Summary summary;
|
||||
ceres::GradientProblem problem(new UpsamplerCostFunction(temp_path, params, upsample_ratio));
|
||||
ceres::Solve(_options, problem, parameters, &summary);
|
||||
|
||||
|
||||
path.resize(total_size / 2);
|
||||
for (int i = 0; i != total_size / 2; i++) {
|
||||
path[i][0] = parameters[2 * i];
|
||||
path[i][1] = parameters[2 * i + 1];
|
||||
}
|
||||
|
||||
// 10-15 hz, regularly
|
||||
// std::vector<Eigen::Vector2d> path_double_sampled;
|
||||
// for (int i = 0; i != path.size() - 1; i++) { // last term should not be upsampled
|
||||
// path_double_sampled.push_back(path[i]);
|
||||
// path_double_sampled.push_back((path[i+1] + path[i]) / 2);
|
||||
// }
|
||||
|
||||
// std::unique_ptr<ceres::Problem> problem = std::make_unique<ceres::Problem>();
|
||||
// for (uint i = 1; i != path_double_sampled.size() - 1; i++) {
|
||||
// ceres::CostFunction * cost_fn =
|
||||
// new UpsamplerConstrainedCostFunction(path_double_sampled, params, 2, i);
|
||||
// problem->AddResidualBlock(
|
||||
// cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1]);
|
||||
// // locking initial coordinates unnecessary since there's no update between terms in NLLS
|
||||
// }
|
||||
|
||||
// ceres::Solver::Summary summary;
|
||||
// _options.minimizer_type = ceres::LINE_SEARCH;
|
||||
// ceres::Solve(_options, problem.get(), &summary);
|
||||
|
||||
// if (upsample_ratio == 4) {
|
||||
// std::vector<Eigen::Vector2d> path_quad_sampled;
|
||||
// for (int i = 0; i != path_double_sampled.size() - 1; i++) {
|
||||
// path_quad_sampled.push_back(path_double_sampled[i]);
|
||||
// path_quad_sampled.push_back((path_double_sampled[i+1] + path_double_sampled[i]) / 2.0);
|
||||
// }
|
||||
|
||||
// std::unique_ptr<ceres::Problem> problem2 = std::make_unique<ceres::Problem>();
|
||||
// for (uint i = 1; i != path_quad_sampled.size() - 1; i++) {
|
||||
// ceres::CostFunction * cost_fn =
|
||||
// new UpsamplerConstrainedCostFunction(path_quad_sampled, params, 4, i);
|
||||
// problem2->AddResidualBlock(
|
||||
// cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1]);
|
||||
// }
|
||||
|
||||
// ceres::Solve(_options, problem2.get(), &summary);
|
||||
|
||||
// path = path_quad_sampled;
|
||||
// } else {
|
||||
// path = path_double_sampled;
|
||||
// }
|
||||
|
||||
if (_debug) {
|
||||
std::cout << summary.FullReport() << '\n';
|
||||
}
|
||||
|
||||
if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
bool _debug;
|
||||
ceres::GradientProblemSolver::Options _options;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__UPSAMPLER_HPP_
|
||||
@@ -0,0 +1,366 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
|
||||
#define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/options.hpp"
|
||||
|
||||
#define EPSILON 0.0001
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
/**
|
||||
* @struct nav2_smac_planner::UpsamplerCostFunction
|
||||
* @brief Cost function for path upsampling with multiple terms using unconstrained
|
||||
* optimization including curvature, smoothness, collision, and avoid obstacles.
|
||||
*/
|
||||
class UpsamplerCostFunction : public ceres::FirstOrderFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::UpsamplerCostFunction
|
||||
* @param num_points Number of path points to consider
|
||||
*/
|
||||
UpsamplerCostFunction(
|
||||
const std::vector<Eigen::Vector2d> & path,
|
||||
const SmootherParams & params,
|
||||
const int & upsample_ratio)
|
||||
: _num_params(2 * path.size()),
|
||||
_params(params),
|
||||
_upsample_ratio(upsample_ratio),
|
||||
_path(path)
|
||||
{
|
||||
}
|
||||
// TODO(stevemacenski) removed upsample_ratio because temp upsampling on path size
|
||||
|
||||
/**
|
||||
* @struct CurvatureComputations
|
||||
* @brief Cache common computations between the curvature terms to minimize recomputations
|
||||
*/
|
||||
struct CurvatureComputations
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::CurvatureComputations
|
||||
*/
|
||||
CurvatureComputations()
|
||||
{
|
||||
valid = false;
|
||||
}
|
||||
|
||||
bool valid;
|
||||
/**
|
||||
* @brief Check if result is valid for penalty
|
||||
* @return is valid (non-nan, non-inf, and turning angle > max)
|
||||
*/
|
||||
bool isValid()
|
||||
{
|
||||
return valid;
|
||||
}
|
||||
|
||||
Eigen::Vector2d delta_xi{0, 0};
|
||||
Eigen::Vector2d delta_xi_p{0, 0};
|
||||
double delta_xi_norm{0};
|
||||
double delta_xi_p_norm{0};
|
||||
double delta_phi_i{0};
|
||||
double turning_rad{0};
|
||||
double ki_minus_kmax{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Smoother cost function evaluation
|
||||
* @param parameters X,Y pairs of points
|
||||
* @param cost total cost of path
|
||||
* @param gradient of path at each X,Y pair from cost function derived analytically
|
||||
* @return if successful in computing values
|
||||
*/
|
||||
virtual bool Evaluate(
|
||||
const double * parameters,
|
||||
double * cost,
|
||||
double * gradient) const
|
||||
{
|
||||
Eigen::Vector2d xi;
|
||||
Eigen::Vector2d xi_p1;
|
||||
Eigen::Vector2d xi_m1;
|
||||
uint x_index, y_index;
|
||||
cost[0] = 0.0;
|
||||
double cost_raw = 0.0;
|
||||
double grad_x_raw = 0.0;
|
||||
double grad_y_raw = 0.0;
|
||||
|
||||
// cache some computations between the residual and jacobian
|
||||
CurvatureComputations curvature_params;
|
||||
|
||||
for (int i = 0; i != NumParameters() / 2; i++) {
|
||||
x_index = 2 * i;
|
||||
y_index = 2 * i + 1;
|
||||
gradient[x_index] = 0.0;
|
||||
gradient[y_index] = 0.0;
|
||||
if (i < 1 || i >= NumParameters() / 2 - 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// if original point's neighbors TODO
|
||||
if (i % _upsample_ratio == 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]);
|
||||
|
||||
// TODO(stevemacenski): from deep copy to make sure no feedback _path
|
||||
xi_p1 = _path.at(i + 1);
|
||||
xi_m1 = _path.at(i - 1);
|
||||
// xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]);
|
||||
// xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]);
|
||||
|
||||
// compute cost
|
||||
addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw);
|
||||
addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw);
|
||||
|
||||
if (gradient != NULL) {
|
||||
// compute gradient
|
||||
addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
|
||||
addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw);
|
||||
|
||||
gradient[x_index] = grad_x_raw;
|
||||
gradient[y_index] = grad_y_raw;
|
||||
}
|
||||
}
|
||||
|
||||
cost[0] = cost_raw;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get number of parameter blocks
|
||||
* @return Number of parameters in cost function
|
||||
*/
|
||||
virtual int NumParameters() const {return _num_params;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Cost function term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addSmoothingResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (
|
||||
pt_p.dot(pt_p) -
|
||||
4 * pt_p.dot(pt) +
|
||||
2 * pt_p.dot(pt_m) +
|
||||
4 * pt.dot(pt) -
|
||||
4 * pt.dot(pt_m) +
|
||||
pt_m.dot(pt_m)); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addSmoothingJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight *
|
||||
(-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative
|
||||
j1 += weight *
|
||||
(-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get path curvature information
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
*/
|
||||
inline void getCurvatureParams(
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params) const
|
||||
{
|
||||
curvature_params.valid = true;
|
||||
curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
|
||||
curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
|
||||
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
|
||||
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
|
||||
|
||||
if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
|
||||
std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
|
||||
std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
|
||||
{
|
||||
// ensure we have non-nan values returned
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const double & delta_xi_by_xi_p =
|
||||
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
|
||||
double projection =
|
||||
curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
|
||||
if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
|
||||
projection = 1.0;
|
||||
}
|
||||
|
||||
curvature_params.delta_phi_i = std::acos(projection);
|
||||
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
|
||||
|
||||
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
|
||||
_params.max_curvature;
|
||||
// TODO(stevemacenski) is use of upsample_ratio correct here? small number?
|
||||
// TODO(stevemacenski) can remove the subtraction with a
|
||||
// lower weight value, does have direction issue, maybe just tuning?
|
||||
|
||||
if (curvature_params.ki_minus_kmax <= EPSILON) {
|
||||
// Quadratic penalty need not apply
|
||||
curvature_params.valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function term for maximum curved paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCurvatureResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & r) const
|
||||
{
|
||||
getCurvatureParams(pt, pt_p, pt_m, curvature_params);
|
||||
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
r += weight *
|
||||
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for maximum curvature paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct with cached values to speed up Jacobian computation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCurvatureJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & /*pt_m*/,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
|
||||
-1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
|
||||
// const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
|
||||
auto neg_pt_plus = -1 * pt_p;
|
||||
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
|
||||
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
|
||||
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
|
||||
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
|
||||
|
||||
const double & u = 2 * curvature_params.ki_minus_kmax;
|
||||
const double & common_prefix =
|
||||
(1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
|
||||
const double & common_suffix = curvature_params.delta_phi_i /
|
||||
(curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
|
||||
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
|
||||
curvature_params.delta_xi_norm;
|
||||
|
||||
const Eigen::Vector2d jacobian = u *
|
||||
(common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_im1 = u *
|
||||
(common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
|
||||
j0 += weight * jacobian[0]; // xi y component of partial-derivative
|
||||
j1 += weight * jacobian[1]; // xi x component of partial-derivative
|
||||
// j0 += weight *
|
||||
// (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
|
||||
// j1 += weight *
|
||||
// (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Computing the normalized orthogonal component of 2 vectors
|
||||
* @param a Vector
|
||||
* @param b Vector
|
||||
* @param norm a Vector's norm
|
||||
* @param norm b Vector's norm
|
||||
* @return Normalized vector of orthogonal components
|
||||
*/
|
||||
inline Eigen::Vector2d normalizedOrthogonalComplement(
|
||||
const Eigen::Vector2d & a,
|
||||
const Eigen::Vector2d & b,
|
||||
const double & a_norm,
|
||||
const double & b_norm) const
|
||||
{
|
||||
return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
|
||||
}
|
||||
|
||||
int _num_params;
|
||||
SmootherParams _params;
|
||||
int _upsample_ratio;
|
||||
std::vector<Eigen::Vector2d> _path;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
|
||||
@@ -0,0 +1,334 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
|
||||
#define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/options.hpp"
|
||||
|
||||
#define EPSILON 0.0001
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
/**
|
||||
* @struct nav2_smac_planner::UpsamplerConstrainedCostFunction
|
||||
* @brief Cost function for path upsampling with multiple terms using NLLS
|
||||
* including curvature, smoothness, collision, and avoid obstacles.
|
||||
*/
|
||||
class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction
|
||||
* @param num_points Number of path points to consider
|
||||
*/
|
||||
UpsamplerConstrainedCostFunction(
|
||||
const std::vector<Eigen::Vector2d> & path,
|
||||
const SmootherParams & params,
|
||||
const int & upsample_ratio,
|
||||
const int & i)
|
||||
: _path(path),
|
||||
_params(params),
|
||||
_upsample_ratio(upsample_ratio),
|
||||
index(i)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @struct CurvatureComputations
|
||||
* @brief Cache common computations between the curvature terms to minimize recomputations
|
||||
*/
|
||||
struct CurvatureComputations
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::CurvatureComputations
|
||||
*/
|
||||
CurvatureComputations()
|
||||
{
|
||||
valid = true;
|
||||
}
|
||||
|
||||
bool valid;
|
||||
/**
|
||||
* @brief Check if result is valid for penalty
|
||||
* @return is valid (non-nan, non-inf, and turning angle > max)
|
||||
*/
|
||||
bool isValid()
|
||||
{
|
||||
return valid;
|
||||
}
|
||||
|
||||
Eigen::Vector2d delta_xi{0, 0};
|
||||
Eigen::Vector2d delta_xi_p{0, 0};
|
||||
double delta_xi_norm{0};
|
||||
double delta_xi_p_norm{0};
|
||||
double delta_phi_i{0};
|
||||
double turning_rad{0};
|
||||
double ki_minus_kmax{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Smoother cost function evaluation
|
||||
* @param parameters X,Y pairs of points
|
||||
* @param cost total cost of path
|
||||
* @param gradient of path at each X,Y pair from cost function derived analytically
|
||||
* @return if successful in computing values
|
||||
*/
|
||||
|
||||
bool Evaluate(
|
||||
double const * const * parameters,
|
||||
double * residuals,
|
||||
double ** jacobians) const override
|
||||
{
|
||||
Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]);
|
||||
Eigen::Vector2d xi_p1 = _path.at(index + 1);
|
||||
Eigen::Vector2d xi_m1 = _path.at(index - 1);
|
||||
CurvatureComputations curvature_params;
|
||||
double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0;
|
||||
|
||||
// compute cost
|
||||
addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw);
|
||||
addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw);
|
||||
|
||||
residuals[0] = 0;
|
||||
residuals[0] = cost_raw; // objective function value x
|
||||
|
||||
if (jacobians != NULL && jacobians[0] != NULL) {
|
||||
addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
|
||||
addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw);
|
||||
|
||||
jacobians[0][0] = 0;
|
||||
jacobians[1][0] = 0;
|
||||
jacobians[0][0] = grad_x_raw; // x derivative
|
||||
jacobians[1][0] = grad_y_raw; // y derivative
|
||||
jacobians[0][1] = 0.0;
|
||||
jacobians[1][1] = 0.0;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Cost function term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addSmoothingResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (
|
||||
pt_p.dot(pt_p) -
|
||||
4 * pt_p.dot(pt) +
|
||||
2 * pt_p.dot(pt_m) +
|
||||
4 * pt.dot(pt) -
|
||||
4 * pt.dot(pt_m) +
|
||||
pt_m.dot(pt_m)); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addSmoothingJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight *
|
||||
(-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative
|
||||
j1 += weight *
|
||||
(-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get path curvature information
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
*/
|
||||
inline void getCurvatureParams(
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params) const
|
||||
{
|
||||
curvature_params.valid = true;
|
||||
curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
|
||||
curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
|
||||
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
|
||||
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
|
||||
|
||||
if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
|
||||
std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
|
||||
std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
|
||||
{
|
||||
// ensure we have non-nan values returned
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const double & delta_xi_by_xi_p =
|
||||
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
|
||||
double projection =
|
||||
curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
|
||||
if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
|
||||
projection = 1.0;
|
||||
}
|
||||
|
||||
curvature_params.delta_phi_i = std::acos(projection);
|
||||
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
|
||||
|
||||
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
|
||||
_params.max_curvature;
|
||||
|
||||
if (curvature_params.ki_minus_kmax <= EPSILON) {
|
||||
// Quadratic penalty need not apply
|
||||
curvature_params.valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function term for maximum curved paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCurvatureResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & r) const
|
||||
{
|
||||
getCurvatureParams(pt, pt_p, pt_m, curvature_params);
|
||||
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// objective function value
|
||||
r += weight *
|
||||
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for maximum curvature paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct with cached values to speed up Jacobian computation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCurvatureJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & /*pt_m*/,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
|
||||
-1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
|
||||
// const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
|
||||
auto neg_pt_plus = -1 * pt_p;
|
||||
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
|
||||
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
|
||||
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
|
||||
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
|
||||
|
||||
const double & u = 2 * curvature_params.ki_minus_kmax;
|
||||
const double & common_prefix =
|
||||
(1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
|
||||
const double & common_suffix = curvature_params.delta_phi_i /
|
||||
(curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
|
||||
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
|
||||
curvature_params.delta_xi_norm;
|
||||
|
||||
const Eigen::Vector2d jacobian = u *
|
||||
(common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_im1 = u *
|
||||
(common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
|
||||
j0 += weight * jacobian[0]; // xi x component of partial-derivative
|
||||
j1 += weight * jacobian[1]; // xi y component of partial-derivative
|
||||
// j0 += weight *
|
||||
// (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
|
||||
// j1 += weight *
|
||||
// (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
|
||||
}
|
||||
/**
|
||||
* @brief Computing the normalized orthogonal component of 2 vectors
|
||||
* @param a Vector
|
||||
* @param b Vector
|
||||
* @param norm a Vector's norm
|
||||
* @param norm b Vector's norm
|
||||
* @return Normalized vector of orthogonal components
|
||||
*/
|
||||
inline Eigen::Vector2d normalizedOrthogonalComplement(
|
||||
const Eigen::Vector2d & a,
|
||||
const Eigen::Vector2d & b,
|
||||
const double & a_norm,
|
||||
const double & b_norm) const
|
||||
{
|
||||
return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
|
||||
}
|
||||
|
||||
std::vector<Eigen::Vector2d> _path;
|
||||
SmootherParams _params;
|
||||
int _upsample_ratio;
|
||||
int index;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
|
||||
Reference in New Issue
Block a user