diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index d2bb6d9c9d..3ca6735e40 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -89,6 +89,7 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/constraint_critic.cpp + src/critics/velocity_deadband_critic.cpp ) set(libraries mppi_controller mppi_critics) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 740dcae1b6..f8a90f1cbc 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -172,6 +172,15 @@ Note: There is a "Legacy" version of this critic also available with the same pa | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | + +#### Velocity Deadband Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 35.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. | + + ### XML configuration example ``` controller_server: @@ -262,6 +271,11 @@ controller_server: threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] # TwirlingCritic: # enabled: true # twirling_cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index d578d23a9e..6085dbec88 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -45,5 +45,9 @@ mppi critic for incentivizing moving within kinematic and dynamic bounds + + mppi critic for restricting command velocities in deadband range + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp new file mode 100644 index 0000000000..76e1dbd670 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp @@ -0,0 +1,54 @@ +// 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__VELOCITY_DEADBAND_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ + +#include + +#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::VelocityDeadbandCritic + * @brief Critic objective function for enforcing feasible constraints + */ +class VelocityDeadbandCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to goal following + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + std::vector deadband_velocities_{0.0f, 0.0f, 0.0f}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp new file mode 100644 index 0000000000..84d3aba303 --- /dev/null +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -0,0 +1,104 @@ +// 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/velocity_deadband_critic.hpp" + +namespace mppi::critics +{ + +void VelocityDeadbandCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 35.0); + + // Recast double to float + std::vector deadband_velocities{0.0, 0.0, 0.0}; + getParam(deadband_velocities, "deadband_velocities", std::vector{0.0, 0.0, 0.0}); + std::transform( + deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(), + [](double d) {return static_cast(d);}); + + RCLCPP_INFO_STREAM( + logger_, "VelocityDeadbandCritic instantiated with " + << power_ << " power, " << weight_ << " weight, deadband_velocity [" + << deadband_velocities_.at(0) << "," << deadband_velocities_.at(1) << "," + << deadband_velocities_.at(2) << "]"); +} + +void VelocityDeadbandCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + + if (!enabled_) { + return; + } + + auto & vx = data.state.vx; + auto & wz = data.state.wz; + + if (data.motion_model->isHolonomic()) { + auto & vy = data.state.vy; + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; + } + + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index af2a61e20e..513a018d27 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -31,6 +31,7 @@ #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" #include "nav2_mppi_controller/critics/twirling_critic.hpp" +#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" #include "utils_test.cpp" // NOLINT // Tests the various critic plugin functions @@ -609,106 +610,51 @@ TEST(CriticTests, PathAlignCritic) EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } -TEST(CriticTests, PathAlignLegacyCritic) +TEST(CriticTests, VelocityDeadbandCritic) { // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", "dummy_costmap"); ParametersHandler param_handler(node); + auto getParam = param_handler.getParamGetter("critic"); + std::vector deadband_velocities_; + getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); 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; + data.motion_model = std::make_shared(); // Initialization testing - // Make sure initializes correctly - PathAlignLegacyCritic critic; + // Make sure initializes correctly and that defaults are reasonable + VelocityDeadbandCritic 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}); + // provide velocities out of deadband bounds, should not have any costs + state.vx = 0.80 * xt::ones({1000, 30}); + state.vy = 0.60 * xt::ones({1000, 30}); + state.wz = 0.80 * 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 - // 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); - } - } + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); - 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}); + // Test cost value + state.vx = 0.01 * xt::ones({1000, 30}); + state.vy = 0.02 * xt::ones({1000, 30}); + state.wz = 0.021 * xt::ones({1000, 30}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 + EXPECT_NEAR(costs(1), 19.845, 0.01); }