Implement Critic for Velocity Deadband Hardware Constraints (ros-navigation#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 <[email protected]>

* Remove velocity deadband critic from defaults

Signed-off-by: Denis Sokolov <[email protected]>

* remove old weight

Signed-off-by: Denis Sokolov <[email protected]>

* fix velocity deadband critic tests

Signed-off-by: Denis Sokolov <[email protected]>

---------

Signed-off-by: Denis Sokolov <[email protected]>

Author: perchess

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)

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

nav2_mppi_controller/critics.xml

@@ -41,5 +41,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
@@ -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<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);
+}