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: add rear_slip_coeff param to adt model #1564

Merged
merged 1 commit into from
Sep 30, 2024
Merged

feat: add rear_slip_coeff param to adt model #1564

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
5 changes: 3 additions & 2 deletions ...ple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,8 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface
double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim,
double front_wheelbase, double rear_wheelbase, double dt, double acc_delay,
double acc_time_constant, double steer_delay, double steer_time_constant,
double steer_dead_band, double steer_bias, double debug_acc_scaling_factor,
double debug_steer_scaling_factor);
double steer_dead_band, double steer_bias, double rear_slip_coeff,
double debug_acc_scaling_factor, double debug_steer_scaling_factor);

/**
* @brief default destructor
Expand Down Expand Up @@ -88,6 +88,7 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface
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 rear_slip_coeff_; //!< @brief rear slip coefficient
const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command
const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command

Expand Down
3 changes: 2 additions & 1 deletion ...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 @@ -229,6 +229,7 @@ void SimplePlanningSimulator::initialize_vehicle_model()
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 rear_slip_coeff = declare_parameter("rear_slip_coeff", 1.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);
Expand Down Expand Up @@ -306,7 +307,7 @@ void SimplePlanningSimulator::initialize_vehicle_model()
vehicle_model_ptr_ = std::make_shared<SimModelDelayArticulateAccGeared>(
vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, front_wheelbase, rear_wheelbase,
timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay,
steer_time_constant, steer_dead_band, steer_bias, debug_acc_scaling_factor,
steer_time_constant, steer_dead_band, steer_bias, rear_slip_coeff, debug_acc_scaling_factor,
debug_steer_scaling_factor);
} else {
throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str);
Expand Down
12 changes: 8 additions & 4 deletions ...ple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,8 @@ SimModelDelayArticulateAccGeared::SimModelDelayArticulateAccGeared(
double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim,
double front_wheelbase, double rear_wheelbase, double dt, double acc_delay,
double acc_time_constant, double steer_delay, double steer_time_constant, double steer_dead_band,
double steer_bias, double debug_acc_scaling_factor, double debug_steer_scaling_factor)
double steer_bias, double rear_slip_coeff, 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 @@ -37,6 +38,7 @@ SimModelDelayArticulateAccGeared::SimModelDelayArticulateAccGeared(
steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
steer_dead_band_(steer_dead_band),
steer_bias_(steer_bias),
rear_slip_coeff_(rear_slip_coeff),
debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)),
debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0))
{
Expand Down Expand Up @@ -72,7 +74,8 @@ double SimModelDelayArticulateAccGeared::getWz()
const double steer = state_(IDX::STEER);
const double c_steer = std::cos(steer);

return (state_(IDX::VX) * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer) /
return (state_(IDX::VX) * std::sin(steer) -
state_steer_rate_ * front_wheelbase_ * c_steer * rear_slip_coeff_) /
(front_wheelbase_ + rear_wheelbase_ * c_steer);
}
double SimModelDelayArticulateAccGeared::getSteer()
Expand Down Expand Up @@ -147,8 +150,9 @@ Eigen::VectorXd SimModelDelayArticulateAccGeared::calcModel(
Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
d_state(IDX::X) = vel * cos(yaw);
d_state(IDX::Y) = vel * sin(yaw);
d_state(IDX::YAW) = (vel * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer) /
(front_wheelbase_ + rear_wheelbase_ * c_steer);
d_state(IDX::YAW) =
(vel * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer * rear_slip_coeff_) /
(front_wheelbase_ + rear_wheelbase_ * c_steer);
d_state(IDX::VX) = acc;
d_state(IDX::STEER) = state_steer_rate_;
d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_;
Expand Down
Loading