Merge pull request #99 from artofnothingness/infomatics

adding gamma term on noise smoothing to base implementation (ready for review)

CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(critics SHARED
   src/critics/path_angle_critic.cpp
   src/critics/prefer_forward_critic.cpp
   src/critics/twirling_critic.cpp
+  src/critics/constraint_critic.cpp
 )
 
 set(libraries mppic critics)
@@ -80,7 +81,7 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
   add_subdirectory(test)
-  add_subdirectory(benchmark)
+  # add_subdirectory(benchmark)
 endif()
 
 ament_export_libraries(${libraries})

benchmark/controller_benchmark.cpp

@@ -1,4 +1,16 @@
-// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research
+// 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 <benchmark/benchmark.h>
 #include <string>
@@ -34,7 +46,6 @@ void prepareAndRunBenchmark(
   bool consider_footprint, std::string motion_model,
   std::vector<std::string> critics, benchmark::State & state)
 {
-
   bool visualize = false;
 
   int batch_size = 300;

benchmark/optimizer_benchmark.cpp

@@ -1,4 +1,16 @@
-// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research
+// 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 <benchmark/benchmark.h>
 #include <string>
@@ -79,7 +91,6 @@ void prepareAndRunBenchmark(
   for (auto _ : state) {
     optimizer->evalControl(pose, velocity, path, dummy_goal_checker);
   }
-
 }
 
 static void BM_DiffDrivePointFootprint(benchmark::State & state)

critics.xml

@@ -33,6 +33,9 @@
       <description>mppi critic for preventing twirling behavior when using omnidirectional models</description>
     </class>
 
+    <class type="mppi::critics::ConstraintCritic" base_class_type="mppi::critics::CriticFunction">
+      <description>mppi critic for incentivizing moving within kinematic and dynamic bounds</description>
+    </class>
 
   </library>
 </class_libraries>

include/mppic/controller.hpp

@@ -1,4 +1,17 @@
-// Copyright 2022 FastSense, Samsung Research
+// 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 MPPIC__CONTROLLER_HPP_
 #define MPPIC__CONTROLLER_HPP_
 
@@ -18,30 +31,79 @@
 namespace mppi
 {
 
-class Controller : public nav2_core::Controller
+/**
+ * @class mppi::MPPIController
+ * @brief Main plugin controller for MPPI Controller
+ */
+class MPPIController : public nav2_core::Controller
 {
 public:
-  Controller() = default;
-
+  /**
+    * @brief Constructor for mppi::MPPIController
+    */
+  MPPIController() = default;
+
+  /**
+    * @brief Configure controller on bringup
+    * @param parent WeakPtr to node
+    * @param name Name of plugin
+    * @param tf TF buffer to use
+    * @param costmap_ros Costmap2DROS object of environment
+    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
     const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
 
+  /**
+    * @brief Cleanup resources
+    */
   void cleanup() override;
+
+  /**
+    * @brief Activate controller
+    */
   void activate() override;
+
+  /**
+    * @brief Deactivate controller
+    */
   void deactivate() override;
 
+  /**
+    * @brief Reset the controller state between tasks
+    */
+  void reset() override;
+
+  /**
+    * @brief Main method to compute velocities using the optimizer
+    * @param robot_pose Robot pose
+    * @param robot_speed Robot speed
+    * @param goal_checker Pointer to the goal checker for awareness if completed task
+    */
   geometry_msgs::msg::TwistStamped computeVelocityCommands(
     const geometry_msgs::msg::PoseStamped & robot_pose,
     const geometry_msgs::msg::Twist & robot_speed,
     nav2_core::GoalChecker * goal_checker) override;
 
+  /**
+    * @brief Set new reference path to track
+    * @param path Path to track
+    */
   void setPlan(const nav_msgs::msg::Path & path) override;
 
+  /**
+    * @brief Set new speed limit from callback
+    * @param speed_limit Speed limit to use
+    * @param percentage Bool if the speed limit is absolute or relative
+    */
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
 protected:
+  /**
+    * @brief Visualize trajectories
+    * @param transformed_plan Transformed input plan
+    */
   void visualize(nav_msgs::msg::Path transformed_plan);
 
   std::string name_;

include/mppic/critic_data.hpp

@@ -1,19 +1,40 @@
-// Copyright 2022 @artofnothingness Aleksei Budyakov, Samsung Research
-
-#pragma once
-
+// 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 MPPIC__CRITIC_DATA_HPP_
+#define MPPIC__CRITIC_DATA_HPP_
+
+#include <memory>
+#include <vector>
 #include <xtensor/xtensor.hpp>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_core/goal_checker.hpp"
 #include "mppic/models/state.hpp"
 #include "mppic/models/trajectories.hpp"
 #include "mppic/models/path.hpp"
+#include "mppic/motion_models.hpp"
 
 
 namespace mppi
 {
 
+/**
+ * @struct mppi::CriticData
+ * @brief Data to pass to critics for scoring, including state, trajectories, path, costs, and
+ * important parameters to share
+ */
 struct CriticData
 {
   const models::State & state;
@@ -25,7 +46,11 @@ struct CriticData
 
   bool fail_flag;
   nav2_core::GoalChecker * goal_checker;
+  std::shared_ptr<MotionModel> motion_model;
+  std::optional<std::vector<bool>> path_pts_valid;
   std::optional<size_t> furthest_reached_path_point;
 };
 
-}  // namespace mppi::models
+}  // namespace mppi
+
+#endif  // MPPIC__CRITIC_DATA_HPP_

include/mppic/critic_function.hpp

@@ -1,4 +1,17 @@
-// Copyright 2022 artofnothingness Alexey Budyakov, Samsung Research
+// 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 MPPIC__CRITIC_FUNCTION_HPP_
 #define MPPIC__CRITIC_FUNCTION_HPP_
 
@@ -13,21 +26,53 @@
 
 namespace mppi::critics
 {
+
+/**
+ * @class mppi::critics::CollisionCost
+ * @brief Utility for storing cost information
+ */
+struct CollisionCost
+{
+  float cost{0};
+  bool using_footprint{false};
+};
+
+/**
+ * @class mppi::critics::CriticFunction
+ * @brief Abstract critic objective function to score trajectories
+ */
 class CriticFunction
 {
 public:
+  /**
+    * @brief Constructor for mppi::critics::CriticFunction
+    */
   CriticFunction() = default;
+
+  /**
+    * @brief Destructor for mppi::critics::CriticFunction
+    */
   virtual ~CriticFunction() = default;
 
+  /**
+    * @brief Configure critic on bringup
+    * @param parent WeakPtr to node
+    * @param parent_name name of the controller
+    * @param name Name of plugin
+    * @param costmap_ros Costmap2DROS object of environment
+    * @param dynamic_parameter_handler Parameter handler object
+    */
   void on_configure(
     rclcpp_lifecycle::LifecycleNode::WeakPtr parent,
+    const std::string & parent_name,
     const std::string & name,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
     ParametersHandler * param_handler)
   {
     parent_ = parent;
     logger_ = parent_.lock()->get_logger();
     name_ = name;
+    parent_name_ = parent_name;
     costmap_ros_ = costmap_ros;
     costmap_ = costmap_ros_->getCostmap();
     parameters_handler_ = param_handler;
@@ -38,18 +83,28 @@ class CriticFunction
     initialize();
   }
 
+  /**
+    * @brief Main function to score trajectory
+    * @param data Critic data to use in scoring
+    */
   virtual void score(CriticData & data) = 0;
 
+  /**
+    * @brief Initialize critic
+    */
   virtual void initialize() = 0;
 
+  /**
+    * @brief Get name of critic
+    */
   std::string getName()
   {
     return name_;
   }
 
 protected:
   bool enabled_;
-  std::string name_;
+  std::string name_, parent_name_;
   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   nav2_costmap_2d::Costmap2D * costmap_{nullptr};