feat: add rear_slip_coeff param to adt model (#1564)

add rear_slip_coeff param to adt model

Signed-off-by: Autumn60 <harada.akiro@gmail.com>

Author: Autumn60

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp

@@ -48,8 +48,8 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface
     double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim,
     double front_wheelbase, double rear_wheelbase, double dt, double acc_delay,
     double acc_time_constant, double steer_delay, double steer_time_constant,
-    double steer_dead_band, double steer_bias, double debug_acc_scaling_factor,
-    double debug_steer_scaling_factor);
+    double steer_dead_band, double steer_bias, double rear_slip_coeff,
+    double debug_acc_scaling_factor, double debug_steer_scaling_factor);
 
   /**
    * @brief default destructor
@@ -88,6 +88,7 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface
   const double steer_time_constant_;         //!< @brief time constant for steering dynamics
   const double steer_dead_band_;             //!< @brief dead band for steering angle [rad]
   const double steer_bias_;                  //!< @brief steering angle bias [rad]
+  const double rear_slip_coeff_;             //!< @brief rear slip coefficient
   const double debug_acc_scaling_factor_;    //!< @brief scaling factor for accel command
   const double debug_steer_scaling_factor_;  //!< @brief scaling factor for steering 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 steer_time_constant = declare_parameter("steer_time_constant", 0.27);
   const double steer_dead_band = declare_parameter("steer_dead_band", 0.0);
   const double steer_bias = declare_parameter("steer_bias", 0.0);
+  const double rear_slip_coeff = declare_parameter("rear_slip_coeff", 1.0);
 
   const double debug_acc_scaling_factor = declare_parameter("debug_acc_scaling_factor", 1.0);
   const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0);
@@ -306,7 +307,7 @@ void SimplePlanningSimulator::initialize_vehicle_model()
     vehicle_model_ptr_ = std::make_shared<SimModelDelayArticulateAccGeared>(
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, front_wheelbase, rear_wheelbase,
       timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay,
-      steer_time_constant, steer_dead_band, steer_bias, debug_acc_scaling_factor,
+      steer_time_constant, steer_dead_band, steer_bias, rear_slip_coeff, debug_acc_scaling_factor,
       debug_steer_scaling_factor);
   } else {
     throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str);

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp

@@ -22,7 +22,8 @@ SimModelDelayArticulateAccGeared::SimModelDelayArticulateAccGeared(
   double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim,
   double front_wheelbase, double rear_wheelbase, double dt, double acc_delay,
   double acc_time_constant, double steer_delay, double steer_time_constant, double steer_dead_band,
-  double steer_bias, double debug_acc_scaling_factor, double debug_steer_scaling_factor)
+  double steer_bias, double rear_slip_coeff, double debug_acc_scaling_factor,
+  double debug_steer_scaling_factor)
 : SimModelInterface(6 /* dim x */, 2 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
@@ -37,6 +38,7 @@ SimModelDelayArticulateAccGeared::SimModelDelayArticulateAccGeared(
   steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
   steer_dead_band_(steer_dead_band),
   steer_bias_(steer_bias),
+  rear_slip_coeff_(rear_slip_coeff),
   debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)),
   debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0))
 {
@@ -72,7 +74,8 @@ double SimModelDelayArticulateAccGeared::getWz()
   const double steer = state_(IDX::STEER);
   const double c_steer = std::cos(steer);
 
-  return (state_(IDX::VX) * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer) /
+  return (state_(IDX::VX) * std::sin(steer) -
+          state_steer_rate_ * front_wheelbase_ * c_steer * rear_slip_coeff_) /
          (front_wheelbase_ + rear_wheelbase_ * c_steer);
 }
 double SimModelDelayArticulateAccGeared::getSteer()
@@ -147,8 +150,9 @@ Eigen::VectorXd SimModelDelayArticulateAccGeared::calcModel(
   Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
   d_state(IDX::X) = vel * cos(yaw);
   d_state(IDX::Y) = vel * sin(yaw);
-  d_state(IDX::YAW) = (vel * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer) /
-                      (front_wheelbase_ + rear_wheelbase_ * c_steer);
+  d_state(IDX::YAW) =
+    (vel * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer * rear_slip_coeff_) /
+    (front_wheelbase_ + rear_wheelbase_ * c_steer);
   d_state(IDX::VX) = acc;
   d_state(IDX::STEER) = state_steer_rate_;
   d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_;