From 0b90a9753b978c2dd61fb120915a02273e560a43 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 6 Feb 2024 00:17:13 +0900 Subject: [PATCH] fix(simple_planning_simulator): fix steering bias model (#6240) * fix(simple_planning_simulator): fix steering bias model Signed-off-by: Takamasa Horibe * remove old implementation Signed-off-by: Takamasa Horibe * fix initialize order Signed-off-by: Takamasa Horibe * fix yawrate measurement Signed-off-by: Takamasa Horibe * remove unused code Signed-off-by: Takamasa Horibe * add bias to steer rate Signed-off-by: kosuke55 * add comments Signed-off-by: kosuke55 * fix getWz() Signed-off-by: kosuke55 * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp --------- Signed-off-by: Takamasa Horibe Signed-off-by: kosuke55 Co-authored-by: kosuke55 --- simulator/simple_planning_simulator/README.md | 24 +++++++++---------- .../simple_planning_simulator_core.hpp | 3 --- .../sim_model_delay_steer_acc.hpp | 6 +++-- .../sim_model_delay_steer_acc_geared.hpp | 6 +++-- .../sim_model_delay_steer_map_acc_geared.hpp | 4 +++- .../sim_model_delay_steer_vel.hpp | 4 +++- .../simple_planning_simulator_core.cpp | 15 ++++++------ .../sim_model_delay_steer_acc.cpp | 9 +++---- .../sim_model_delay_steer_acc_geared.cpp | 14 +++++++---- .../sim_model_delay_steer_map_acc_geared.cpp | 7 +++--- .../sim_model_delay_steer_vel.cpp | 9 +++---- 11 files changed, 57 insertions(+), 44 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index ed2b06ee1f44d..d2ed19708c5c5 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -40,18 +40,17 @@ The purpose of this simulator is for the integration test of planning and contro ### Common Parameters -| Name | Type | Description | Default value | -| :--------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | -| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" | -| origin_frame_id | string | set to the frame_id in output tf | "odom" | -| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | -| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | -| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | -| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | -| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | -| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | -| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | -| measurement_steer_bias | double | Measurement bias for steering angle | 0.0 | +| Name | Type | Description | Default value | +| :-------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | +| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" | +| origin_frame_id | string | set to the frame_id in output tf | "odom" | +| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | +| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | +| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | +| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | +| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | +| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | +| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | ### Vehicle Model Parameters @@ -82,6 +81,7 @@ The table below shows which models correspond to what parameters. The model name | vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | 7.0 | [m/ss] | | steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | 1.0 | [rad] | | steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | 5.0 | [rad/s] | +| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | 0.0 | [rad] | | debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | 1.0 | [-] | | debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 1.0 | [-] | | acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | - | [-] | diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 4c3a3aed24d90..58914d19552df 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -195,9 +195,6 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node bool is_initialized_ = false; //!< @brief flag to check the initial position is set bool add_measurement_noise_ = false; //!< @brief flag to add measurement noise - /* measurement bias */ - double measurement_steer_bias_ = 0.0; //!< @brief measurement bias for steering measurement - DeltaTime delta_time_{}; //!< @brief to calculate delta time MeasurementNoiseGenerator measurement_noise_{}; //!< @brief for measurement noise 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 92129541ff891..f43abd8572874 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,14 +40,15 @@ 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] steer_bias steering bias [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 debug_acc_scaling_factor, - double debug_steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor); /** * @brief default destructor @@ -84,6 +85,7 @@ 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 steer_bias_; //!< @brief steering angle bias [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 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 376ee55f1aa5e..35b95bf4b1ae5 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,14 +40,15 @@ 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] steer_bias steering bias [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 debug_acc_scaling_factor, - double debug_steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor); /** * @brief default destructor @@ -84,6 +85,7 @@ 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 steer_bias_; //!< @brief steering angle bias [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 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index fecd3c2bc8be4..d7ca032aa6c48 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -92,12 +92,13 @@ class SimModelDelaySteerMapAccGeared : 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_bias steering bias [rad] * @param [in] path path to csv file for acceleration conversion map */ SimModelDelaySteerMapAccGeared( 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, std::string path); + double steer_time_constant, double steer_bias, std::string path); /** * @brief default destructor @@ -135,6 +136,7 @@ class SimModelDelaySteerMapAccGeared : 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_bias_; //!< @brief steering angle bias [rad] const std::string path_; //!< @brief conversion map path /** 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 5c1ec55d522bf..61a9e8d0a5643 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 @@ -43,11 +43,12 @@ class SimModelDelaySteerVel : 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] steer_bias steering bias [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_dead_band); + double steer_time_constant, double steer_dead_band, double steer_bias); /** * @brief destructor @@ -86,6 +87,7 @@ class SimModelDelaySteerVel : public SimModelInterface 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] + const double steer_bias_; //!< @brief steering angle bias [rad] /** * @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 37ec5b33014a7..26be4841e2f2a 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 @@ -102,7 +102,6 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link"); origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); add_measurement_noise_ = declare_parameter("add_measurement_noise", false); - measurement_steer_bias_ = declare_parameter("measurement_steer_bias", 0.0); simulate_motion_ = declare_parameter("initial_engage_state"); enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false); @@ -232,6 +231,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 steer_bias = declare_parameter("steer_bias", 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(); @@ -250,19 +251,20 @@ 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, steer_dead_band); + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + steer_bias); } 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, steer_dead_band, - debug_acc_scaling_factor, debug_steer_scaling_factor); + steer_bias, 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, - debug_acc_scaling_factor, debug_steer_scaling_factor); + steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor); } else if (vehicle_model_type_str == "DELAY_STEER_MAP_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_MAP_ACC_GEARED; const std::string acceleration_map_path = @@ -277,7 +279,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() } 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_bias, acceleration_map_path); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); @@ -383,9 +385,6 @@ void SimplePlanningSimulator::on_timer() add_measurement_noise(current_odometry_, current_velocity_, current_steer_); } - // add measurement bias - current_steer_.steering_tire_angle += measurement_steer_bias_; - // add estimate covariance { using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; 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 670671165de60..6a9b5c65d4760 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,8 +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 debug_acc_scaling_factor, - double debug_steer_scaling_factor) + double steer_time_constant, double steer_dead_band, double steer_bias, + 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,6 +33,7 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias), debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { @@ -69,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) { @@ -111,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_; 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 d72b8bf116d77..10e6a97d703cd 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,8 +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 debug_acc_scaling_factor, - double debug_steer_scaling_factor) + double steer_time_constant, double steer_dead_band, double steer_bias, + 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), @@ -35,6 +35,7 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias), debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { @@ -68,10 +69,12 @@ double SimModelDelaySteerAccGeared::getAx() double SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + ; } double SimModelDelaySteerAccGeared::getSteer() { - return state_(IDX::STEER); + // return measured values with bias added to actual values + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerAccGeared::update(const double & dt) { @@ -119,7 +122,10 @@ 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; + // NOTE: `steer_des` is calculated by control from measured values. getSteer() also gets the + // measured value. The steer_rate used in the motion calculation is obtained from these + // differences. + 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_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index b04a10667adcf..72273f0b21ec2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -21,7 +21,7 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( 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, std::string path) + double steer_time_constant, double steer_bias, std::string path) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -33,6 +33,7 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( 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_bias_(steer_bias), path_(path) { initializeInputQueue(dt); @@ -69,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) { @@ -113,7 +114,7 @@ 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_); 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 6d155ee921107..1ee08fad566f5 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_dead_band) + double steer_time_constant, double steer_dead_band, double steer_bias) : SimModelInterface(5 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -31,7 +31,8 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( 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_dead_band_(steer_dead_band) + steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias) { initializeInputQueue(dt); } @@ -66,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) { @@ -109,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_;