From afa913dd629d4279ca3a6ed062ce4732333f08c8 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Mon, 30 Sep 2024 13:24:08 +0900 Subject: [PATCH] feat: add rear_slip_coeff param to adt model (#1564) add rear_slip_coeff param to adt model Signed-off-by: Autumn60 --- .../sim_model_delay_articulate_acc_geared.hpp | 5 +++-- .../simple_planning_simulator_core.cpp | 3 ++- .../sim_model_delay_articulate_acc_geared.cpp | 12 ++++++++---- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp index cefcdb5e1b71f..905eb3dc13da1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp +++ b/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 diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index bc74942fbc291..0e2b43afdb68a 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/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( 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); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp index 1eea34f7f8cd3..aeb0f4cb2e31f 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp +++ b/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_;