feat(simple_planning_simulator): add steer dead band

Signed-off-by: kosuke55 <[email protected]>

simulator/simple_planning_simulator/README.md

@@ -74,6 +74,7 @@ The table below shows which models correspond to what parameters. The model name
 | vel_time_delay      | double | dead time for the velocity input                     | x      | x      | x        | o      | x      | x        | 0.25          | [s]     |
 | acc_time_constant   | double | time constant of the 1st-order acceleration dynamics | x      | x      | x        | x      | o      | o        | 0.1           | [s]     |
 | steer_time_constant | double | time constant of the 1st-order steering dynamics     | x      | x      | x        | o      | o      | o        | 0.27          | [s]     |
+| steer_dead_band     | double | dead band for steering angle                         | x      | x      | x        | o      | o      | o        | 0.0012        | [rad]   |
 | vel_time_constant   | double | time constant of the 1st-order velocity dynamics     | x      | x      | x        | o      | x      | x        | 0.5           | [s]     |
 | vel_lim             | double | limit of velocity                                    | x      | x      | x        | o      | o      | o        | 50.0          | [m/s]   |
 | vel_rate_lim        | double | limit of acceleration                                | x      | x      | x        | o      | o      | o        | 7.0           | [m/ss]  |

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp

@@ -39,11 +39,12 @@ class SimModelDelaySteerAcc : public SimModelInterface
    * @param [in] acc_time_constant time constant for 1D model of accel dynamics
    * @param [in] steer_delay time delay for steering command [s]
    * @param [in] steer_time_constant time constant for 1D model of steering dynamics
+   * @param [in] steer_dead_band dead band for steering angle [rad]
    */
   SimModelDelaySteerAcc(
     double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
     double dt, double acc_delay, double acc_time_constant, double steer_delay,
-    double steer_time_constant);
+    double steer_time_constant, double steer_dead_band);
 
   /**
    * @brief default destructor
@@ -79,6 +80,7 @@ class SimModelDelaySteerAcc : public SimModelInterface
   const double acc_time_constant_;        //!< @brief time constant for accel dynamics
   const double steer_delay_;              //!< @brief time delay for steering command [s]
   const double steer_time_constant_;      //!< @brief time constant for steering dynamics
+  const double steer_dead_band_;          //!< @brief dead band for steering angle [rad]
 
   /**
    * @brief set queue buffer for input command

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp

@@ -39,11 +39,12 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
    * @param [in] acc_time_constant time constant for 1D model of accel dynamics
    * @param [in] steer_delay time delay for steering command [s]
    * @param [in] steer_time_constant time constant for 1D model of steering dynamics
+   * @param [in] steer_dead_band dead band for steering angle [rad]
    */
   SimModelDelaySteerAccGeared(
     double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
     double dt, double acc_delay, double acc_time_constant, double steer_delay,
-    double steer_time_constant);
+    double steer_time_constant, double steer_dead_band);
 
   /**
    * @brief default destructor
@@ -79,6 +80,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
   const double acc_time_constant_;        //!< @brief time constant for accel dynamics
   const double steer_delay_;              //!< @brief time delay for steering command [s]
   const double steer_time_constant_;      //!< @brief time constant for steering dynamics
+  const double steer_dead_band_;          //!< @brief dead band for steering angle [rad]
 
   /**
    * @brief set queue buffer for input command

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp

@@ -42,11 +42,12 @@ class SimModelDelaySteerVel : public SimModelInterface
    * @param [in] vx_time_constant time constant for 1D model of velocity dynamics
    * @param [in] steer_delay time delay for steering command [s]
    * @param [in] steer_time_constant time constant for 1D model of steering dynamics
+   * @param [in] steer_dead_band dead band for steering angle [rad]
    */
   SimModelDelaySteerVel(
     double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
     double dt, double vx_delay, double vx_time_constant, double steer_delay,
-    double steer_time_constant);
+    double steer_time_constant, double steer_dead_band);
 
   /**
    * @brief destructor
@@ -84,6 +85,7 @@ class SimModelDelaySteerVel : public SimModelInterface
   const double steer_delay_;  //!< @brief time delay for angular-velocity command [s]
   const double
     steer_time_constant_;  //!< @brief time constant for 1D model of angular-velocity dynamics
+  const double steer_dead_band_;  //!< @brief dead band for steering angle [rad]
 
   /**
    * @brief set queue buffer for input command

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -229,6 +229,7 @@ void SimplePlanningSimulator::initialize_vehicle_model()
   const double vel_time_constant = declare_parameter("vel_time_constant", 0.5);
   const double steer_time_delay = declare_parameter("steer_time_delay", 0.24);
   const double steer_time_constant = declare_parameter("steer_time_constant", 0.27);
+  const double steer_dead_band = declare_parameter("steer_dead_band", 0.0012);
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
   const double wheelbase = vehicle_info.wheel_base_m;
 
@@ -245,17 +246,17 @@ void SimplePlanningSimulator::initialize_vehicle_model()
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL;
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerVel>(
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
-      vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant);
+      vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band);
   } else if (vehicle_model_type_str == "DELAY_STEER_ACC") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC;
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAcc>(
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
-      acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant);
+      acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band);
   } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED;
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAccGeared>(
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
-      acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant);
+      acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band);
   } else {
     throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str);
   }

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

@@ -19,7 +19,7 @@
 SimModelDelaySteerAcc::SimModelDelaySteerAcc(
   double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
   double dt, double acc_delay, double acc_time_constant, double steer_delay,
-  double steer_time_constant)
+  double steer_time_constant, double steer_dead_band)
 : SimModelInterface(6 /* dim x */, 2 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
