planner - added SlowTranslation cost function [#108]

CMakeLists.txt

@@ -208,6 +208,7 @@ add_library(hubero_local_planner
     src/fformation_space_intrusion_cost_function.cpp
     src/heading_disturbance_cost_function.cpp
     src/passing_speed_cost_function.cpp
+    src/slow_translation_cost_function.cpp
     src/latched_stop_rotate_controller.cpp
     src/planner_state.cpp
 )

include/hubero_local_planner/hubero_planner.h

@@ -15,6 +15,7 @@
 #include <hubero_local_planner/personal_space_intrusion_cost_function.h>
 #include <hubero_local_planner/fformation_space_intrusion_cost_function.h>
 #include <hubero_local_planner/passing_speed_cost_function.h>
+#include <hubero_local_planner/slow_translation_cost_function.h>
 
 //for creating a local cost grid
 #include <base_local_planner/map_grid_visualizer.h>
@@ -558,6 +559,8 @@ class HuberoPlanner {
 	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 moving slower than the maximum speed allows
+	SlowTranslationCostFunction slow_translation_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

include/hubero_local_planner/slow_translation_cost_function.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <base_local_planner/trajectory_cost_function.h>
+
+namespace hubero_local_planner {
+
+/**
+ * @brief Slow Translational Movement cost function
+ *
+ * Penalizes robot's for not keeping its velocity close to the maximum allowable
+ */
+class SlowTranslationCostFunction: public base_local_planner::TrajectoryCostFunction {
+public:
+	SlowTranslationCostFunction() = default;
+
+	/**
+	 * @brief Updates internal parameters
+	 *
+	 * @param max_trans_vel max translational speed of the robot
+	 */
+	void setParameters(double max_trans_vel, double max_vel_x, double max_vel_y);
+
+	/**
+	 * @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_;
+};
+
+} // namespace hubero_local_planner

src/hubero_planner.cpp

@@ -66,6 +66,7 @@ HuberoPlanner::HuberoPlanner(
 	critics.push_back(&goal_costs_);
 	critics.push_back(&alignment_costs_);
 	critics.push_back(&goal_front_costs_);
+	critics.push_back(&slow_translation_costs_);
 	critics.push_back(&backward_costs_);
 	critics.push_back(&ttc_costs_);
 	critics.push_back(&heading_change_smoothness_costs_);
@@ -723,6 +724,7 @@ void HuberoPlanner::updateCostParameters() {
 	alignment_costs_.setScale(scales_cm_costs_.alignment_scale);
 
 	obstacle_costs_.setScale(cfg_->getCost()->occdist_scale);
+	slow_translation_costs_.setScale(cfg_->getCost()->slow_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);
@@ -745,6 +747,11 @@ void HuberoPlanner::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);
+	slow_translation_costs_.setParameters(
+		cfg_->getLimits()->max_vel_trans,
+		cfg_->getLimits()->max_vel_x,
+		cfg_->getLimits()->max_vel_y
+	);
 	backward_costs_.setPenalty(cfg_->getCost()->backward_penalty);
 	ttc_costs_.setParameters(cfg_->getCost()->ttc_rollout_time, cfg_->getCost()->ttc_collision_distance);
 	heading_disturbance_costs_.setParameters(
@@ -1261,7 +1268,7 @@ void HuberoPlanner::logTrajectoriesDetails() {
 			cfg_->getDiagnostics()->log_trajectory_cost_details,
 			"HuberoPlanner",
 			"%sExplored trajectory %3d / %3lu cost details: "
-			"obstacle %2.2f, oscillation %2.2f, path %2.2f, goal %2.2f, goal_front %2.2f, alignment %2.2f, "
+			"obstacle %2.2f, oscillation %2.2f, path %2.2f, goal %2.2f, goal_front %2.2f, alignment %2.2f, slow %2.2f, "
 			"backward %2.2f, TTC %2.2f, HSM %2.2f, vel_smoothness %2.2f, "
 			"head_dir %2.2f, PSI %2.2f, FSI %2.2f, PSPD %2.2f%s",
 			result_traj_.cost_ == traj.cost_ ? "\033[32m" : "", // mark the best trajectory green
@@ -1273,6 +1280,7 @@ void HuberoPlanner::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),
+			slow_translation_costs_.getScale() * slow_translation_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),

src/slow_translation_cost_function.cpp

@@ -0,0 +1,63 @@
+#include <hubero_local_planner/slow_translation_cost_function.h>
+#include <hubero_local_planner/trajectory.h>
+
+#include <cmath>
+
+namespace hubero_local_planner {
+
+void SlowTranslationCostFunction::setParameters(double max_trans_vel, double max_vel_x, double max_vel_y) {
+	max_trans_vel_ = max_trans_vel;
+	max_vel_x_ = max_vel_x;
+	max_vel_y_ = max_vel_y;
+}
+
+bool SlowTranslationCostFunction::prepare() {
+	return true;
+}
+
+double SlowTranslationCostFunction::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);
+	};
+
+	// first velocity of the trajectory is the control command, let's compare it with the current velocity
+	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_);
+
+	// iterate over trajectory points (velocity must be computed)
+	for (unsigned int sim_step = 0; sim_step < t.getVelocitiesNum(); sim_step++) {
+		// compute deviation from the current 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(0).getX(), t.getVelocity(0).getY());
+		vxy_deviation += std::abs(vxy - max_trans_vel_);
+	}
+
+	// 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<double>(t.getVelocitiesNum());
+}
+
+} // namespace hubero_local_planner