From 472459eabea26d186e693f4afa8b841be3db25aa Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Wed, 30 Aug 2023 14:35:31 +0200 Subject: [PATCH] `planner` - added `UnsaturatedTranslation` cost function [#108] --- CMakeLists.txt | 1 + include/humap_local_planner/humap_planner.h | 3 + .../unsaturated_translation_cost_function.h | 50 +++++++++++ src/humap_planner.cpp | 11 ++- src/unsaturated_translation_cost_function.cpp | 89 +++++++++++++++++++ 5 files changed, 153 insertions(+), 1 deletion(-) create mode 100644 include/humap_local_planner/unsaturated_translation_cost_function.h create mode 100644 src/unsaturated_translation_cost_function.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0907915d..0f7daa11 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -212,6 +212,7 @@ add_library(humap_local_planner src/fformation_space_intrusion_cost_function.cpp src/heading_disturbance_cost_function.cpp src/passing_speed_cost_function.cpp + src/unsaturated_translation_cost_function.cpp src/latched_stop_rotate_controller.cpp src/planner_state.cpp ) diff --git a/include/humap_local_planner/humap_planner.h b/include/humap_local_planner/humap_planner.h index db200b1b..f9f63a35 100644 --- a/include/humap_local_planner/humap_planner.h +++ b/include/humap_local_planner/humap_planner.h @@ -16,6 +16,7 @@ #include #include #include +#include //for creating a local cost grid #include @@ -609,6 +610,8 @@ class HumapPlanner { base_local_planner::MapGridCostFunction alignment_costs_; /// Cost function that prefers trajectories that make the nose go towards (local) nose goal base_local_planner::MapGridCostFunction goal_front_costs_; + // Penalizes robot for not saturating the trans. vels - moving slower than the maximum speed allows + UnsaturatedTranslationCostFunction unsaturated_trans_costs_; /// Cost function that prefers forward trajectories instead of those that consist of backward motions base_local_planner::PreferForwardCostFunction backward_costs_; /// Cost function that penalizes trajectories that can cause collision in a longer horizon diff --git a/include/humap_local_planner/unsaturated_translation_cost_function.h b/include/humap_local_planner/unsaturated_translation_cost_function.h new file mode 100644 index 00000000..5d73ed1d --- /dev/null +++ b/include/humap_local_planner/unsaturated_translation_cost_function.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +namespace humap_local_planner { + +/** + * @brief Cost function penalizing the unsaturated translational movements + * + * Penalizes robot's for not keeping its velocity close to the maximum allowable + */ +class UnsaturatedTranslationCostFunction: public base_local_planner::TrajectoryCostFunction { +public: + UnsaturatedTranslationCostFunction(); + + /** + * @brief Updates internal parameters + * + * @param max_trans_vel max translational speed of the robot + * @param compute_whole_horizon set to true if the cost function should be computed for each entry + * of the trajectory + */ + void setParameters( + double max_trans_vel, + double max_vel_x, + double max_vel_y, + bool compute_whole_horizon = true + ); + + /** + * @brief General updating of context values if required. + * Subclasses may overwrite. Return false in case there is any error. + */ + virtual bool prepare() override; + + /** + * @brief Returns a score for trajectory @ref traj + */ + virtual double scoreTrajectory(base_local_planner::Trajectory& traj) override; + +protected: + /// Max translational speed of the robot + double max_trans_vel_; + double max_vel_x_; + double max_vel_y_; + + bool compute_whole_horizon_; +}; + +} // namespace humap_local_planner diff --git a/src/humap_planner.cpp b/src/humap_planner.cpp index bfafc54b..a45531b7 100644 --- a/src/humap_planner.cpp +++ b/src/humap_planner.cpp @@ -71,6 +71,7 @@ HumapPlanner::HumapPlanner( critics.push_back(&goal_costs_); critics.push_back(&alignment_costs_); critics.push_back(&goal_front_costs_); + critics.push_back(&unsaturated_trans_costs_); critics.push_back(&backward_costs_); critics.push_back(&ttc_costs_); critics.push_back(&heading_change_smoothness_costs_); @@ -728,6 +729,7 @@ void HumapPlanner::updateCostParameters() { alignment_costs_.setScale(scales_cm_costs_.alignment_scale); obstacle_costs_.setScale(cfg_->getCost()->occdist_scale); + unsaturated_trans_costs_.setScale(cfg_->getCost()->unsaturated_translation_scale); backward_costs_.setScale(cfg_->getCost()->backward_scale); ttc_costs_.setScale(cfg_->getCost()->ttc_scale); heading_change_smoothness_costs_.setScale(cfg_->getCost()->heading_change_smoothness_scale); @@ -752,6 +754,12 @@ void HumapPlanner::updateCostParameters() { obstacle_costs_.setSumScores(cfg_->getCost()->occdist_sum_scores); goal_front_costs_.setXShift(cfg_->getCost()->forward_point_distance); alignment_costs_.setXShift(cfg_->getCost()->forward_point_distance); + unsaturated_trans_costs_.setParameters( + cfg_->getLimits()->max_vel_trans, + cfg_->getLimits()->max_vel_x, + cfg_->getLimits()->max_vel_y, + cfg_->getCost()->unsaturated_translation_compute_whole_horizon + ); backward_costs_.setPenalty(cfg_->getCost()->backward_penalty); ttc_costs_.setParameters(cfg_->getCost()->ttc_rollout_time, cfg_->getCost()->ttc_collision_distance); heading_disturbance_costs_.setParameters( @@ -1423,7 +1431,7 @@ void HumapPlanner::logTrajectoriesDetails() { invalid_traj || cfg_->getDiagnostics()->log_trajectory_cost_details, "HumapPlanner", "%sExplored trajectory %3d / %3lu cost details: " - "obstacle %5.2f, oscillation %5.2f, path %8.2f, goal %8.2f, goal_front %8.2f, alignment %8.2f, " + "obstacle %5.2f, oscillation %5.2f, path %8.2f, goal %8.2f, goal_front %8.2f, alignment %8.2f, utrans. %8.2f, " "backward %5.2f, TTC %5.2f, HSM %5.2f, vel_smoothness %5.2f, " "head_dir %5.2f, PSI %5.2f, FSI %5.2f, PSPD %5.2f%s", // mark the best trajectory green and all non-feasible red @@ -1436,6 +1444,7 @@ void HumapPlanner::logTrajectoriesDetails() { goal_costs_.getScale() * goal_costs_.scoreTrajectory(traj_copy), goal_front_costs_.getScale() * goal_front_costs_.scoreTrajectory(traj_copy), alignment_costs_.getScale() * alignment_costs_.scoreTrajectory(traj_copy), + unsaturated_trans_costs_.getScale() * unsaturated_trans_costs_.scoreTrajectory(traj_copy), backward_costs_.getScale() * backward_costs_.scoreTrajectory(traj_copy), ttc_costs_.getScale() * ttc_costs_.scoreTrajectory(traj_copy), heading_change_smoothness_costs_.getScale() * heading_change_smoothness_costs_.scoreTrajectory(traj_copy), diff --git a/src/unsaturated_translation_cost_function.cpp b/src/unsaturated_translation_cost_function.cpp new file mode 100644 index 00000000..dea7cf3d --- /dev/null +++ b/src/unsaturated_translation_cost_function.cpp @@ -0,0 +1,89 @@ +#include +#include + +#include + +namespace humap_local_planner { + +UnsaturatedTranslationCostFunction::UnsaturatedTranslationCostFunction(): + max_trans_vel_(0.0), + max_vel_x_(0.0), + max_vel_y_(0.0), + compute_whole_horizon_(true) +{} + +void UnsaturatedTranslationCostFunction::setParameters( + double max_trans_vel, + double max_vel_x, + double max_vel_y, + bool compute_whole_horizon +) { + max_trans_vel_ = max_trans_vel; + max_vel_x_ = max_vel_x; + max_vel_y_ = max_vel_y; + compute_whole_horizon_ = compute_whole_horizon; +} + +bool UnsaturatedTranslationCostFunction::prepare() { + return true; +} + +double UnsaturatedTranslationCostFunction::scoreTrajectory(base_local_planner::Trajectory& traj) { + if (traj.getPointsSize() == 0) { + return 0.0; + } + + // compute differences between the current velocity and maximum + double vx_deviation = 0.0; + double vy_deviation = 0.0; + double vxy_deviation = 0.0; + + // convert to a more friendly type than received + // `false` keeps velocities in the base coordinate system + Trajectory t(traj, false); + + // check if we can compare the current velocity with anything + if (t.getVelocitiesNum() == 0) { + return 0.0; + } + + // helper + auto computeTransVel = [](double vx, double vy) -> double { + return std::hypot(vx, vy); + }; + + // counter to compute mean + size_t vels_num = 0; + + // first velocity of the trajectory is the control command + vx_deviation += std::abs(t.getVelocity(0).getX() - max_vel_x_); + vy_deviation += std::abs(t.getVelocity(0).getY() - max_vel_y_); + double vxy = computeTransVel( + t.getVelocity(0).getX(), + t.getVelocity(0).getY() + ); + vxy_deviation += std::abs(vxy - max_trans_vel_); + vels_num++; + + if (compute_whole_horizon_) { + // iterate over velocities of the trajectory + for (unsigned int sim_step = 1; sim_step < t.getVelocitiesNum(); sim_step++) { + // compute deviation from the currently investigated velocity + vx_deviation += std::abs(t.getVelocity(sim_step).getX() - max_vel_x_); + vy_deviation += std::abs(t.getVelocity(sim_step).getY() - max_vel_y_); + + double vxy = computeTransVel( + t.getVelocity(sim_step).getX(), + t.getVelocity(sim_step).getY() + ); + vxy_deviation += std::abs(vxy - max_trans_vel_); + vels_num++; + } + } + + // divide by trajectory length to make cost function's scale independent of the actual trajectory horizon + double max_deviation = std::max(std::max(vx_deviation, vy_deviation), vxy_deviation); + return (max_deviation) / static_cast(vels_num); +} + +} // namespace humap_local_planner