@@ -30,7 +30,8 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc(
   acc_delay_(acc_delay),
   acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)),
   steer_delay_(steer_delay),
-  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT))
+  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
+  steer_dead_band_(steer_dead_band)
 {
   initializeInputQueue(dt);
 }
@@ -105,8 +106,18 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel(
   const double steer = state(IDX::STEER);
   const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_);
   const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_);
-  double steer_rate = -(steer - steer_des) / steer_time_constant_;
-  steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_);
+  const double steer_diff = steer - steer_des;
+  const double steer_diff_with_dead_band = std::invoke([&]() {
+    if (steer_diff > steer_dead_band_) {
+      return steer_diff - steer_dead_band_;
+    } else if (steer_diff < -steer_dead_band_) {
+      return steer_diff + steer_dead_band_;
+    } else {
+      return 0.0;
+    }
+  });
+  const double steer_rate =
+    sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_);
 
   Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
   d_state(IDX::X) = vel * cos(yaw);

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp

@@ -21,7 +21,7 @@
 SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared(
   double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
   double dt, double acc_delay, double acc_time_constant, double steer_delay,
-  double steer_time_constant)
+  double steer_time_constant, double steer_dead_band)
 : SimModelInterface(6 /* dim x */, 2 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
@@ -32,7 +32,9 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared(
   acc_delay_(acc_delay),
   acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)),
   steer_delay_(steer_delay),
-  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT))
+  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
+  steer_dead_band_(steer_dead_band)
+
 {
   initializeInputQueue(dt);
 }
@@ -113,8 +115,18 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel(
   const double steer = state(IDX::STEER);
   const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_);
   const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_);
-  double steer_rate = -(steer - steer_des) / steer_time_constant_;
-  steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_);
+  const double steer_diff = steer - steer_des;
+  const double steer_diff_with_dead_band = std::invoke([&]() {
+    if (steer_diff > steer_dead_band_) {
+      return steer_diff - steer_dead_band_;
+    } else if (steer_diff < -steer_dead_band_) {
+      return steer_diff + steer_dead_band_;
+    } else {
+      return 0.0;
+    }
+  });
+  const double steer_rate =
+    sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_);
 
   Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
   d_state(IDX::X) = vel * cos(yaw);

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp

@@ -19,7 +19,7 @@
 SimModelDelaySteerVel::SimModelDelaySteerVel(
   double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
   double dt, double vx_delay, double vx_time_constant, double steer_delay,
-  double steer_time_constant)
+  double steer_time_constant, double steer_dead_band)
 : SimModelInterface(5 /* dim x */, 2 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
@@ -30,7 +30,8 @@ SimModelDelaySteerVel::SimModelDelaySteerVel(
   vx_delay_(vx_delay),
   vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)),
   steer_delay_(steer_delay),
-  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT))
+  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
+  steer_dead_band_(steer_dead_band)
 {
   initializeInputQueue(dt);
 }
@@ -106,11 +107,20 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel(
   const double delay_input_vx = input(IDX_U::VX_DES);
   const double delay_input_steer = input(IDX_U::STEER_DES);
   const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_);
+  const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_);
   const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_);
-  double vx_rate = -(vx - delay_vx_des) / vx_time_constant_;
-  double steer_rate = -(steer - delay_steer_des) / steer_time_constant_;
-  vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_);
-  steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_);
+  const double steer_diff = steer - delay_steer_des;
+  const double steer_diff_with_dead_band = std::invoke([&]() {
+    if (steer_diff > steer_dead_band_) {
+      return steer_diff - steer_dead_band_;
+    } else if (steer_diff < -steer_dead_band_) {
+      return steer_diff + steer_dead_band_;
+    } else {
+      return 0.0;
+    }
+  });
+  const double steer_rate =
+    sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_);
 
   Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
   d_state(IDX::X) = vx * cos(yaw);