From c1aa8a7c07c7d466b99de70601f87f34dc40e77d Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 9 Nov 2023 18:31:46 +0900 Subject: [PATCH] feat(simple_planning_simulator): add acceleration and steer command scaling factor Signed-off-by: kosuke55 --- simulator/simple_planning_simulator/README.md | 28 ++++++++++--------- .../sim_model_delay_steer_acc.hpp | 7 ++++- .../sim_model_delay_steer_acc_geared.hpp | 7 ++++- ...mple_planning_simulator_default.param.yaml | 2 ++ .../simple_planning_simulator_core.cpp | 8 ++++-- .../sim_model_delay_steer_acc.cpp | 13 ++++++--- .../sim_model_delay_steer_acc_geared.cpp | 14 ++++++---- 7 files changed, 53 insertions(+), 26 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 75f77489daada..bb66725c589db 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -67,19 +67,21 @@ The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves base The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | -| :------------------ | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | -| 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.0 | [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] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | +| :------------------- | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | +| 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.0 | [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] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | +| acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] | +| steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | 1.0 | [-] | diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index 289c607a18d90..5b95969f6898c 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -40,11 +40,14 @@ class SimModelDelaySteerAcc : public SimModelInterface * @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] + * @param [in] acc_scaling_factor scaling factor for accel command + * @param [in] steer_scaling_factor scaling factor for steering command */ 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_dead_band); + double steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor); /** * @brief default destructor @@ -81,6 +84,8 @@ class SimModelDelaySteerAcc : public SimModelInterface 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] + const double acc_scaling_factor_; //!< @brief scaling factor for accel command + const double steer_scaling_factor_; //!< @brief scaling factor for steering command /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 3bda878f8cd76..9664d4d953fe1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -40,11 +40,14 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @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] + * @param [in] acc_scaling_factor scaling factor for accel command + * @param [in] steer_scaling_factor scaling factor for steering command */ 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_dead_band); + double steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor); /** * @brief default destructor @@ -81,6 +84,8 @@ class SimModelDelaySteerAccGeared : public SimModelInterface 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] + const double acc_scaling_factor_; //!< @brief scaling factor for accel command + const double steer_scaling_factor_; //!< @brief scaling factor for steering command /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml index dbd56bf9e9bff..c24b23a1119ed 100644 --- a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -15,6 +15,8 @@ steer_time_delay: 0.1 steer_time_constant: 0.1 steer_dead_band: 0.0 + acc_scaling_factor: 1.0 + steer_scaling_factor: 1.0 x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate 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 f2629a0586045..8fafd24fe5ed2 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 @@ -230,6 +230,8 @@ void SimplePlanningSimulator::initialize_vehicle_model() 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.0); + const double acc_scaling_factor = declare_parameter("acc_scaling_factor", 1.0); + const double steer_scaling_factor = declare_parameter("steer_scaling_factor", 1.0); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; @@ -251,12 +253,14 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( 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, steer_dead_band); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + acc_scaling_factor, steer_scaling_factor); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( 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, steer_dead_band); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + acc_scaling_factor, 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/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 964157cdeb64c..368a57d2f6019 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -19,7 +19,8 @@ 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_dead_band) + double steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -31,7 +32,9 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( 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_dead_band_(steer_dead_band) + steer_dead_band_(steer_dead_band), + acc_scaling_factor_(std::max(acc_scaling_factor, 0.0)), + steer_scaling_factor_(std::max(steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -104,8 +107,10 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); const double yaw = state(IDX::YAW); 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_); + const double acc_des = + sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * acc_scaling_factor_; + const double steer_des = + sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * steer_scaling_factor_; const double steer_diff = steer - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 76669c9490fc6..206e140ede00a 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -21,7 +21,8 @@ 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_dead_band) + double steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -33,8 +34,9 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( 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_dead_band_(steer_dead_band) - + steer_dead_band_(steer_dead_band), + acc_scaling_factor_(std::max(acc_scaling_factor, 0.0)), + steer_scaling_factor_(std::max(steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -113,8 +115,10 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); const double yaw = state(IDX::YAW); 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_); + const double acc_des = + sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * acc_scaling_factor_; + const double steer_des = + sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * steer_scaling_factor_; const double steer_diff = steer - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) {