From a37b4f36b9ecc4f59ac4b9d298f4ffaf263107b1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 13 Nov 2023 13:23:03 +0900 Subject: [PATCH] feat(simple_planning_simulator): add acceleration and steer command scaling factor for debug (#5534) * feat(simple_planning_simulator): add acceleration and steer command scaling factor Signed-off-by: kosuke55 * update params as debug Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- simulator/simple_planning_simulator/README.md | 28 ++++++++++--------- .../sim_model_delay_steer_acc.hpp | 21 ++++++++------ .../sim_model_delay_steer_acc_geared.hpp | 21 ++++++++------ .../simple_planning_simulator_core.cpp | 8 ++++-- .../sim_model_delay_steer_acc.cpp | 13 ++++++--- .../sim_model_delay_steer_acc_geared.cpp | 14 ++++++---- 6 files changed, 65 insertions(+), 40 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 75f77489daada..0c5441c1ad9c8 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] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] | +| debug_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..92129541ff891 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] debug_acc_scaling_factor scaling factor for accel command + * @param [in] debug_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 debug_acc_scaling_factor, + double debug_steer_scaling_factor); /** * @brief default destructor @@ -74,13 +77,15 @@ class SimModelDelaySteerAcc : public SimModelInterface const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] const double wheelbase_; //!< @brief vehicle wheelbase length [m] - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - 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] + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + 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] + const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command + const double debug_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..376ee55f1aa5e 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] debug_acc_scaling_factor scaling factor for accel command + * @param [in] debug_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 debug_acc_scaling_factor, + double debug_steer_scaling_factor); /** * @brief default destructor @@ -74,13 +77,15 @@ class SimModelDelaySteerAccGeared : public SimModelInterface const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] const double wheelbase_; //!< @brief vehicle wheelbase length [m] - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - 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] + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + 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] + const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command + const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command /** * @brief set queue buffer for input 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 f2629a0586045..07a5d34f63c39 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 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); 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, + debug_acc_scaling_factor, debug_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, + 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/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..670671165de60 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 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), @@ -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), + debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), + debug_steer_scaling_factor_(std::max(debug_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_) * 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_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..d72b8bf116d77 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 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), @@ -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), + debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), + debug_steer_scaling_factor_(std::max(debug_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_) * 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_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) {