diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index 02590490199..95eabf6c411 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -88,6 +88,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 893ed426a56..d061958135c 100644
--- a/nav2_mppi_controller/README.md
+++ b/nav2_mppi_controller/README.md
@@ -171,6 +171,15 @@ Uses inflated costmap cost directly to avoid obstacles
| 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 2e7d110338b..deae2c11cb9 100644
--- a/nav2_mppi_controller/critics.xml
+++ b/nav2_mppi_controller/critics.xml
@@ -41,5 +41,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 00000000000..76e1dbd670f
--- /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 00000000000..84d3aba303b
--- /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 edc1c8dfb27..ef85ab293de 100644
--- a/nav2_mppi_controller/test/critics_tests.cpp
+++ b/nav2_mppi_controller/test/critics_tests.cpp
@@ -30,6 +30,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
@@ -607,3 +608,52 @@ TEST(CriticTests, PathAlignCritic)
critic.score(data);
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
}
+
+TEST(CriticTests, VelocityDeadbandCritic)
+{
+ // Standard preamble
+ auto node = std::make_shared("my_node");
+ auto costmap_ros = std::make_shared(
+ "dummy_costmap", "", "dummy_costmap", true);
+ 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;
+ models::ControlSequence control_sequence;
+ models::Trajectories generated_trajectories;
+ 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();
+
+ // Initialization testing
+
+ // 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 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);
+ EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
+
+ // 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);
+ // 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);
+}