Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Path Angle Critic for bi-directionality #3454

Closed
wants to merge 3 commits into from
Closed

Fix Path Angle Critic for bi-directionality #3454

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e76f412
PoC
Mar 7, 2023
896d156
abs backward
Mar 8, 2023
46804c5
change bi directional method
Mar 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 8 additions & 1 deletion nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -418,13 +418,20 @@ inline void setPathCostsIfNotSet(
* @param point_y Point to find angle relative to Y axis
* @return Angle between two points
*/
inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y)
inline double posePointAngle(
const geometry_msgs::msg::Pose & pose, double point_x, double point_y,
double point_yaw)
{
double pose_x = pose.position.x;
double pose_y = pose.position.y;
double pose_yaw = tf2::getYaw(pose.orientation);

double yaw = atan2(point_y - pose_y, point_x - pose_x);

if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) {
Copy link
Member

@SteveMacenski SteveMacenski Mar 9, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not a bad solution, but again I think this requires explicit parameterization if this is allowable. If you have a robot system with a preference not to reverse if possible, then we don't want to set the angle this way. Else, we should be using the actual orientation. It also feels weird to me that you're hiding this detail into a generic utility which is just supposed to be finding the angle between a point and a pose. This feels wrong to have this logic here, that's not the role of this function.

Copy link
Member

@SteveMacenski SteveMacenski Mar 9, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

... Or maybe as long as moving backward is possible I suppose the other critic should take care of that, but it needs to be tested to make sure this doesn't incentivize more backward motion.

The main concern I see is if a user literally has min_vel_x set to 0.0 so it cannot at at all move backward, this would incentivize reversing motions that are not possible at all rather than fully using the force of the purpose of this plugin to make the robot do a full 180 turn to get to its desired forward-facing heading.

So I think a parameter is needed for the situation, at minimum, where reversing is 100% unequivocally impossible by the users' settings. The question I then have is if we should only apply it in that situation or if we should also apply it when we would prefer not to move backwards. I'm just not entirely sure from a quick glance - I'd need to think about it and probably test.

There's definitely reason to make sure that the PathAngle doesn't use the inverted orientation though - when reversing is impossible. When it is possible but is weighted not to do so, that's when I'm not sure what we should do exactly here.

yaw = angles::normalize_angle(yaw + M_PI);
}

return abs(angles::shortest_angular_distance(yaw, pose_yaw));
}

Expand Down
18 changes: 16 additions & 2 deletions nav2_mppi_controller/src/critics/path_angle_critic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,16 +57,30 @@ void PathAngleCritic::score(CriticData & data)

const float goal_x = xt::view(data.path.x, offseted_idx);
const float goal_y = xt::view(data.path.y, offseted_idx);
const float goal_yaw = xt::view(data.path.yaws, offseted_idx);

if (utils::posePointAngle(data.state.pose.pose, goal_x, goal_y) < max_angle_to_furthest_) {
if (utils::posePointAngle(
data.state.pose.pose, goal_x, goal_y,
goal_yaw) < max_angle_to_furthest_)
{
return;
}

const auto yaws_between_points = xt::atan2(
goal_y - data.trajectories.y,
goal_x - data.trajectories.x);

const auto yaws_between_points_corrected = xt::eval(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think I made this same comment before, this is doing elementwise selection of which orientation to use by what's the most convenient, that's not a valid application of a critic function I don't believe. I think we have to decide which orientation we're trying to achieve and penalize applicable to that. Else, some trajectories in a set will be penalized by different criteria than others. I have to think about that a little bit though, maybe it is OK to do elementwise but it feels like that needs to be really well thought out, not on a whim.

If we have a system preferring forward, then only forward should be considered. If we have a system where reversing is viable and we're reversing or wanting to reverse, then it makes sense to use the inverted orientation. If we have a system where reversing is viable and we're not reversing or wanting to reverse, then it makes sense to use the real orientation.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm thinking it might be OK to do elementwise as long as a trajectory is generally going forward or reverse -- but what happens if we have a backup maneuver where the trajectory legitimately has a portion of the path which is backing up and another section that is moving forward within the time horizon? In that situation, we'd be applying different orientation direction penalties which makes my head hurt in trying to figure out if that's a good idea or not

xt::where(
xt::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) <= M_PI_2,
yaws_between_points,
utils::normalize_angles(yaws_between_points + M_PI)));

const auto yaws =
xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points));
xt::abs(
utils::shortest_angular_distance(
data.trajectories.yaws,
yaws_between_points_corrected));

data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_);
}
Expand Down