From 201e0e166856dae52e9229319d7e4f39d8ecd24e Mon Sep 17 00:00:00 2001 From: kyoichi sugahara <81.s.kyo.19@gmail.com> Date: Fri, 24 Dec 2021 18:25:41 +0900 Subject: [PATCH 1/5] add delay steer vel in psim --- .../simple_planning_simulator_core.hpp | 1 + .../vehicle_model/sim_model.hpp | 1 + .../sim_model_delay_steer_vel.hpp | 136 ++++++++++++++++++ .../simple_planning_simulator_core.cpp | 7 + .../sim_model_delay_steer_vel.cpp | 95 ++++++++++++ 5 files changed, 240 insertions(+) create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp 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 e25160bd86f9c..faa5d0a008874 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 @@ -189,6 +189,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node DELAY_STEER_ACC = 2, DELAY_STEER_ACC_GEARED = 3, IDEAL_STEER_VEL = 4, + DELAY_STEER_VEL = 5 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index ef72b9e1f2c2c..5f1de6e2e819b 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -21,6 +21,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ 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 new file mode 100644 index 0000000000000..4172f0a72a195 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -0,0 +1,136 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +/** + * @class SimModelDelaySteerVel + * @brief calculate delay steering dynamics + */ +class SimModelDelaySteerVel : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] angel_lim angular velocity limit [m/s] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] wz_rate_lim angular acceleration limit [rad/ss] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] vx_delay time delay for velocity command [s] + * @param [in] vx_time_constant time constant for 1D model of velocity dynamics + * @param [in] wx_delay time delay for angular-velocity command [s] + * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics + */ + SimModelDelaySteerVel( + float64_t vx_lim, float64_t angel_lim, float64_t vx_rate_lim, float64_t wz_rate_lim, + float64_t dt, float64_t vx_delay, float64_t vx_time_constant, float64_t wz_delay, + float64_t wz_time_constant); + + /** + * @brief destructor + */ + ~SimModelDelaySteerVel() = default; + +private: + const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX + { + X = 0, + Y, + YAW, + VX, + WZ, + }; + enum IDX_U + { + VX_DES = 0, + WZ_DES, + }; + + const float64_t vx_lim_; //!< @brief velocity limit + const float64_t vx_rate_lim_; //!< @brief acceleration limit + const float64_t wz_lim_; //!< @brief angular velocity limit + const float64_t wz_rate_lim_; //!< @brief angular acceleration limit + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque wz_input_queue_; //!< @brief buffer for angular velocity command + const float64_t vx_delay_; //!< @brief time delay for velocity command [s] + const float64_t vx_time_constant_; + //!< @brief time constant for 1D model of velocity dynamics + const float64_t wz_delay_; //!< @brief time delay for angular-velocity command [s] + const float64_t + wz_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const float_t & dt); + + /** + * @brief get vehicle position x + */ + float64_t getX() override; + + /** + * @brief get vehicle position y + */ + float64_t getY() override; + + /** + * @brief get vehicle angle yaw + */ + float64_t getYaw() override; + + /** + * @brief get vehicle longitudinal velocity + */ + float64_t getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + float64_t getWz() override; + + /** + * @brief get vehicle steering angle + */ + float64_t getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const float64_t & dt) override; + + /** + * @brief calculate derivative of states with delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ 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 32bbcbe8fcad9..ac4097ad4df02 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 @@ -175,6 +175,8 @@ void SimplePlanningSimulator::initialize_vehicle_model() const float64_t steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); const float64_t acc_time_delay = declare_parameter("acc_time_delay", 0.1); const float64_t acc_time_constant = declare_parameter("acc_time_constant", 0.1); + const float64_t vel_time_delay = declare_parameter("vel_time_delay", 0.25); + const float64_t vel_time_constant = declare_parameter("vel_time_constant", 0.5); const float64_t steer_time_delay = declare_parameter("steer_time_delay", 0.24); const float64_t steer_time_constant = declare_parameter("steer_time_constant", 0.27); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -189,6 +191,11 @@ void SimplePlanningSimulator::initialize_vehicle_model() } else if (vehicle_model_type_str == "IDEAL_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared(wheelbase); + } else if (vehicle_model_type_str == "DELAY_STEER_VEL") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, timer_sampling_time_ms_ / 1000.0, + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant); } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( 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 new file mode 100644 index 0000000000000..250cef60cf712 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -0,0 +1,95 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" + +#include + +SimModelDelaySteerVel::SimModelDelaySteerVel( + float64_t vx_lim, float64_t wz_lim, float64_t vx_rate_lim, float64_t wz_rate_lim, float64_t dt, + float64_t vx_delay, float64_t vx_time_constant, float64_t wz_delay, float64_t wz_time_constant) +: SimModelInterface(5 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + wz_lim_(wz_lim), + wz_rate_lim_(wz_rate_lim), + vx_delay_(vx_delay), + vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), + wz_delay_(wz_delay), + wz_time_constant_(std::max(wz_time_constant, MIN_TIME_CONSTANT)) +{ + initializeInputQueue(dt); +} + +float64_t SimModelDelaySteerVel::getX() { return state_(IDX::X); } +float64_t SimModelDelaySteerVel::getY() { return state_(IDX::Y); } +float64_t SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); } +float64_t SimModelDelaySteerVel::getVx() { return input_(IDX_U::VX_DES); } +float64_t SimModelDelaySteerVel::getWz() { return state_(IDX::WZ); } +float64_t SimModelDelaySteerVel::getSteer() { return 0.0; } +void SimModelDelaySteerVel::update(const float64_t & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + vx_input_queue_.push_back(input_(IDX_U::VX_DES)); + delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); + vx_input_queue_.pop_front(); + wz_input_queue_.push_back(input_(IDX_U::WZ_DES)); + delayed_input(IDX_U::WZ_DES) = wz_input_queue_.front(); + wz_input_queue_.pop_front(); + // do not use deadzone_delta_steer (Steer IF does not exist in this model) + updateRungeKutta(dt, delayed_input); +} + +void SimModelDelaySteerVel::initializeInputQueue(const float_t & dt) +{ + size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); + for (size_t i = 0; i < vx_input_queue_size; i++) { + vx_input_queue_.push_back(0.0); + } + size_t wz_input_queue_size = static_cast(round(wz_delay_ / dt)); + for (size_t i = 0; i < wz_input_queue_size; i++) { + wz_input_queue_.push_back(0.0); + } +} + + +Eigen::VectorXd SimModelDelaySteerVel::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; + + const float_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const float_t wz = sat(state(IDX::WZ), wz_lim_, -wz_lim_); + const float_t yaw = state(IDX::YAW); + const float_t delay_input_vx = input(IDX_U::VX_DES); + const float_t delay_input_wz = input(IDX_U::WZ_DES); + const float_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); + const float_t delay_wz_des = sat(delay_input_wz, wz_lim_, -wz_lim_); + float_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_; + float_t wz_rate = -(wz - delay_wz_des) / wz_time_constant_; + vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_); + wz_rate = sat(wz_rate, wz_rate_lim_, -wz_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * cos(yaw); + d_state(IDX::Y) = vx * sin(yaw); + d_state(IDX::YAW) = wz; + d_state(IDX::VX) = vx_rate; + d_state(IDX::WZ) = wz_rate; + + return d_state; +} + From 31d332597697de8ca60ce7a923d157dec3f7560b Mon Sep 17 00:00:00 2001 From: kyoichi sugahara <81.s.kyo.19@gmail.com> Date: Fri, 24 Dec 2021 19:04:06 +0900 Subject: [PATCH 2/5] change wz to steer --- .../sim_model_delay_steer_vel.hpp | 31 ++++++++---- .../simple_planning_simulator_core.cpp | 2 +- .../sim_model_delay_steer_vel.cpp | 50 +++++++++++-------- 3 files changed, 50 insertions(+), 33 deletions(-) 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 4172f0a72a195..61fa5a119150e 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,9 +43,9 @@ class SimModelDelaySteerVel : public SimModelInterface * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics */ SimModelDelaySteerVel( - float64_t vx_lim, float64_t angel_lim, float64_t vx_rate_lim, float64_t wz_rate_lim, - float64_t dt, float64_t vx_delay, float64_t vx_time_constant, float64_t wz_delay, - float64_t wz_time_constant); + float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, + float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant, + float64_t steer_delay, float64_t steer_time_constant); /** * @brief destructor @@ -61,27 +61,28 @@ class SimModelDelaySteerVel : public SimModelInterface Y, YAW, VX, - WZ, + STEER, }; enum IDX_U { VX_DES = 0, - WZ_DES, + STEER_DES, }; const float64_t vx_lim_; //!< @brief velocity limit const float64_t vx_rate_lim_; //!< @brief acceleration limit - const float64_t wz_lim_; //!< @brief angular velocity limit - const float64_t wz_rate_lim_; //!< @brief angular acceleration limit + const float64_t steer_lim_; //!< @brief steering limit [rad] + const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m] std::deque vx_input_queue_; //!< @brief buffer for velocity command - std::deque wz_input_queue_; //!< @brief buffer for angular velocity command + std::deque steer_input_queue_; //!< @brief buffer for angular velocity command const float64_t vx_delay_; //!< @brief time delay for velocity command [s] const float64_t vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const float64_t wz_delay_; //!< @brief time delay for angular-velocity command [s] + const float64_t steer_delay_; //!< @brief time delay for angular-velocity command [s] const float64_t - wz_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics /** * @brief set queue buffer for input command @@ -109,6 +110,16 @@ class SimModelDelaySteerVel : public SimModelInterface */ float64_t getVx() override; + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal acceleration + */ + float64_t getAx() override; + /** * @brief get vehicle angular-velocity wz */ 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 ac4097ad4df02..c7aba5c19508a 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 @@ -194,7 +194,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() } else if (vehicle_model_type_str == "DELAY_STEER_VEL") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL; vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, timer_sampling_time_ms_ / 1000.0, + 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); } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; 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 250cef60cf712..57903d5200c7c 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 @@ -17,18 +17,20 @@ #include SimModelDelaySteerVel::SimModelDelaySteerVel( - float64_t vx_lim, float64_t wz_lim, float64_t vx_rate_lim, float64_t wz_rate_lim, float64_t dt, - float64_t vx_delay, float64_t vx_time_constant, float64_t wz_delay, float64_t wz_time_constant) + float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, + float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant, + float64_t steer_delay, float64_t steer_time_constant) : SimModelInterface(5 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), vx_rate_lim_(vx_rate_lim), - wz_lim_(wz_lim), - wz_rate_lim_(wz_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), vx_delay_(vx_delay), vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), - wz_delay_(wz_delay), - wz_time_constant_(std::max(wz_time_constant, MIN_TIME_CONSTANT)) + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) { initializeInputQueue(dt); } @@ -36,9 +38,13 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( float64_t SimModelDelaySteerVel::getX() { return state_(IDX::X); } float64_t SimModelDelaySteerVel::getY() { return state_(IDX::Y); } float64_t SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); } -float64_t SimModelDelaySteerVel::getVx() { return input_(IDX_U::VX_DES); } -float64_t SimModelDelaySteerVel::getWz() { return state_(IDX::WZ); } -float64_t SimModelDelaySteerVel::getSteer() { return 0.0; } +float64_t SimModelDelaySteerVel::getVx() { return input_(IDX::VX); } +float64_t SimModelDelaySteerVel::getVy() {return 0.0;} +float64_t SimModelDelaySteerVel::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +float64_t SimModelDelaySteerVel::getSteer() {return state_(IDX::STEER);} void SimModelDelaySteerVel::update(const float64_t & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -46,9 +52,9 @@ void SimModelDelaySteerVel::update(const float64_t & dt) vx_input_queue_.push_back(input_(IDX_U::VX_DES)); delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); vx_input_queue_.pop_front(); - wz_input_queue_.push_back(input_(IDX_U::WZ_DES)); - delayed_input(IDX_U::WZ_DES) = wz_input_queue_.front(); - wz_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); } @@ -59,9 +65,9 @@ void SimModelDelaySteerVel::initializeInputQueue(const float_t & dt) for (size_t i = 0; i < vx_input_queue_size; i++) { vx_input_queue_.push_back(0.0); } - size_t wz_input_queue_size = static_cast(round(wz_delay_ / dt)); - for (size_t i = 0; i < wz_input_queue_size; i++) { - wz_input_queue_.push_back(0.0); + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + for (size_t i = 0; i < steer_input_queue_size; i++) { + steer_input_queue_.push_back(0.0); } } @@ -72,23 +78,23 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; const float_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const float_t wz = sat(state(IDX::WZ), wz_lim_, -wz_lim_); + const float_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); const float_t yaw = state(IDX::YAW); const float_t delay_input_vx = input(IDX_U::VX_DES); - const float_t delay_input_wz = input(IDX_U::WZ_DES); + const float_t delay_input_steer = input(IDX_U::STEER_DES); const float_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); - const float_t delay_wz_des = sat(delay_input_wz, wz_lim_, -wz_lim_); + const float_t delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); float_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_; - float_t wz_rate = -(wz - delay_wz_des) / wz_time_constant_; + float_t steer_rate = -(steer - delay_steer_des) / steer_time_constant_; vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_); - wz_rate = sat(wz_rate, wz_rate_lim_, -wz_rate_lim_); + steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); d_state(IDX::Y) = vx * sin(yaw); - d_state(IDX::YAW) = wz; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; d_state(IDX::VX) = vx_rate; - d_state(IDX::WZ) = wz_rate; + d_state(IDX::STEER) = steer_rate; return d_state; } From 6d478b6e17df6a97c325485571ffe017edc613c0 Mon Sep 17 00:00:00 2001 From: kyoichi sugahara <81.s.kyo.19@gmail.com> Date: Fri, 24 Dec 2021 19:07:43 +0900 Subject: [PATCH 3/5] fix param description --- .../vehicle_model/sim_model_delay_steer_vel.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 61fa5a119150e..ac413c46817db 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 @@ -33,14 +33,15 @@ class SimModelDelaySteerVel : public SimModelInterface /** * @brief constructor * @param [in] vx_lim velocity limit [m/s] - * @param [in] angel_lim angular velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] * @param [in] vx_rate_lim acceleration limit [m/ss] - * @param [in] wz_rate_lim angular acceleration limit [rad/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] * @param [in] dt delta time information to set input buffer for delay * @param [in] vx_delay time delay for velocity command [s] * @param [in] vx_time_constant time constant for 1D model of velocity dynamics - * @param [in] wx_delay time delay for angular-velocity command [s] - * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics */ SimModelDelaySteerVel( float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, From 210ea3742ac46907c0d14ba1fe3dacff22bffe0b Mon Sep 17 00:00:00 2001 From: kyoichi sugahara <81.s.kyo.19@gmail.com> Date: Fri, 24 Dec 2021 19:28:10 +0900 Subject: [PATCH 4/5] modify readme --- .../simple_planning_simulator-design.md | 23 +++++++++++-------- .../test/test_simple_planning_simulator.cpp | 1 + 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md index a2e34cf6c7cc6..f9c2c074b77a6 100644 --- a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md +++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md @@ -60,6 +60,7 @@ The purpose of this simulator is for the integration test of planning and contro - `IDEAL_STEER_VEL` - `IDEAL_STEER_ACC` - `IDEAL_STEER_ACC_GEARED` + - `DELAY_STEER_VEL` - `DELAY_STEER_ACC` - `DELAY_STEER_ACC_GEARED` @@ -68,16 +69,18 @@ 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_A|D_ST_A_G|Default value| unit | -|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---| -|acc_time_delay | double | dead time for the acceleration input | x | x | x | o | o | 0.1 | [s] | -|steer_time_delay | double | dead time for the steering input | x | x | x | o | o | 0.24| [s] | -|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | 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 | 0.27| [s] | -|vel_lim | double | limit of velocity | x | x | x | o | o | 50.0| [m/s] | -|vel_rate_lim | double | limit of acceleration | x | x | x | o | o | 7.0 | [m/ss] | -|steer_lim | double | limit of steering angle | x | x | x | o | o | 1.0 | [rad] | -|steer_rate_lim | double | limit of steering angle change rate | x | x | x | 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] | +|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] | diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index ab33e270c2443..62dbd964cff31 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -42,6 +42,7 @@ const std::vector vehicle_model_type_vec = { // NOLINT "IDEAL_STEER_VEL", "IDEAL_STEER_ACC", "IDEAL_STEER_ACC_GEARED", + "DELAY_STEER_VEL", "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED", }; From caa2c0c4eeac2c63c9b6e0f8720932d957cd541c Mon Sep 17 00:00:00 2001 From: kyoichi sugahara <81.s.kyo.19@gmail.com> Date: Fri, 24 Dec 2021 19:47:45 +0900 Subject: [PATCH 5/5] modify cmake --- .../simple_planning_simulator/CMakeLists.txt | 1 + .../sim_model_delay_steer_vel.hpp | 2 +- .../sim_model_delay_steer_vel.cpp | 20 +++++++++---------- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index e217ca572e583..0b5ebbd482db8 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -16,6 +16,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp + src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp ) 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 ac413c46817db..40eb218217f5f 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 @@ -89,7 +89,7 @@ class SimModelDelaySteerVel : public SimModelInterface * @brief set queue buffer for input command * @param [in] dt delta time */ - void initializeInputQueue(const float_t & dt); + void initializeInputQueue(const float64_t & dt); /** * @brief get vehicle position 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 57903d5200c7c..4023c128cbde1 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 @@ -59,7 +59,7 @@ void SimModelDelaySteerVel::update(const float64_t & dt) updateRungeKutta(dt, delayed_input); } -void SimModelDelaySteerVel::initializeInputQueue(const float_t & dt) +void SimModelDelaySteerVel::initializeInputQueue(const float64_t & dt) { size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); for (size_t i = 0; i < vx_input_queue_size; i++) { @@ -77,15 +77,15 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( { auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; - const float_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const float_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); - const float_t yaw = state(IDX::YAW); - const float_t delay_input_vx = input(IDX_U::VX_DES); - const float_t delay_input_steer = input(IDX_U::STEER_DES); - const float_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); - const float_t delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - float_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_; - float_t steer_rate = -(steer - delay_steer_des) / steer_time_constant_; + const float64_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const float64_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); + const float64_t yaw = state(IDX::YAW); + const float64_t delay_input_vx = input(IDX_U::VX_DES); + const float64_t delay_input_steer = input(IDX_U::STEER_DES); + const float64_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); + const float64_t delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); + float64_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_; + float64_t steer_rate = -(steer - delay_steer_des) / steer_time_constant_; vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_); steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_);