F/ackermann (#59)

* init * update * ament uncristify * minor * bring back prev arch * bring back s * final -> override * bring back guards * fix typo * bring back predict * clean up * add param to readme Co-authored-by: Alex <[email protected]>
artofnothingness · Jun 10, 2022 · 6d37c4d · 6d37c4d
1 parent 862856c
commit 6d37c4d
Show file tree

Hide file tree

Showing 9 changed files with 64 additions and 34 deletions.
diff --git a/README.md b/README.md
@@ -73,6 +73,12 @@ sudo make install
  | retry_attempt_limit        | int    | Number of attempts to find feasible trajectory before failure                                                                                                                                                                                                                                                      |
 
 
+#### AckermannConstrains params
+ | Parameter            | Type   | Definition                                                                                                  |
+ | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
+ | min_turning_r        | double | minimum turning radius for ackermann motion model                                                           |
+
+
 #### GoalCritic params
  | Parameter            | Type   | Definition                                                                                                  |
  | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
@@ -142,6 +148,8 @@ controller_server:
       temperature: 0.25
       motion_model: "DiffDrive"
       visualize: false
+      AckermannConstrains:
+        min_turning_r: 0.2
       critics: ["ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic" ]
       GoalCritic:
         enabled: true

diff --git a/include/mppic/models/constraints.hpp b/include/mppic/models/constraints.hpp
@@ -12,7 +12,6 @@ struct ControlConstraints
   double wz;
 };
 
-
 struct SamplingStd
 {
   double vx;

diff --git a/include/mppic/models/control_sequence.hpp b/include/mppic/models/control_sequence.hpp
@@ -7,8 +7,6 @@
 
 #include <xtensor/xtensor.hpp>
 
-#include "mppic/motion_models.hpp"
-
 namespace mppi::models
 {
 

diff --git a/include/mppic/models/optimizer_settings.hpp b/include/mppic/models/optimizer_settings.hpp
@@ -2,6 +2,7 @@
 #pragma once
 
 #include "mppic/models/constraints.hpp"
+#include <cstddef>
 
 namespace mppi::models
 {

diff --git a/include/mppic/motion_models.hpp b/include/mppic/motion_models.hpp
@@ -1,19 +1,24 @@
-// Copyright 2022 FastSense, Samsung Research
+// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research
+
 #ifndef MPPIC__MOTION_MODELS_HPP_
 #define MPPIC__MOTION_MODELS_HPP_
 
 #include <cstdint>
 
 #include "mppic/models/state.hpp"
+#include <xtensor/xmath.hpp>
+#include <xtensor/xmasked_view.hpp>
+
+#include "mppic/parameters_handler.hpp"
 
 namespace mppi
 {
 
-class MotionModel
+class IMotionModel
 {
 public:
-  MotionModel() = default;
-  virtual ~MotionModel() = default;
+  IMotionModel() = default;
+  virtual ~IMotionModel() = default;
 
   /**
    * @brief Predict velocities for given trajectories the next time step
@@ -30,33 +35,60 @@ class MotionModel
     return xt::view(state, xt::all(), xt::range(idx.cbegin(), idx.cend()));
   }
 
-  virtual bool isHolonomic() const = 0;
+  virtual bool isHolonomic() = 0;
+  virtual void applyConstraints(models::State & /*state*/) {}
 };
 
-class DiffDriveMotionModel : public MotionModel
+class AckermannMotionModel : public IMotionModel
 {
 public:
-  bool isHolonomic() const override {return false;}
+  explicit AckermannMotionModel(ParametersHandler * param_handler)
+  {
+    auto getParam = param_handler->getParamGetter("AckermannConstraints");
+    getParam(min_turning_r_, "min_turning_r", 0.2);
+  }
+
+  bool isHolonomic() override
+  {
+    return false;
+  }
+
+  void applyConstraints(models::State & state) override
+  {
+    auto v = state.getVelocitiesVX();
+    auto w = state.getVelocitiesWZ();
+
+    auto view = xt::masked_view(w, v / w > min_turning_r_);
+    view = xt::sign(v) / min_turning_r_;
+  }
+
+private:
+  double min_turning_r_{0};
 };
 
-class OmniMotionModel : public MotionModel
+class DiffDriveMotionModel : public IMotionModel
 {
 public:
-  bool isHolonomic() const override {return true;}
+  DiffDriveMotionModel() = default;
+
+  bool isHolonomic() override
+  {
+    return false;
+  }
 };
 
-class AckermannMotionModel : public MotionModel
+class OmniMotionModel : public IMotionModel
 {
 public:
-  xt::xtensor<double, 2> predict(
-    const xt::xtensor<double, 2> & /*state*/, const models::StateIdxes & /*idx*/) override
+  OmniMotionModel() = default;
+
+  bool isHolonomic() override
   {
-    throw std::runtime_error("Ackermann motion model not yet implemented");
+    return true;
   }
 
-  bool isHolonomic() const override {return false;}
 };
 
-}  // namespace mppi
+}  // namespace mppi::models
 
 #endif  // MPPIC__MOTION_MODELS_HPP_
diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp
@@ -123,9 +123,9 @@ class Optimizer
   nav2_costmap_2d::Costmap2D * costmap_;
   std::string name_;
 
-  ParametersHandler * parameters_handler_;
+  std::unique_ptr<IMotionModel> motion_model_;
 
-  std::unique_ptr<MotionModel> motion_model_;
+  ParametersHandler * parameters_handler_;
   CriticManager critic_manager_;
 
   models::OptimizerSettings settings_;

diff --git a/src/controller.cpp b/src/controller.cpp
@@ -2,7 +2,6 @@
 #include <stdint.h>
 #include <chrono>
 #include "mppic/controller.hpp"
-#include "mppic/motion_models.hpp"
 #include "mppic/utils.hpp"
 
 namespace mppi

diff --git a/src/optimizer.cpp b/src/optimizer.cpp
@@ -213,6 +213,8 @@ void Optimizer::applyControlConstraints()
     vy = xt::clip(vy, -s.constraints.vy, s.constraints.vy);
   }
 
