add bias to steer rate

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Jan 31, 2024 · e4dadc5 · e4dadc5
1 parent 6018a2c
commit e4dadc5
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 12 deletions.
diff --git a/...nning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/...nning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
@@ -70,7 +70,7 @@ double SimModelDelaySteerAcc::getWz()
 }
 double SimModelDelaySteerAcc::getSteer()
 {
-  return state_(IDX::STEER);
+  return state_(IDX::STEER) + steer_bias_;
 }
 void SimModelDelaySteerAcc::update(const double & dt)
 {
@@ -112,7 +112,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel(
     sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_;
   const double steer_des =
     sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_;
-  const double steer_diff = steer - steer_des;
+  const double steer_diff = getSteer() - steer_des;
   const double steer_diff_with_dead_band = std::invoke([&]() {
     if (steer_diff > steer_dead_band_) {
       return steer_diff - steer_dead_band_;
@@ -128,7 +128,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::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::tan(steer + steer_bias_) / wheelbase_;
+  d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_;
   d_state(IDX::VX) = acc;
   d_state(IDX::STEER) = steer_rate;
   d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_;

diff --git a/...imulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/...imulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
@@ -72,7 +72,7 @@ double SimModelDelaySteerAccGeared::getWz()
 }
 double SimModelDelaySteerAccGeared::getSteer()
 {
-  return state_(IDX::STEER);
+  return state_(IDX::STEER) + steer_bias_;
 }
 void SimModelDelaySteerAccGeared::update(const double & dt)
 {
@@ -120,7 +120,7 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel(
     sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_;
   const double steer_des =
     sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_;
-  const double steer_diff = steer - steer_des;
+  const double steer_diff = getSteer() - steer_des;
   const double steer_diff_with_dead_band = std::invoke([&]() {
     if (steer_diff > steer_dead_band_) {
       return steer_diff - steer_dead_band_;
@@ -136,7 +136,7 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::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::tan(steer + steer_bias_) / wheelbase_;
+  d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_;
   d_state(IDX::VX) = acc;
   d_state(IDX::STEER) = steer_rate;
   d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_;

diff --git a/...ator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/...ator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
@@ -70,7 +70,7 @@ double SimModelDelaySteerMapAccGeared::getWz()
 }
 double SimModelDelaySteerMapAccGeared::getSteer()
 {
-  return state_(IDX::STEER);
+  return state_(IDX::STEER) + steer_bias_;
 }
 void SimModelDelaySteerMapAccGeared::update(const double & dt)
 {
@@ -114,13 +114,13 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel(
   const double steer = state(IDX::STEER);
   const double acc_des = std::clamp(input(IDX_U::ACCX_DES), -vx_rate_lim_, vx_rate_lim_);
   const double steer_des = std::clamp(input(IDX_U::STEER_DES), -steer_lim_, steer_lim_);
-  double steer_rate = -(steer - steer_des) / steer_time_constant_;
+  double steer_rate = -(getSteer() - steer_des) / steer_time_constant_;
   steer_rate = std::clamp(steer_rate, -steer_rate_lim_, steer_rate_lim_);
 
   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::tan(steer + steer_bias_) / wheelbase_;
+  d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_;
   d_state(IDX::VX) = acc;
   d_state(IDX::STEER) = steer_rate;
   const double converted_acc =

diff --git a/...nning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/...nning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
@@ -67,7 +67,7 @@ double SimModelDelaySteerVel::getWz()
 }
 double SimModelDelaySteerVel::getSteer()
 {
-  return state_(IDX::STEER);
+  return state_(IDX::STEER) + steer_bias_;
 }
 void SimModelDelaySteerVel::update(const double & dt)
 {
@@ -110,7 +110,7 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel(
   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_);
-  const double steer_diff = steer - delay_steer_des;
+  const double steer_diff = getSteer() - delay_steer_des;
   const double steer_diff_with_dead_band = std::invoke([&]() {
     if (steer_diff > steer_dead_band_) {
       return steer_diff - steer_dead_band_;
@@ -126,7 +126,7 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel(
   Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
   d_state(IDX::X) = vx * cos(yaw);
   d_state(IDX::Y) = vx * sin(yaw);
-  d_state(IDX::YAW) = vx * std::tan(steer + steer_bias_) / wheelbase_;
+  d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_;
   d_state(IDX::VX) = vx_rate;
   d_state(IDX::STEER) = steer_rate;