feat: add sim_model_delay_articulate_acc_geared for adt model (#1548)

* add sim_model_delay_articulate_acc_geared for adt model

Signed-off-by: Autumn60 <harada.akiro@gmail.com>

* style(pre-commit): autofix

* enable articulate model VehicleModelType for psim

Signed-off-by: Autumn60 <harada.akiro@gmail.com>

* style(pre-commit): autofix

* parameterize wheelbase ratio

Signed-off-by: Autumn60 <harada.akiro@gmail.com>

---------

Signed-off-by: Autumn60 <harada.akiro@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: Autumn60

simulator/simple_planning_simulator/CMakeLists.txt

@@ -21,6 +21,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
+  src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp
   src/simple_planning_simulator/utils/csv_loader.cpp
 )
 target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS})

simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

@@ -206,7 +206,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
     DELAY_STEER_VEL = 5,
     DELAY_STEER_MAP_ACC_GEARED = 6,
     LEARNED_STEER_VEL = 7,
-    DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8
+    DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8,
+    DELAY_ARTICULATE_ACC_GEARED = 9
   } vehicle_model_type_;  //!< @brief vehicle model type to decide the model dynamics
   std::shared_ptr<SimModelInterface> vehicle_model_ptr_;  //!< @brief vehicle model pointer
 

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

@@ -0,0 +1,167 @@
+// Copyright 2024 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__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_
+#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_
+
+#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <deque>
+#include <iostream>
+#include <queue>
+
+class SimModelDelayArticulateAccGeared : public SimModelInterface
+{
+public:
+  /**
+   * @brief constructor
+   * @param [in] vx_lim velocity limit [m/s]
+   * @param [in] steer_lim steering limit [rad]
+   * @param [in] vx_rate_lim acceleration limit [m/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] acc_delay time delay for accel command [s]
+   * @param [in] acc_time_constant time constant for 1D model of accel dynamics
+   * @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] steer_bias steering bias [rad]
+   * @param [in] debug_acc_scaling_factor scaling factor for accel command
+   * @param [in] debug_steer_scaling_factor scaling factor for steering command
+   */
+  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);
+
+  /**
+   * @brief default destructor
+   */
+  ~SimModelDelayArticulateAccGeared() = default;
+
+private:
+  const double MIN_TIME_CONSTANT;  //!< @brief minimum time constant
+
+  enum IDX {
+    X = 0,
+    Y,
+    YAW,
+    VX,
+    STEER,
+    ACCX,
+  };
+  enum IDX_U {
+    ACCX_DES = 0,
+    STEER_DES,
+    DRIVE_SHIFT,
+  };
+
+  const double vx_lim_;           //!< @brief velocity limit [m/s]
+  const double vx_rate_lim_;      //!< @brief acceleration limit [m/ss]
+  const double steer_lim_;        //!< @brief steering limit [rad]
+  const double steer_rate_lim_;   //!< @brief steering angular velocity limit [rad/s]
+  const double front_wheelbase_;  //!< @brief front wheelbase length [m]
+  const double rear_wheelbase_;   //!< @brief rear 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]
+  const double steer_bias_;                  //!< @brief steering angle bias [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
+
+  double state_steer_rate_;  //!< @brief steering angular velocity [rad/s]
+
+  /**
+   * @brief set queue buffer for input command
+   * @param [in] dt delta time
+   */
+  void initializeInputQueue(const double & dt);
+
+  /**
+   * @brief get vehicle position x
+   */
+  double getX() override;
+
+  /**
+   * @brief get vehicle position y
+   */
+  double getY() override;
+
+  /**
+   * @brief get vehicle angle yaw
+   */
+  double getYaw() override;
+
+  /**
+   * @brief get vehicle velocity vx
+   */
+  double getVx() override;
+
+  /**
+   * @brief get vehicle lateral velocity
+   */
+  double getVy() override;
+
+  /**
+   * @brief get vehicle longitudinal acceleration
+   */
+  double getAx() override;
+
+  /**
+   * @brief get vehicle angular-velocity wz
+   */
+  double getWz() override;
+
+  /**
+   * @brief get vehicle steering angle
+   */
+  double getSteer() override;
+
+  /**
+   * @brief update vehicle states
+   * @param [in] dt delta time [s]
+   */
+  void update(const double & dt) override;
+
+  /**
+   * @brief calculate derivative of states with time 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;
+
+  /**
+   * @brief update state considering current gear
+   * @param [in] state current state
+   * @param [in] prev_state previous state
+   * @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand)
+   * @param [in] dt delta time to update state
+   */
+  void updateStateWithGear(
+    Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear,
+    const double dt);
+};
+
+#endif  // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp

@@ -15,6 +15,7 @@
 #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_
 #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_
 
+#include "simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_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_acc_geared_wo_fall_guard.hpp"

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -234,6 +234,7 @@ void SimplePlanningSimulator::initialize_vehicle_model()
   const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0);
   const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
   const double wheelbase = vehicle_info.wheel_base_m;
+  const double rear_wheelbase_ratio = declare_parameter("rear_wheelbase_ratio", 1.0);
 
   std::vector<std::string> model_module_paths = declare_parameter<std::vector<std::string>>(
     "model_module_paths", std::vector<std::string>({""}));
@@ -297,6 +298,16 @@ void SimplePlanningSimulator::initialize_vehicle_model()
     vehicle_model_ptr_ = std::make_shared<SimModelLearnedSteerVel>(
       timer_sampling_time_ms_ / 1000.0, model_module_paths, model_param_paths, model_class_names);
 
+  } else if (vehicle_model_type_str == "DELAY_ARTICULATE_ACC_GEARED") {
+    const double front_wheelbase = wheelbase * (1.0 - rear_wheelbase_ratio);
+    const double rear_wheelbase = wheelbase * rear_wheelbase_ratio;
+
+    vehicle_model_type_ = VehicleModelType::DELAY_ARTICULATE_ACC_GEARED;
+    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,
+      debug_steer_scaling_factor);
   } else {
     throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str);
   }
@@ -507,7 +518,8 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by
   } else if (  // NOLINT
     vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED ||
-    vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) {
+    vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED ||
+    vehicle_model_type_ == VehicleModelType::DELAY_ARTICULATE_ACC_GEARED) {
     input << combined_acc, steer;
   } else if (  // NOLINT
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD) {
@@ -612,7 +624,8 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist &
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED ||
     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD ||
-    vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) {
+    vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED ||
+    vehicle_model_type_ == VehicleModelType::DELAY_ARTICULATE_ACC_GEARED) {
     state << x, y, yaw, vx, steer, accx;
   }
   vehicle_model_ptr_->setState(state);