Skip to content

Commit 23ffc70

Browse files
Autumn60Autumn60
committed
add sim_model_delay_articulate_acc_geared for adt model
Signed-off-by: Autumn60 <harada.akiro@gmail.com>
1 parent 79dc610 commit 23ffc70

File tree

3 files changed

+362
-0
lines changed

3 files changed

+362
-0
lines changed

‎simulator/simple_planning_simulator/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
2121
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
2222
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp
2323
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
24+
src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp
2425
src/simple_planning_simulator/utils/csv_loader.cpp
2526
)
2627
target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS})

‎simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp

+168
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
// Copyright 2024 The Autoware Foundation.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_
16+
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_
17+
18+
#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
19+
20+
#include <Eigen/Core>
21+
#include <Eigen/LU>
22+
23+
#include <deque>
24+
#include <iostream>
25+
#include <queue>
26+
27+
class SimModelDelayArticulateAccGeared : public SimModelInterface
28+
{
29+
public:
30+
/**
31+
* @brief constructor
32+
* @param [in] vx_lim velocity limit [m/s]
33+
* @param [in] steer_lim steering limit [rad]
34+
* @param [in] vx_rate_lim acceleration limit [m/ss]
35+
* @param [in] steer_rate_lim steering angular velocity limit [rad/ss]
36+
* @param [in] front_wheelbase vehicle front wheelbase length [m]
37+
* @param [in] rear_wheelbase vehicle rear wheelbase length [m]
38+
* @param [in] dt delta time information to set input buffer for delay
39+
* @param [in] acc_delay time delay for accel command [s]
40+
* @param [in] acc_time_constant time constant for 1D model of accel dynamics
41+
* @param [in] steer_delay time delay for steering command [s]
42+
* @param [in] steer_time_constant time constant for 1D model of steering dynamics
43+
* @param [in] steer_dead_band dead band for steering angle [rad]
44+
* @param [in] steer_bias steering bias [rad]
45+
* @param [in] debug_acc_scaling_factor scaling factor for accel command
46+
* @param [in] debug_steer_scaling_factor scaling factor for steering command
47+
*/
48+
SimModelDelayArticulateAccGeared(
49+
double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim,
50+
double front_wheelbase, double rear_wheelbase, double dt, double acc_delay,
51+
double acc_time_constant, double steer_delay, double steer_time_constant,
52+
double steer_dead_band, double steer_bias, double debug_acc_scaling_factor,
53+
double debug_steer_scaling_factor);
54+
55+
/**
56+
* @brief default destructor
57+
*/
58+
~SimModelDelayArticulateAccGeared() = default;
59+
60+
private:
61+
const double MIN_TIME_CONSTANT; //!< @brief minimum time constant
62+
63+
enum IDX {
64+
X = 0,
65+
Y,
66+
YAW,
67+
VX,
68+
STEER,
69+
ACCX,
70+
};
71+
enum IDX_U {
72+
ACCX_DES = 0,
73+
STEER_DES,
74+
DRIVE_SHIFT,
75+
};
76+
77+
const double vx_lim_; //!< @brief velocity limit [m/s]
78+
const double vx_rate_lim_; //!< @brief acceleration limit [m/ss]
79+
const double steer_lim_; //!< @brief steering limit [rad]
80+
const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s]
81+
const double front_wheelbase_; //!< @brief vehicle front wheelbase length [m]
82+
const double rear_wheelbase_; //!< @brief vehicle rear wheelbase length [m]
83+
84+
std::deque<double> acc_input_queue_; //!< @brief buffer for accel command
85+
std::deque<double> steer_input_queue_; //!< @brief buffer for steering command
86+
const double acc_delay_; //!< @brief time delay for accel command [s]
87+
const double acc_time_constant_; //!< @brief time constant for accel dynamics
88+
const double steer_delay_; //!< @brief time delay for steering command [s]
89+
const double steer_time_constant_; //!< @brief time constant for steering dynamics
90+
const double steer_dead_band_; //!< @brief dead band for steering angle [rad]
91+
const double steer_bias_; //!< @brief steering angle bias [rad]
92+
const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command
93+
const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command
94+
95+
double state_steer_rate_;
96+
97+
/**
98+
* @brief set queue buffer for input command
99+
* @param [in] dt delta time
100+
*/
101+
void initializeInputQueue(const double & dt);
102+
103+
/**
104+
* @brief get vehicle position x
105+
*/
106+
double getX() override;
107+
108+
/**
109+
* @brief get vehicle position y
110+
*/
111+
double getY() override;
112+
113+
/**
114+
* @brief get vehicle angle yaw
115+
*/
116+
double getYaw() override;
117+
118+
/**
119+
* @brief get vehicle velocity vx
120+
*/
121+
double getVx() override;
122+
123+
/**
124+
* @brief get vehicle lateral velocity
125+
*/
126+
double getVy() override;
127+
128+
/**
129+
* @brief get vehicle longitudinal acceleration
130+
*/
131+
double getAx() override;
132+
133+
/**
134+
* @brief get vehicle angular-velocity wz
135+
*/
136+
double getWz() override;
137+
138+
/**
139+
* @brief get vehicle steering(=articulation) angle
140+
*/
141+
double getSteer() override;
142+
143+
/**
144+
* @brief update vehicle states
145+
* @param [in] dt delta time [s]
146+
*/
147+
void update(const double & dt) override;
148+
149+
/**
150+
* @brief calculate derivative of states with time delay steering(=articulation) model
151+
* @param [in] state current model state
152+
* @param [in] input input vector to model
153+
*/
154+
Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
155+
156+
/**
157+
* @brief update state considering current gear
158+
* @param [in] state current state
159+
* @param [in] prev_state previous state
160+
* @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand)
161+
* @param [in] dt delta time to update state
162+
*/
163+
void updateStateWithGear(
164+
Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear,
165+
const double dt);
166+
};
167+
168+
#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_

‎simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp

+193
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,193 @@
1+
// Copyright 2024 The Autoware Foundation.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp"
16+
17+
#include "autoware_vehicle_msgs/msg/gear_command.hpp"
18+
19+
#include <algorithm>
20+
21+
SimModelDelayArticulateAccGeared::SimModelDelayArticulateAccGeared(
22+
double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim,
23+
double front_wheelbase, double rear_wheelbase, double dt, double acc_delay,
24+
double acc_time_constant, double steer_delay, double steer_time_constant, double steer_dead_band,
25+
double steer_bias, double debug_acc_scaling_factor, double debug_steer_scaling_factor)
26+
: SimModelInterface(6 /* dim x */, 2 /* dim u */),
27+
MIN_TIME_CONSTANT(0.03),
28+
vx_lim_(vx_lim),
29+
vx_rate_lim_(vx_rate_lim),
30+
steer_lim_(steer_lim),
31+
steer_rate_lim_(steer_rate_lim),
32+
front_wheelbase_(front_wheelbase),
33+
rear_wheelbase_(rear_wheelbase),
34+
acc_delay_(acc_delay),
35+
acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)),
36+
steer_delay_(steer_delay),
37+
steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
38+
steer_dead_band_(steer_dead_band),
39+
steer_bias_(steer_bias),
40+
debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)),
41+
debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0))
42+
{
43+
initializeInputQueue(dt);
44+
}
45+
46+
double SimModelDelayArticulateAccGeared::getX()
47+
{
48+
return state_(IDX::X);
49+
}
50+
double SimModelDelayArticulateAccGeared::getY()
51+
{
52+
return state_(IDX::Y);
53+
}
54+
double SimModelDelayArticulateAccGeared::getYaw()
55+
{
56+
return state_(IDX::YAW);
57+
}
58+
double SimModelDelayArticulateAccGeared::getVx()
59+
{
60+
return state_(IDX::VX);
61+
}
62+
double SimModelDelayArticulateAccGeared::getVy()
63+
{
64+
return 0.0;
65+
}
66+
double SimModelDelayArticulateAccGeared::getAx()
67+
{
68+
return state_(IDX::ACCX);
69+
}
70+
double SimModelDelayArticulateAccGeared::getWz()
71+
{
72+
const double steer = state_(IDX::STEER);
73+
const double c_steer = std::cos(steer);
74+
const double pseudo_wheelbase = front_wheelbase_ * c_steer + rear_wheelbase_;
75+
76+
return state_(IDX::VX) * std::sin(steer) / pseudo_wheelbase -
77+
state_steer_rate_ * front_wheelbase_ * c_steer / pseudo_wheelbase;
78+
}
79+
double SimModelDelayArticulateAccGeared::getSteer()
80+
{
81+
// return measured values with bias added to actual values
82+
return state_(IDX::STEER) + steer_bias_;
83+
}
84+
void SimModelDelayArticulateAccGeared::update(const double & dt)
85+
{
86+
Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_);
87+
88+
acc_input_queue_.push_back(input_(IDX_U::ACCX_DES));
89+
delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front();
90+
acc_input_queue_.pop_front();
91+
steer_input_queue_.push_back(input_(IDX_U::STEER_DES));
92+
delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front();
93+
steer_input_queue_.pop_front();
94+
95+
const auto prev_state = state_;
96+
updateRungeKutta(dt, delayed_input);
97+
98+
// take velocity limit explicitly
99+
state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_));
100+
101+
// consider gear
102+
// update position and velocity first, and then acceleration is calculated naturally
103+
updateStateWithGear(state_, prev_state, gear_, dt);
104+
}
105+
106+
void SimModelDelayArticulateAccGeared::initializeInputQueue(const double & dt)
107+
{
108+
size_t acc_input_queue_size = static_cast<size_t>(round(acc_delay_ / dt));
109+
acc_input_queue_.resize(acc_input_queue_size);
110+
std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0);
111+
112+
size_t steer_input_queue_size = static_cast<size_t>(round(steer_delay_ / dt));
113+
steer_input_queue_.resize(steer_input_queue_size);
114+
std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0);
115+
}
116+
117+
Eigen::VectorXd SimModelDelayArticulateAccGeared::calcModel(
118+
const Eigen::VectorXd & state, const Eigen::VectorXd & input)
119+
{
120+
auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); };
121+
122+
const double vel = sat(state(IDX::VX), vx_lim_, -vx_lim_);
123+
const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_);
124+
const double yaw = state(IDX::YAW);
125+
const double steer = state(IDX::STEER);
126+
const double acc_des =
127+
sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_;
128+
const double steer_des =
129+
sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_;
130+
// NOTE: `steer_des` is calculated by control from measured values. getSteer() also gets the
131+
// measured value. The steer_rate used in the motion calculation is obtained from these
132+
// differences.
133+
const double steer_diff = getSteer() - steer_des;
134+
const double steer_diff_with_dead_band = std::invoke([&]() {
135+
if (steer_diff > steer_dead_band_) {
136+
return steer_diff - steer_dead_band_;
137+
} else if (steer_diff < -steer_dead_band_) {
138+
return steer_diff + steer_dead_band_;
139+
} else {
140+
return 0.0;
141+
}
142+
});
143+
state_steer_rate_ =
144+
sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_);
145+
146+
const double c_steer = std::cos(steer);
147+
const double pseudo_wheelbase = front_wheelbase_ * c_steer + rear_wheelbase_;
148+
149+
Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
150+
d_state(IDX::X) = vel * cos(yaw);
151+
d_state(IDX::Y) = vel * sin(yaw);
152+
d_state(IDX::YAW) = vel * std::sin(steer) / pseudo_wheelbase -
153+
state_steer_rate_ * front_wheelbase_ * c_steer / pseudo_wheelbase;
154+
d_state(IDX::STEER) = state_steer_rate_;
155+
d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_;
156+
157+
return d_state;
158+
}
159+
160+
void SimModelDelayArticulateAccGeared::updateStateWithGear(
161+
Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt)
162+
{
163+
const auto setStopState = [&]() {
164+
state(IDX::VX) = 0.0;
165+
state(IDX::X) = prev_state(IDX::X);
166+
state(IDX::Y) = prev_state(IDX::Y);
167+
state(IDX::YAW) = prev_state(IDX::YAW);
168+
state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
169+
};
170+
171+
using autoware_vehicle_msgs::msg::GearCommand;
172+
if (
173+
gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 ||
174+
gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 ||
175+
gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 ||
176+
gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 ||
177+
gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 ||
178+
gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 ||
179+
gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 ||
180+
gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) {
181+
if (state(IDX::VX) < 0.0) {
182+
setStopState();
183+
}
184+
} else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) {
185+
if (state(IDX::VX) > 0.0) {
186+
setStopState();
187+
}
188+
} else if (gear == GearCommand::PARK) {
189+
setStopState();
190+
} else {
191+
setStopState();
192+
}
193+
}

0 commit comments

Comments
 (0)