diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index ceecacfe8cd8c..75f77489daada 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -74,6 +74,7 @@ The table below shows which models correspond to what parameters. The model name | 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] | 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 9e7c101e76407..289c607a18d90 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 @@ -39,11 +39,12 @@ class SimModelDelaySteerAcc : public SimModelInterface * @param [in] acc_time_constant time constant for 1D model of accel dynamics * @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] */ 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_time_constant, double steer_dead_band); /** * @brief default destructor @@ -79,6 +80,7 @@ class SimModelDelaySteerAcc : public SimModelInterface 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] /** * @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 1beec46a48ee9..3bda878f8cd76 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 @@ -39,11 +39,12 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @param [in] acc_time_constant time constant for 1D model of accel dynamics * @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] */ 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_time_constant, double steer_dead_band); /** * @brief default destructor @@ -79,6 +80,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface 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] /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp index 30cf5d3c7c24a..5c1ec55d522bf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -42,11 +42,12 @@ class SimModelDelaySteerVel : public SimModelInterface * @param [in] vx_time_constant time constant for 1D model of velocity dynamics * @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] */ SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant); + double steer_time_constant, double steer_dead_band); /** * @brief destructor @@ -84,6 +85,7 @@ class SimModelDelaySteerVel : public SimModelInterface const double steer_delay_; //!< @brief time delay for angular-velocity command [s] const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** * @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 d7c002a1fcaa9..dbd56bf9e9bff 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 @@ -14,6 +14,7 @@ acc_time_constant: 0.1 steer_time_delay: 0.1 steer_time_constant: 0.1 + steer_dead_band: 0.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 fb6a5457e8f05..d7e25860c8abf 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 vel_time_constant = declare_parameter("vel_time_constant", 0.5); 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 auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; @@ -245,17 +246,17 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant); + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { 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); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); } 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); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); } 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 870444acb5215..964157cdeb64c 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,7 @@ 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_time_constant, double steer_dead_band) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -30,7 +30,8 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( acc_delay_(acc_delay), 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_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) { initializeInputQueue(dt); } @@ -105,8 +106,18 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( 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_); - double steer_rate = -(steer - steer_des) / steer_time_constant_; - steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + const double steer_diff = steer - steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); 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 5170ed914e0f1..76669c9490fc6 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,7 @@ 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_time_constant, double steer_dead_band) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -32,7 +32,9 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( acc_delay_(acc_delay), 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_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) + { initializeInputQueue(dt); } @@ -113,8 +115,18 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( 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_); - double steer_rate = -(steer - steer_des) / steer_time_constant_; - steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + const double steer_diff = steer - steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 1ac71fbb58c23..6d155ee921107 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -19,7 +19,7 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant) + double steer_time_constant, double steer_dead_band) : SimModelInterface(5 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -30,7 +30,8 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( vx_delay_(vx_delay), vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) { initializeInputQueue(dt); } @@ -106,11 +107,20 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( const double delay_input_vx = input(IDX_U::VX_DES); const double delay_input_steer = input(IDX_U::STEER_DES); 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_); - double vx_rate = -(vx - delay_vx_des) / vx_time_constant_; - double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; - vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_); - steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + const double steer_diff = steer - delay_steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw);