From b655d6342bfa9d100307002f339af745e440c8a6 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 15 Jun 2023 19:47:31 -0700 Subject: [PATCH 1/2] fixing path angle critic's non-directional bias --- .../src/critics/path_angle_critic.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 1225ccde83f..b7f18e3c274 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -88,10 +88,13 @@ void PathAngleCritic::score(CriticData & data) xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); if (reversing_allowed_ && !forward_preference_) { - data.costs += xt::pow( - xt::mean( - xt::where(yaws < M_PI_2, yaws, utils::normalize_angles(yaws + M_PI)), - {1}, immediate) * weight_, power_); + const auto yaws_between_points_corrected = xt::where( + yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance( + data.trajectories.yaws, + yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); } else { data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); } From 8d80562688c4c28d1d44117e25597b93d6f20706 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 21 Jun 2023 14:58:44 -0700 Subject: [PATCH 2/2] adding reformat --- .../include/nav2_mppi_controller/tools/utils.hpp | 2 +- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 818d1fe357b..4d2295b6260 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -432,7 +432,7 @@ inline double posePointAngle( if (!forward_preference) { return std::min( abs(angles::shortest_angular_distance(yaw, pose_yaw)), - abs(angles::shortest_angular_distance(yaw, pose_yaw + M_PI))); + abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); } return abs(angles::shortest_angular_distance(yaw, pose_yaw)); diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index b7f18e3c274..85cae5838d3 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -91,9 +91,7 @@ void PathAngleCritic::score(CriticData & data) const auto yaws_between_points_corrected = xt::where( yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); const auto corrected_yaws = xt::abs( - utils::shortest_angular_distance( - data.trajectories.yaws, - yaws_between_points_corrected)); + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); } else { data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_);