Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(simple_planning_simulator): add acceleration and steer command scaling factor for debug #5534

Merged
merged 2 commits into from
Nov 13, 2023
Merged

feat(simple_planning_simulator): add acceleration and steer command scaling factor for debug #5534

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c1aa8a7
feat(simple_planning_simulator): add acceleration and steer command s…
kosuke55 Nov 9, 2023
7880ea6
update params as debug
kosuke55 Nov 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 15 additions & 13 deletions simulator/simple_planning_simulator/README.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading