Add max_vel_trans parameter

cfg/MPPIController.cfg

@@ -15,6 +15,9 @@ grp_robot.add("vy_max", double_t, 0, "Max VY (m/s)", 0.5, 0.0, 100.0)
 grp_robot.add("vx_min", double_t, 0, "Min VX (m/s)", -0.35, -10.0, 0.0)
 grp_robot.add("vy_min", double_t, 0, "Min VY (m/s)", 0.5, 0.0, 10.0)
 grp_robot.add("wz_max", double_t, 0, "Max WZ (rad/s)", 1.9, 0.0, 10.0)
+
+grp_robot.add("max_vel_trans", double_t, 0, "Max resultant Vx and Vy", 3.0, 0.0, 10.0)
+
 grp_robot.add("vx_std", double_t, 0, "Sampling standard deviation for VX", 0.2, 0.0, 4.0)
 grp_robot.add("vy_std", double_t, 0, "Sampling standard deviation for VY", 0.2, 0.0, 4.0)
 grp_robot.add("wz_std", double_t, 0, "Sampling standard deviation for WZ", 0.4, 0.0, 4.0)

include/mppi_controller/models/constraints.hpp

@@ -28,6 +28,7 @@ struct ControlConstraints
   double vx_min;
   double vy;
   double wz;
+  double max_vel_trans;
 };
 
 /**

include/mppi_controller/models/optimizer_settings.hpp

@@ -27,8 +27,8 @@ namespace mppi::models
  */
 struct OptimizerSettings
 {
-  models::ControlConstraints base_constraints{ 0, 0, 0, 0 };
-  models::ControlConstraints constraints{ 0, 0, 0, 0 };
+  models::ControlConstraints base_constraints{ 0, 0, 0, 0, 0 };
+  models::ControlConstraints constraints{ 0, 0, 0, 0, 0};
   models::SamplingStd sampling_std{ 0, 0, 0 };
   double model_dt{ 0 };
   double temperature{ 0 };

include/mppi_controller/models/state.hpp

@@ -28,13 +28,13 @@ namespace mppi::models {
  * @brief State information: velocities, controls, poses, speed
  */
 struct State {
-  xt::xtensor<float, 2> vx;
-  xt::xtensor<float, 2> vy;
-  xt::xtensor<float, 2> wz;
+  xt::xtensor<double, 2> vx;
+  xt::xtensor<double, 2> vy;
+  xt::xtensor<double, 2> wz;
 
-  xt::xtensor<float, 2> cvx;
-  xt::xtensor<float, 2> cvy;
-  xt::xtensor<float, 2> cwz;
+  xt::xtensor<double, 2> cvx;
+  xt::xtensor<double, 2> cvy;
+  xt::xtensor<double, 2> cwz;
 
   geometry_msgs::PoseStamped pose;
   geometry_msgs::Twist speed;
@@ -43,13 +43,13 @@ struct State {
    * @brief Reset state data
    */
   void reset(unsigned int batch_size, unsigned int time_steps) {
-    vx = xt::zeros<float>({batch_size, time_steps});
-    vy = xt::zeros<float>({batch_size, time_steps});
-    wz = xt::zeros<float>({batch_size, time_steps});
+    vx = xt::zeros<double>({batch_size, time_steps});
+    vy = xt::zeros<double>({batch_size, time_steps});
+    wz = xt::zeros<double>({batch_size, time_steps});
 
-    cvx = xt::zeros<float>({batch_size, time_steps});
-    cvy = xt::zeros<float>({batch_size, time_steps});
-    cwz = xt::zeros<float>({batch_size, time_steps});
+    cvx = xt::zeros<double>({batch_size, time_steps});
+    cvy = xt::zeros<double>({batch_size, time_steps});
+    cwz = xt::zeros<double>({batch_size, time_steps});
   }
 };
 }  // namespace mppi::models

include/mppi_controller/motion_models.hpp

@@ -21,12 +21,13 @@
 #include <xtensor/xmath.hpp>
 #include <xtensor/xnoalias.hpp>
 #include <xtensor/xview.hpp>
-
+#include <cmath>
 #include <dynamic_reconfigure/server.h>
 
 #include "mppi_controller/AckermannConfig.h"
 #include "mppi_controller/models/control_sequence.hpp"
 #include "mppi_controller/models/state.hpp"
+#include "mppi_controller/models/constraints.hpp"
 
 namespace mppi {
 
@@ -46,22 +47,60 @@ class MotionModel {
    */
   virtual ~MotionModel() = default;
 
+  /**
+    * @brief Initialize motion model on bringup and set required variables
+    * @param control_constraints Constraints on control
+    * @param model_dt duration of a time step
+    */
+  void initialize(const models::ControlConstraints & control_constraints, float model_dt)
+  {
+    control_constraints_ = control_constraints;
+    model_dt_ = model_dt;
+  }
+
   /**
    * @brief With input velocities, find the vehicle's output velocities
    * @param state Contains control velocities to use to populate vehicle
    * velocities
    */
   virtual void predict(models::State& state) {
-    using namespace xt::placeholders;  // NOLINT
-    xt::noalias(xt::view(state.vx, xt::all(), xt::range(1, _))) =
-        xt::view(state.cvx, xt::all(), xt::range(0, -1));
-
-    xt::noalias(xt::view(state.wz, xt::all(), xt::range(1, _))) =
-        xt::view(state.cwz, xt::all(), xt::range(0, -1));
-
-    if (isHolonomic()) {
-      xt::noalias(xt::view(state.vy, xt::all(), xt::range(1, _))) =
-          xt::view(state.cvy, xt::all(), xt::range(0, -1));
+    const bool is_holo = isHolonomic();
+    const double max_vel_trans = control_constraints_.max_vel_trans;
+    for (unsigned int i = 0; i != state.vx.shape(0); i++) {
+      float vx_last = state.vx(i, 0);
+      float vy_last = state.vy(i, 0);
+      float wz_last = state.wz(i, 0);
+      for (unsigned int j = 1; j != state.vx.shape(1); j++) {
+        double & cvx_curr = state.cvx(i, j - 1);
+        state.vx(i, j) = cvx_curr;
+        vx_last = cvx_curr;
+
+        double & cwz_curr = state.cwz(i, j - 1);
+        state.wz(i, j) = cwz_curr;
+        wz_last = cwz_curr;
+
+        if (is_holo) {
+          double & cvy_curr = state.cvy(i, j - 1);
+          state.vy(i, j) = cvy_curr;
+          vy_last = cvy_curr;
+        }
+        // Apply max_vel_trans constraint
+        double current_vx = state.vx(i, j);
+        double current_vy = state.vy(i, j);
+        double speed_sq = current_vx * current_vx + current_vy * current_vy;
+        const double max_vel_trans_sq = max_vel_trans * max_vel_trans;
+        if (speed_sq > max_vel_trans_sq) {
+          double speed = std::sqrt(speed_sq);
+          double scale = max_vel_trans / speed;
+          double new_vx = current_vx * scale;
+          double new_vy = current_vy * scale;
+
+          state.vx(i, j) = new_vx;
+          state.vy(i, j) = new_vy;
+          vx_last = new_vx;
+          vy_last = new_vy;
+        }
+      }
     }
   }
 
@@ -71,12 +110,17 @@ class MotionModel {
    */
   virtual bool isHolonomic() = 0;
 
+
   /**
    * @brief Apply hard vehicle constraints to a control sequence
    * @param control_sequence Control sequence to apply constraints to
    */
   virtual void applyConstraints(models::ControlSequence& /*control_sequence*/) {
   }
+
+  protected:
+    float model_dt_{0.0};
+  models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f};
 };
 
 /**

src/optimizer.cpp

@@ -63,6 +63,7 @@ void Optimizer::setParams(const mppi_controller::MPPIControllerConfig& config)
   s.base_constraints.vx_min = config.vx_min;
   s.base_constraints.vy = config.vy_max;
   s.base_constraints.wz = config.wz_max;
+  s.base_constraints.max_vel_trans = config.max_vel_trans;
   s.sampling_std.vx = config.vx_std;
   s.sampling_std.vy = config.vy_std;
   s.sampling_std.wz = config.wz_std;
@@ -239,6 +240,37 @@ void Optimizer::applyControlSequenceConstraints()
   control_sequence_.vx = xt::clip(control_sequence_.vx, s.constraints.vx_min, s.constraints.vx_max);
   control_sequence_.wz = xt::clip(control_sequence_.wz, -s.constraints.wz, s.constraints.wz);
 
+  //max_vel_trans constraint
+  float max_vel_trans = s.constraints.max_vel_trans;
+  float vx_last = control_sequence_.vx(0);
+  float vy_last = control_sequence_.vy(0);
+  float wz_last = control_sequence_.wz(0);
+
+  for (unsigned int i = 1; i != control_sequence_.vx.shape(0); i++) {
+    float & vx_curr = control_sequence_.vx(i);
+    vx_last = vx_curr;
+
+    float & wz_curr = control_sequence_.wz(i);
+    wz_last = wz_curr;
+
+    if (isHolonomic()) {
+      float & vy_curr = control_sequence_.vy(i);
+      vy_last = vy_curr;
+    }
+    // Apply max_vel_trans constraint
+    float current_vx = control_sequence_.vx(i);
+    float current_vy = control_sequence_.vy(i);
+    float speed_sq = current_vx * current_vx + current_vy * current_vy;
+    float max_vel_trans_sq = max_vel_trans * max_vel_trans;
+
+    if (speed_sq > max_vel_trans_sq) {
+      float speed = std::sqrt(speed_sq);
+      float scale = max_vel_trans / speed;
+      control_sequence_.vx(i) = current_vx * scale;
+      control_sequence_.vy(i) = current_vy * scale;
+    }
+  }
+
   motion_model_->applyConstraints(control_sequence_);
 }
 
@@ -418,6 +450,7 @@ void Optimizer::setMotionModel(const int model)
       motion_model_ = std::make_shared<DiffDriveMotionModel>();
       break;
   }
+  motion_model_->initialize(settings_.constraints, settings_.model_dt);
   is_holonomic_ = motion_model_->isHolonomic();
 }