From 7009ffba5f85c50ac97fd0057924b0f1447c5e85 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 13 Oct 2023 18:24:04 -0700 Subject: [PATCH] [MPPI] Reworked Path Align Critic; 70% faster + Tracks Paths Better! Edit: strike that, now 80% (#3872) * adding regenerate noise param + adding docs * fix tests * remove unnecessary normalization * Update optimizer.cpp * adding refactored path alignment critic * fix visualization bug * speed up another 30% * remove a little jitter * a few more small optimizaitons * fixing unit tests * retain legacy critic * adding tests for legacy --- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/LICENSE.md | 1 + nav2_mppi_controller/README.md | 2 + nav2_mppi_controller/critics.xml | 4 + .../critics/path_align_critic.hpp | 2 +- .../critics/path_align_legacy_critic.hpp | 60 ++++++++ .../nav2_mppi_controller/tools/utils.hpp | 27 +++- .../src/critics/path_align_critic.cpp | 84 ++++++----- .../src/critics/path_align_legacy_critic.cpp | 137 ++++++++++++++++++ nav2_mppi_controller/src/optimizer.cpp | 2 +- nav2_mppi_controller/test/critics_tests.cpp | 107 +++++++++++++- 11 files changed, 380 insertions(+), 47 deletions(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index af64e1b1ed..29dfde9672 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -78,6 +78,7 @@ add_library(mppi_critics SHARED src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp + src/critics/path_align_legacy_critic.cpp src/critics/path_follow_critic.cpp src/critics/path_angle_critic.cpp src/critics/prefer_forward_critic.cpp diff --git a/nav2_mppi_controller/LICENSE.md b/nav2_mppi_controller/LICENSE.md index da24c0064f..57a5719427 100644 --- a/nav2_mppi_controller/LICENSE.md +++ b/nav2_mppi_controller/LICENSE.md @@ -2,6 +2,7 @@ MIT License Copyright (c) 2021-2022 Fast Sense Studio Copyright (c) 2022-2023 Samsung Research America +Copyright (c) 2023 Open Navigation LLC Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 3872b72610..13c314205b 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -119,6 +119,8 @@ This process is then repeated a number of times and returns a converged solution | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | +Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023. + #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 669944f2fe..790568e796 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -17,6 +17,10 @@ mppi critic for aligning to path + + mppi critic for aligning to path (legacy) + + mppi critic for tracking the path in the correct heading diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index a549995557..f7ad2fda6a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp new file mode 100644 index 0000000000..65d9d0d61e --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::PathAlignLegacyCritic + * @brief Critic objective function for aligning to the path. Note: + * High settings of this will follow the path more precisely, but also makes it + * difficult (or impossible) to deviate in the presence of dynamic obstacles. + * This is an important critic to tune and consider in tandem with Obstacle. + * This is the initial 'Legacy' implementation before replacement Oct 2023. + */ +class PathAlignLegacyCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to trajectories path alignment + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + size_t offset_from_furthest_{0}; + int trajectory_point_step_{0}; + float threshold_to_consider_{0}; + float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ 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 6baa27873e..4a67bc276c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -308,14 +308,16 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) const auto dists = dx * dx + dy * dy; - size_t max_id_by_trajectories = 0; + size_t max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); + float cur_dist = 0.0f; for (size_t i = 0; i < dists.shape(0); i++) { - size_t min_id_by_path = 0; + min_id_by_path = 0; for (size_t j = 0; j < dists.shape(1); j++) { - if (dists(i, j) < min_distance_by_path) { - min_distance_by_path = dists(i, j); + cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; min_id_by_path = j; } } @@ -688,6 +690,23 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) return first_after_inversion; } +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + */ +inline size_t findClosestPathPt(const std::vector & vec, float dist, size_t init = 0) +{ + auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist); + if (iter == vec.begin()) { + return 0; + } + if (dist - *(iter - 1) < *iter - dist) { + return iter - 1 - vec.begin(); + } + return iter - vec.begin(); +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 4ff12d4e55..4b4d93abf3 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -54,7 +54,8 @@ void PathAlignCritic::score(CriticData & data) // Don't apply when first getting bearing w.r.t. the path utils::setPathFurthestPointIfNotSet(data); - if (*data.furthest_reached_path_point < offset_from_furthest_) { + const size_t path_segments_count = *data.furthest_reached_path_point; // up to furthest only + if (path_segments_count < offset_from_furthest_) { return; } @@ -70,59 +71,62 @@ void PathAlignCritic::score(CriticData & data) } } - const auto & T_x = data.trajectories.x; - const auto & T_y = data.trajectories.y; - const auto & T_yaw = data.trajectories.yaws; - const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points - const size_t batch_size = T_x.shape(0); - const size_t time_steps = T_x.shape(1); - const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); - const size_t path_segments_count = data.path.x.shape(0) - 1; + const size_t batch_size = data.trajectories.x.shape(0); + const size_t time_steps = data.trajectories.x.shape(1); auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); - if (path_segments_count < 1) { - return; + // Find integrated distance in the path + std::vector path_integrated_distances(path_segments_count, 0.0f); + float dx = 0.0f, dy = 0.0f; + for (unsigned int i = 1; i != path_segments_count; i++) { + dx = P_x(i) - P_x(i - 1); + dy = P_y(i) - P_y(i - 1); + float curr_dist = sqrtf(dx * dx + dy * dy); + path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist; } - float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; - float min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; - + float traj_integrated_distance = 0.0f; + float summed_path_dist = 0.0f, dyaw = 0.0f; + float num_samples = 0.0f; + float Tx = 0.0f, Ty = 0.0f; + size_t path_pt = 0; for (size_t t = 0; t < batch_size; ++t) { - summed_dist = 0.0f; + traj_integrated_distance = 0.0f; + summed_path_dist = 0.0f; + num_samples = 0.0f; + const auto T_x = xt::view(data.trajectories.x, t, xt::all()); + const auto T_y = xt::view(data.trajectories.y, t, xt::all()); for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - min_dist_sq = std::numeric_limits::max(); - min_s = 0; - - // Find closest path segment to the trajectory point - for (size_t s = 0; s < path_segments_count - 1; s++) { - xt::xtensor_fixed> P; - dx = P_x(s) - T_x(t, p); - dy = P_y(s) - T_y(t, p); - if (use_path_orientations_) { - dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); - dist_sq = dx * dx + dy * dy + dyaw * dyaw; - } else { - dist_sq = dx * dx + dy * dy; - } - if (dist_sq < min_dist_sq) { - min_dist_sq = dist_sq; - min_s = s; - } - } + Tx = T_x(p); + Ty = T_y(p); + dx = Tx - T_x(p - trajectory_point_step_); + dy = Ty - T_y(p - trajectory_point_step_); + traj_integrated_distance += sqrtf(dx * dx + dy * dy); + path_pt = utils::findClosestPathPt( + path_integrated_distances, traj_integrated_distance, path_pt); // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles - if (min_s != 0 && (*data.path_pts_valid)[min_s]) { - summed_dist += sqrtf(min_dist_sq); + if ((*data.path_pts_valid)[path_pt]) { + dx = P_x(path_pt) - Tx; + dy = P_y(path_pt) - Ty; + num_samples += 1.0f; + if (use_path_orientations_) { + const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all()); + dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p)); + summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); + } else { + summed_path_dist += sqrtf(dx * dx + dy * dy); + } } } - - cost[t] = summed_dist / traj_pts_eval; + if (num_samples > 0) { + cost[t] = summed_path_dist / num_samples; + } } data.costs += xt::pow(std::move(cost) * weight_, power_); diff --git a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp new file mode 100644 index 0000000000..a04f4a9fa9 --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" + +#include +#include + +namespace mppi::critics +{ + +using namespace xt::placeholders; // NOLINT +using xt::evaluation_strategy::immediate; + +void PathAlignLegacyCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 10.0); + + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(trajectory_point_step_, "trajectory_point_step", 4); + getParam( + threshold_to_consider_, + "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); + + RCLCPP_INFO( + logger_, + "ReferenceTrajectoryCritic instantiated with %d power and %f weight", + power_, weight_); +} + +void PathAlignLegacyCritic::score(CriticData & data) +{ + // Don't apply close to goal, let the goal critics take over + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { + return; + } + + // Don't apply when first getting bearing w.r.t. the path + utils::setPathFurthestPointIfNotSet(data); + if (*data.furthest_reached_path_point < offset_from_furthest_) { + return; + } + + // Don't apply when dynamic obstacles are blocking significant proportions of the local path + utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data); + unsigned int invalid_ctr = 0; + const float range = *data.furthest_reached_path_point - closest_initial_path_point; + for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) { + if (!(*data.path_pts_valid)[i]) {invalid_ctr++;} + if (static_cast(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) { + return; + } + } + + const auto & T_x = data.trajectories.x; + const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; + + const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points + const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points + + const size_t batch_size = T_x.shape(0); + const size_t time_steps = T_x.shape(1); + const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); + const size_t path_segments_count = data.path.x.shape(0) - 1; + auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); + + if (path_segments_count < 1) { + return; + } + + float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; + float min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + + for (size_t t = 0; t < batch_size; ++t) { + summed_dist = 0.0f; + for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { + min_dist_sq = std::numeric_limits::max(); + min_s = 0; + + // Find closest path segment to the trajectory point + for (size_t s = 0; s < path_segments_count - 1; s++) { + xt::xtensor_fixed> P; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; + min_s = s; + } + } + + // The nearest path point to align to needs to be not in collision, else + // let the obstacle critic take over in this region due to dynamic obstacles + if (min_s != 0 && (*data.path_pts_valid)[min_s]) { + summed_dist += sqrtf(min_dist_sq); + } + } + + cost[t] = summed_dist / traj_pts_eval; + } + + data.costs += xt::pow(std::move(cost) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::PathAlignLegacyCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 1dd8420230..3b053f4577 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -279,7 +279,7 @@ void Optimizer::integrateStateVelocities( auto traj_y = xt::view(trajectory, xt::all(), 1); auto traj_yaws = xt::view(trajectory, xt::all(), 2); - xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0 + initial_yaw); + xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw; auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 8b5efae8da..f88249aed1 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -25,6 +25,7 @@ #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" +#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" @@ -586,7 +587,111 @@ TEST(CriticTests, PathAlignCritic) path.x(21) = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); - // 0.04 * 1000 * 10 weight * 4 num pts eval / 4 normalization term + // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term + EXPECT_NEAR(xt::sum(costs, immediate)(), 6600.0, 1e-2); + + // provide state pose and path far enough to enable, with data to pass condition + // but path is blocked in collision + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m + for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m + costmap->setCost(i, j, 254); + } + } + + data.path_pts_valid.reset(); // Recompute on new path + costs = xt::zeros({1000}); + path.x = 1.5 * xt::ones({22}); + path.y = 1.5 * xt::ones({22}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); +} + +TEST(CriticTests, PathAlignLegacyCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathAlignLegacyCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 0.85; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable + // but data furthest point reached is 0 and offset default is 20, so returns + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // but with empty trajectories and paths, should still be zero + *data.furthest_reached_path_point = 21; + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // and with a valid path to pass invalid path condition + state.pose.pose.position.x = 0.0; + data.path_pts_valid.reset(); // Recompute on new path + path.reset(22); + path.x(0) = 0; + path.x(1) = 0.1; + path.x(2) = 0.2; + path.x(3) = 0.3; + path.x(4) = 0.4; + path.x(5) = 0.5; + path.x(6) = 0.6; + path.x(7) = 0.7; + path.x(8) = 0.8; + path.x(9) = 0.9; + path.x(10) = 0.9; + path.x(11) = 0.9; + path.x(12) = 0.9; + path.x(13) = 0.9; + path.x(14) = 0.9; + path.x(15) = 0.9; + path.x(16) = 0.9; + path.x(17) = 0.9; + path.x(18) = 0.9; + path.x(19) = 0.9; + path.x(20) = 0.9; + path.x(21) = 0.9; + generated_trajectories.x = 0.66 * xt::ones({1000, 30}); + critic.score(data); + // 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); // provide state pose and path far enough to enable, with data to pass condition