Implement Critic for Velocity Deadband Hardware Constraints (#4256)

* Adding new velocity deadband critic.

- add some tests
- cast double to float
- add new features from "main" branch

- fix formating

- add cost test
- fix linting issue
- add README

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* Remove velocity deadband critic from defaults

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* remove old weight

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

* fix velocity deadband critic tests

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

---------

Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>

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)

nav2_mppi_controller/README.md

@@ -174,6 +174,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:
@@ -264,6 +273,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

nav2_mppi_controller/critics.xml

@@ -45,5 +45,9 @@
  <description>mppi critic for incentivizing moving within kinematic and dynamic bounds</description>
  </class>
 
+ <class type="mppi::critics::VelocityDeadbandCritic" base_class_type="mppi::critics::CriticFunction">
+ <description>mppi critic for restricting command velocities in deadband range</description>
+ </class>
+
  </library>
 </class_libraries>

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 <vector>
+
+#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<float> deadband_velocities_{0.0f, 0.0f, 0.0f};
+};
+
+} // namespace mppi::critics
+
+#endif // NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_

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<double> deadband_velocities{0.0, 0.0, 0.0};
+ getParam(deadband_velocities, "deadband_velocities", std::vector<double>{0.0, 0.0, 0.0});
+ std::transform(
+ deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(),
+ [](double d) {return static_cast<float>(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/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction)

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
@@ -639,3 +640,52 @@ TEST(CriticTests, PathAlignLegacyCritic)
  critic.score(data);
  EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
 }
+
+TEST(CriticTests, VelocityDeadbandCritic)
+{
+ // Standard preamble
+ auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+ auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+ "dummy_costmap", "", "dummy_costmap", true);
+ ParametersHandler param_handler(node);
+ auto getParam = param_handler.getParamGetter("critic");
+ std::vector<double> deadband_velocities_;
+ getParam(deadband_velocities_, "deadband_velocities", std::vector<double>{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<float, 1> costs = xt::zeros<float>({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<OmniMotionModel>();
+
+ // Initialization testing
+
+ // Make sure initializes correctly and that defaults are reasonable
+ VelocityDeadbandCritic critic;
+ critic.on_configure(node, "mppi", "critic", costmap_ros, &param_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<float>({1000, 30});
+ state.vy = 0.60 * xt::ones<float>({1000, 30});
+ state.wz = 0.80 * xt::ones<float>({1000, 30});
+ critic.score(data);
+ EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
+
+ // Test cost value
+ state.vx = 0.01 * xt::ones<float>({1000, 30});
+ state.vy = 0.02 * xt::ones<float>({1000, 30});
+ state.wz = 0.021 * xt::ones<float>({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);
+}