+  motion_model_->applyConstraints(state_);
+
   vx = xt::clip(vx, -s.constraints.vx, s.constraints.vx);
   wz = xt::clip(wz, -s.constraints.wz, s.constraints.wz);
 }
@@ -254,7 +256,7 @@ void Optimizer::propagateStateVelocitiesFromInitials(
 xt::xtensor<double, 2> Optimizer::getOptimizedTrajectory()
 {
   models::State state;
-  state.idx.setLayout(motion_model_->isHolonomic());
+  state.idx.setLayout(isHolonomic());
   state.reset(1U, settings_.time_steps);
   state.getControls() = control_sequence_.data;
   state.getTimeIntervals() = settings_.model_dt;
@@ -333,7 +335,7 @@ void Optimizer::setMotionModel(const std::string & model)
   } else if (model == "Omni") {
     motion_model_ = std::make_unique<OmniMotionModel>();
   } else if (model == "Ackermann") {
-    motion_model_ = std::make_unique<AckermannMotionModel>();
+    motion_model_ = std::make_unique<AckermannMotionModel>(parameters_handler_);
   } else {
     throw std::runtime_error(
             std::string(
@@ -342,8 +344,8 @@ void Optimizer::setMotionModel(const std::string & model)
               model.c_str()));
   }
 
-  state_.idx.setLayout(motion_model_->isHolonomic());
-  control_sequence_.idx.setLayout(motion_model_->isHolonomic());
+  state_.idx.setLayout(isHolonomic());
+  control_sequence_.idx.setLayout(isHolonomic());
 }
 
 void Optimizer::setSpeedLimit(double speed_limit, bool percentage)

diff --git a/test/optimizer_test.cpp b/test/optimizer_test.cpp
@@ -127,12 +127,3 @@ TEST(MPPIOptimizer, OptimizerTestOmniCircle)
   EXPECT_TRUE(!inCollision(trajectory, *costmap));
   EXPECT_TRUE(isGoalReached(trajectory, *costmap, goal_point));
 }
-
-TEST(MPPIOptimizer, AckermannException)
-{
-  mppi::AckermannMotionModel model;
-  xt::xtensor<double, 2> in;
-  mppi::models::StateIdxes idx;
-  EXPECT_FALSE(model.isHolonomic());
-  EXPECT_THROW(model.predict(in, idx), std::runtime_error);
-}