planner - added UnsaturatedTranslation cost function [#108]

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
 )

include/humap_local_planner/humap_planner.h

@@ -16,6 +16,7 @@
 #include <humap_local_planner/personal_space_intrusion_cost_function.h>
 #include <humap_local_planner/fformation_space_intrusion_cost_function.h>
 #include <humap_local_planner/passing_speed_cost_function.h>
+#include <humap_local_planner/unsaturated_translation_cost_function.h>
 
 //for creating a local cost grid
 #include <base_local_planner/map_grid_visualizer.h>
@@ -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

include/humap_local_planner/unsaturated_translation_cost_function.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <base_local_planner/trajectory_cost_function.h>
+
+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

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),

src/unsaturated_translation_cost_function.cpp

@@ -0,0 +1,89 @@
+#include <humap_local_planner/unsaturated_translation_cost_function.h>
+#include <humap_local_planner/trajectory.h>
+
+#include <cmath>
+
+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<double>(vels_num);
+}
+
+} // namespace humap_local_planner