Skip to content

Commit

Permalink
feat(simple_planning_simulator): add acceleration and steer command s…
Browse files Browse the repository at this point in the history 
…caling factor for debug (autowarefoundation#5534)

* feat(simple_planning_simulator): add acceleration and steer command scaling factor

Signed-off-by: kosuke55 <[email protected]>

* update params as debug

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@kosuke55
kosuke55 authored Nov 13, 2023
1 parent 2a697c2 commit a37b4f3
Show file tree
Hide file tree
Showing 6 changed files with 65 additions and 40 deletions.
28 changes: 15 additions & 13 deletions simulator/simple_planning_simulator/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 | [-] |

<!-- deadzone_delta_steer | double | dead zone for the steering dynamics | x | x | x | o | o | 0.0 | [rad] | | -->

Expand Down
21 changes: 13 additions & 8 deletions ...g_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<double> acc_input_queue_; //!< @brief buffer for accel command
std::deque<double> 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<double> acc_input_queue_; //!< @brief buffer for accel command
std::deque<double> 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
Expand Down
21 changes: 13 additions & 8 deletions ...ator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<double> acc_input_queue_; //!< @brief buffer for accel command
std::deque<double> 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<double> acc_input_queue_; //!< @brief buffer for accel command
std::deque<double> 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
Expand Down
8 changes: 6 additions & 2 deletions ...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -251,12 +253,14 @@ void SimplePlanningSimulator::initialize_vehicle_model()
vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC;
vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAcc>(
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<SimModelDelaySteerAccGeared>(
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);
}
Expand Down
13 changes: 9 additions & 4 deletions ...nning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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);
}
Expand Down Expand Up @@ -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_) {
Expand Down
14 changes: 9 additions & 5 deletions ...imulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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);
}
Expand Down Expand Up @@ -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_) {
Expand Down

0 comments on commit a37b4f3

Please sign in to comment.